Create a gist now

Instantly share code, notes, and snippets.

@aarmea /0-readme.md
Last active Nov 15, 2017

What would you like to do?
OpenCV Python example
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 commented Nov 15, 2017

Hi aarmea,
Thanks for the post. It is really helpful.
I'm also using ELP dual camera but unable to capture both the images simultaneously. I tried to run your code capture.py but it is exiting from the condition check by printing "No more frames" since it is unable to get both left and right grab. Please suggest fix to this issue.
Thank you.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment