This is a set of scripts that calibrates and calculates a depth map from a stereo camera. It is primarily written with this ELP stereo camera in mind, but it theoretically should work with other hardware.
See http://albertarmea.com/post/opencv-stereo-camera for a full writeup.
| import numpy as np | |
| import cv2 | |
| LEFT_PATH = "capture/left/{:06d}.jpg" | |
| RIGHT_PATH = "capture/right/{:06d}.jpg" | |
| CAMERA_WIDTH = 1280 | |
| CAMERA_HEIGHT = 720 | |
| # TODO: Use more stable identifiers | |
| left = cv2.VideoCapture(0) | |
| right = cv2.VideoCapture(1) | |
| # Increase the resolution | |
| left.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH) | |
| left.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT) | |
| right.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH) | |
| right.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT) | |
| # Use MJPEG to avoid overloading the USB 2.0 bus at this resolution | |
| left.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG")) | |
| right.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG")) | |
| # The distortion in the left and right edges prevents a good calibration, so | |
| # discard the edges | |
| CROP_WIDTH = 960 | |
| def cropHorizontal(image): | |
| return image[:, | |
| int((CAMERA_WIDTH-CROP_WIDTH)/2): | |
| int(CROP_WIDTH+(CAMERA_WIDTH-CROP_WIDTH)/2)] | |
| frameId = 0 | |
| # Grab both frames first, then retrieve to minimize latency between cameras | |
| while(True): | |
| if not (left.grab() and right.grab()): | |
| print("No more frames") | |
| break | |
| _, leftFrame = left.retrieve() | |
| leftFrame = cropHorizontal(leftFrame) | |
| _, rightFrame = right.retrieve() | |
| rightFrame = cropHorizontal(rightFrame) | |
| cv2.imwrite(LEFT_PATH.format(frameId), leftFrame) | |
| cv2.imwrite(RIGHT_PATH.format(frameId), rightFrame) | |
| cv2.imshow('left', leftFrame) | |
| cv2.imshow('right', rightFrame) | |
| if cv2.waitKey(1) & 0xFF == ord('q'): | |
| break | |
| frameId += 1 | |
| left.release() | |
| right.release() | |
| cv2.destroyAllWindows() |
| import glob | |
| import os | |
| import random | |
| import sys | |
| import numpy as np | |
| import cv2 | |
| CHESSBOARD_SIZE = (7, 6) | |
| CHESSBOARD_OPTIONS = (cv2.CALIB_CB_ADAPTIVE_THRESH | | |
| cv2.CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_CB_FAST_CHECK) | |
| OBJECT_POINT_ZERO = np.zeros((CHESSBOARD_SIZE[0] * CHESSBOARD_SIZE[1], 3), | |
| np.float32) | |
| OBJECT_POINT_ZERO[:, :2] = np.mgrid[0:CHESSBOARD_SIZE[0], | |
| 0:CHESSBOARD_SIZE[1]].T.reshape(-1, 2) | |
| OPTIMIZE_ALPHA = 0.25 | |
| TERMINATION_CRITERIA = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_MAX_ITER, 30, | |
| 0.001) | |
| MAX_IMAGES = 64 | |
| if len(sys.argv) != 4: | |
| print("Syntax: {0} LEFT_IMAGE_DIR RIGHT_IMAGE_DIR OUTPUT_FILENAME" | |
| .format(sys.argv[0])) | |
| sys.exit(1) | |
| leftImageDir = sys.argv[1] | |
| rightImageDir = sys.argv[2] | |
| outputFile = sys.argv[3] | |
| def readImagesAndFindChessboards(imageDirectory): | |
| cacheFile = "{0}/chessboards.npz".format(imageDirectory) | |
| try: | |
| cache = np.load(cacheFile) | |
| print("Loading image data from cache file at {0}".format(cacheFile)) | |
| return (list(cache["filenames"]), list(cache["objectPoints"]), | |
| list(cache["imagePoints"]), tuple(cache["imageSize"])) | |
| except IOError: | |
| print("Cache file at {0} not found".format(cacheFile)) | |
| print("Reading images at {0}".format(imageDirectory)) | |
| imagePaths = glob.glob("{0}/*.jpg".format(imageDirectory)) | |
| filenames = [] | |
| objectPoints = [] | |
| imagePoints = [] | |
| imageSize = None | |
| for imagePath in sorted(imagePaths): | |
| image = cv2.imread(imagePath) | |
| grayImage = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) | |
| newSize = grayImage.shape[::-1] | |
| if imageSize != None and newSize != imageSize: | |
| raise ValueError( | |
| "Calibration image at {0} is not the same size as the others" | |
| .format(imagePath)) | |
| imageSize = newSize | |
| hasCorners, corners = cv2.findChessboardCorners(grayImage, | |
| CHESSBOARD_SIZE, cv2.CALIB_CB_FAST_CHECK) | |
| if hasCorners: | |
| filenames.append(os.path.basename(imagePath)) | |
| objectPoints.append(OBJECT_POINT_ZERO) | |
| cv2.cornerSubPix(grayImage, corners, (11, 11), (-1, -1), | |
| TERMINATION_CRITERIA) | |
| imagePoints.append(corners) | |
| cv2.drawChessboardCorners(image, CHESSBOARD_SIZE, corners, hasCorners) | |
| cv2.imshow(imageDirectory, image) | |
| # Needed to draw the window | |
| cv2.waitKey(1) | |
| cv2.destroyWindow(imageDirectory) | |
| print("Found corners in {0} out of {1} images" | |
| .format(len(imagePoints), len(imagePaths))) | |
| np.savez_compressed(cacheFile, | |
| filenames=filenames, objectPoints=objectPoints, | |
| imagePoints=imagePoints, imageSize=imageSize) | |
| return filenames, objectPoints, imagePoints, imageSize | |
| (leftFilenames, leftObjectPoints, leftImagePoints, leftSize | |
| ) = readImagesAndFindChessboards(leftImageDir) | |
| (rightFilenames, rightObjectPoints, rightImagePoints, rightSize | |
| ) = readImagesAndFindChessboards(rightImageDir) | |
| if leftSize != rightSize: | |
| print("Camera resolutions do not match") | |
| sys.exit(1) | |
| imageSize = leftSize | |
| filenames = list(set(leftFilenames) & set(rightFilenames)) | |
| if (len(filenames) > MAX_IMAGES): | |
| print("Too many images to calibrate, using {0} randomly selected images" | |
| .format(MAX_IMAGES)) | |
| filenames = random.sample(filenames, MAX_IMAGES) | |
| filenames = sorted(filenames) | |
| print("Using these images:") | |
| print(filenames) | |
| def getMatchingObjectAndImagePoints(requestedFilenames, | |
| allFilenames, objectPoints, imagePoints): | |
| requestedFilenameSet = set(requestedFilenames) | |
| requestedObjectPoints = [] | |
| requestedImagePoints = [] | |
| for index, filename in enumerate(allFilenames): | |
| if filename in requestedFilenameSet: | |
| requestedObjectPoints.append(objectPoints[index]) | |
| requestedImagePoints.append(imagePoints[index]) | |
| return requestedObjectPoints, requestedImagePoints | |
| leftObjectPoints, leftImagePoints = getMatchingObjectAndImagePoints(filenames, | |
| leftFilenames, leftObjectPoints, leftImagePoints) | |
| rightObjectPoints, rightImagePoints = getMatchingObjectAndImagePoints(filenames, | |
| rightFilenames, rightObjectPoints, rightImagePoints) | |
| # TODO: Fix this validation | |
| # Keep getting "Use a.any() or a.all()" even though it's already used?! | |
| # if (leftObjectPoints != rightObjectPoints).all(): | |
| # print("Object points do not match") | |
| # sys.exit(1) | |
| objectPoints = leftObjectPoints | |
| print("Calibrating left camera...") | |
| _, leftCameraMatrix, leftDistortionCoefficients, _, _ = cv2.calibrateCamera( | |
| objectPoints, leftImagePoints, imageSize, None, None) | |
| print("Calibrating right camera...") | |
| _, rightCameraMatrix, rightDistortionCoefficients, _, _ = cv2.calibrateCamera( | |
| objectPoints, rightImagePoints, imageSize, None, None) | |
| print("Calibrating cameras together...") | |
| (_, _, _, _, _, rotationMatrix, translationVector, _, _) = cv2.stereoCalibrate( | |
| objectPoints, leftImagePoints, rightImagePoints, | |
| leftCameraMatrix, leftDistortionCoefficients, | |
| rightCameraMatrix, rightDistortionCoefficients, | |
| imageSize, None, None, None, None, | |
| cv2.CALIB_FIX_INTRINSIC, TERMINATION_CRITERIA) | |
| print("Rectifying cameras...") | |
| # TODO: Why do I care about the disparityToDepthMap? | |
| (leftRectification, rightRectification, leftProjection, rightProjection, | |
| dispartityToDepthMap, leftROI, rightROI) = cv2.stereoRectify( | |
| leftCameraMatrix, leftDistortionCoefficients, | |
| rightCameraMatrix, rightDistortionCoefficients, | |
| imageSize, rotationMatrix, translationVector, | |
| None, None, None, None, None, | |
| cv2.CALIB_ZERO_DISPARITY, OPTIMIZE_ALPHA) | |
| print("Saving calibration...") | |
| leftMapX, leftMapY = cv2.initUndistortRectifyMap( | |
| leftCameraMatrix, leftDistortionCoefficients, leftRectification, | |
| leftProjection, imageSize, cv2.CV_32FC1) | |
| rightMapX, rightMapY = cv2.initUndistortRectifyMap( | |
| rightCameraMatrix, rightDistortionCoefficients, rightRectification, | |
| rightProjection, imageSize, cv2.CV_32FC1) | |
| np.savez_compressed(outputFile, imageSize=imageSize, | |
| leftMapX=leftMapX, leftMapY=leftMapY, leftROI=leftROI, | |
| rightMapX=rightMapX, rightMapY=rightMapY, rightROI=rightROI) | |
| cv2.destroyAllWindows() |
| import sys | |
| import numpy as np | |
| import cv2 | |
| REMAP_INTERPOLATION = cv2.INTER_LINEAR | |
| DEPTH_VISUALIZATION_SCALE = 2048 | |
| if len(sys.argv) != 2: | |
| print("Syntax: {0} CALIBRATION_FILE".format(sys.argv[0])) | |
| sys.exit(1) | |
| calibration = np.load(sys.argv[1], allow_pickle=False) | |
| imageSize = tuple(calibration["imageSize"]) | |
| leftMapX = calibration["leftMapX"] | |
| leftMapY = calibration["leftMapY"] | |
| leftROI = tuple(calibration["leftROI"]) | |
| rightMapX = calibration["rightMapX"] | |
| rightMapY = calibration["rightMapY"] | |
| rightROI = tuple(calibration["rightROI"]) | |
| CAMERA_WIDTH = 1280 | |
| CAMERA_HEIGHT = 720 | |
| # TODO: Use more stable identifiers | |
| left = cv2.VideoCapture(0) | |
| right = cv2.VideoCapture(1) | |
| # Increase the resolution | |
| left.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH) | |
| left.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT) | |
| right.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH) | |
| right.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT) | |
| # Use MJPEG to avoid overloading the USB 2.0 bus at this resolution | |
| left.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG")) | |
| right.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"MJPG")) | |
| # The distortion in the left and right edges prevents a good calibration, so | |
| # discard the edges | |
| CROP_WIDTH = 960 | |
| def cropHorizontal(image): | |
| return image[:, | |
| int((CAMERA_WIDTH-CROP_WIDTH)/2): | |
| int(CROP_WIDTH+(CAMERA_WIDTH-CROP_WIDTH)/2)] | |
| # TODO: Why these values in particular? | |
| # TODO: Try applying brightness/contrast/gamma adjustments to the images | |
| stereoMatcher = cv2.StereoBM_create() | |
| stereoMatcher.setMinDisparity(4) | |
| stereoMatcher.setNumDisparities(128) | |
| stereoMatcher.setBlockSize(21) | |
| stereoMatcher.setROI1(leftROI) | |
| stereoMatcher.setROI2(rightROI) | |
| stereoMatcher.setSpeckleRange(16) | |
| stereoMatcher.setSpeckleWindowSize(45) | |
| # Grab both frames first, then retrieve to minimize latency between cameras | |
| while(True): | |
| if not left.grab() or not right.grab(): | |
| print("No more frames") | |
| break | |
| _, leftFrame = left.retrieve() | |
| leftFrame = cropHorizontal(leftFrame) | |
| leftHeight, leftWidth = leftFrame.shape[:2] | |
| _, rightFrame = right.retrieve() | |
| rightFrame = cropHorizontal(rightFrame) | |
| rightHeight, rightWidth = rightFrame.shape[:2] | |
| if (leftWidth, leftHeight) != imageSize: | |
| print("Left camera has different size than the calibration data") | |
| break | |
| if (rightWidth, rightHeight) != imageSize: | |
| print("Right camera has different size than the calibration data") | |
| break | |
| fixedLeft = cv2.remap(leftFrame, leftMapX, leftMapY, REMAP_INTERPOLATION) | |
| fixedRight = cv2.remap(rightFrame, rightMapX, rightMapY, REMAP_INTERPOLATION) | |
| grayLeft = cv2.cvtColor(fixedLeft, cv2.COLOR_BGR2GRAY) | |
| grayRight = cv2.cvtColor(fixedRight, cv2.COLOR_BGR2GRAY) | |
| depth = stereoMatcher.compute(grayLeft, grayRight) | |
| cv2.imshow('left', fixedLeft) | |
| cv2.imshow('right', fixedRight) | |
| cv2.imshow('depth', depth / DEPTH_VISUALIZATION_SCALE) | |
| if cv2.waitKey(1) & 0xFF == ord('q'): | |
| break | |
| left.release() | |
| right.release() | |
| cv2.destroyAllWindows() |
@sanjudk please check your two camera's connection port, i.e., two cameras must have to on at a time.
I keep getting the IOError "Cache file at {0} not found" and I cannot figure out why. The file definitely exists when I print mine out it is VideoFileName.mp4/chessboards.npz. Why do I continue to get this error?