def stereo_initialize():
#left
camera_matrixL = np.array([[219.44178, 0., 157.85633], [0., 219.82357, 122.50906], [0.,0.,1.]])
dist_coefsL = np.array([-0.35878, 0.10911, -0.00069, -0.00088, 0.])
#right
camera_matrixR = np.array([[216.15875, 0., 163.36207], [0., 216.12017, 118.46570], [0.,0.,1.]])
dist_coefsR = np.array([-0.37513, 0.13854, -0.00075, 0.00137, 0.])
#extrinsic
om = np.array([-0.00549, 0.01268, -0.06347])
T = np.array([-63.36115, 0.61235, -1.71106])
R1 = np.zeros(shape=(3,3))
R2 = np.zeros(shape=(3,3))
P1 = np.zeros(shape=(3,3))
P2 = np.zeros(shape=(3,3))
R = cv2.Rodrigues(om)
cv2.stereoRectify(camera_matrixL, dist_coefsL, camera_matrixR, dist_coefsR,(320, 240), R[0], T, R1, R2, P1, P2, Q=None, alpha=-1, newImageSize=(0,0))
map1x, map1y = cv2.initUndistortRectifyMap(camera_matrixL, dist_coefsL, R1, camera_matrixL, (320, 240), cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(camera_matrixR, dist_coefsR, R2, camera_matrixR, (320, 240), cv2.CV_32FC1)
return map1x, map1y, map2x, map2y
def __stereo(self, left, right):
# return self.undistortLeft(left), self.undistortRight(right)
if not self.maps_read:
# clear init flag
self.maps_read = True
# use stereoRectify to calculate what we need to rectify stereo images
M1 = self.info["cameraMatrix1"]
d1 = self.info["distCoeffs1"]
M2 = self.info["cameraMatrix2"]
d2 = self.info["distCoeffs2"]
size = self.info['imageSize']
R = self.info['R']
T = self.info['T']
h, w = size[:2]
R1, R2, self.P1, self.P2, self.Q, roi1, roi2 = cv2.stereoRectify(M1, d1, M2, d2, (w,h), R, T, alpha=self.alpha)
# these return undistortion and rectification maps which are both
# stored in maps_x for camera 1 and 2
self.maps_1 = cv2.initUndistortRectifyMap(M1, d1, R1, self.P1, (w,h), cv2.CV_16SC2) # CV_32F?
self.maps_2 = cv2.initUndistortRectifyMap(M2, d2, R2, self.P2, (w,h), cv2.CV_16SC2)
# return self.__fix2(left, self.maps_1), self.__fix2(right, self.maps_2)
inter=cv2.INTER_LANCZOS4
return cv2.remap(left, self.maps_1[0], self.maps_1[1], inter),
cv2.remap(right, self.maps_2[0], self.maps_2[1], inter)
def rectify(mtx1, dist1, mtx2, dist2, R, T):
# R：行对准的矫正旋转矩阵；P：3*4的左右投影方程；Q：4*4的重投影矩阵
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
mtx1, dist1,
mtx2, dist2,
(BINIMG_W, BINIMG_H),
R,
T,
flags=cv2.CALIB_ZERO_DISPARITY,
alpha=-1,
newImageSize=(BINIMG_W, BINIMG_H)
)
if __name__ == '__main__':
printMat(R1, R2, P1, P2, Q, roi1, roi2)
# 产生校正图像所需的变换参数（mapx, mapy）
mapx1, mapy1 = cv2.initUndistortRectifyMap(
mtx1, dist1,
R1, P1,
(BINIMG_W, BINIMG_H),
cv2.CV_16SC2
)
mapx2, mapy2 = cv2.initUndistortRectifyMap(
mtx2, dist2,
R2, P2,
(BINIMG_W, BINIMG_H),
cv2.CV_16SC2
)
return mapx1, mapy1, mapx2, mapy2, Q, roi1, roi2
def undistortMap(H1, H2, img1, img2):
# http://stackoverflow.com/questions/10192552/rectification-of-uncalibrated-cameras-via-fundamental-matrix
# http://ece631web.groups.et.byu.net/Lectures/ECEn631%2014%20-%20Calibration%20and%20Rectification.pdf
imgsize = (imgL.shape[1], imgL.shape[0])
K = np.array([[50, 0, 20], [0, 50, 30], [0, 0, 1]]) # guess by PT
d = None # Assume no distortion
rH = la.inv(K).dot(H1).dot(K)
lH = la.inv(K).dot(H2).dot(K)
# TODO: lRect or rRect for img1/img2 ??
map1x, map1y = cv2.initUndistortRectifyMap(K, d, rH, K, imgsize,
cv2.CV_16SC2)
map2x, map2y = cv2.initUndistortRectifyMap(K, d, lH, K, imgsize,
cv2.CV_16SC2)
# Convert the images to RGBA (add an axis with 4 values)
img1 = cv2.cvtColor(img1, cv2.COLOR_RGB2RGBA)
img2 = cv2.cvtColor(img2, cv2.COLOR_RGB2RGBA)
return img1, map1x, map1y, img2, map2x, map2y
def draw_rect(K, d, train_frame, R, T, name):
#perform the rectification
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(K, d, K, d, train_frame.shape[:2], R, T, alpha=1)
mapx1, mapy1 = cv2.initUndistortRectifyMap(K, d, R1, K, train_frame.shape[:2], cv2.CV_32F)
mapx2, mapy2 = cv2.initUndistortRectifyMap(K, d, R2, K, query_frame.shape[:2], cv2.CV_32F)
img_rect1 = cv2.remap(train_bckp, mapx1, mapy1, cv2.INTER_LINEAR)
img_rect2 = cv2.remap(query_bckp, mapx2, mapy2, cv2.INTER_LINEAR)
# draw the images side by side
total_size = (max(img_rect1.shape[0], img_rect2.shape[0]), img_rect1.shape[1] + img_rect2.shape[1],3)
img = np.zeros(total_size, dtype=np.uint8)
img[:img_rect1.shape[0], :img_rect1.shape[1]] = img_rect1
img[:img_rect2.shape[0], img_rect1.shape[1]:] = img_rect2
# draw horizontal lines every 25 px accross the side by side image
for i in range(20, img.shape[0], 25):
cv2.line(img, (0, i), (img.shape[1], i), (255, 0, 0))
h1, w1 = train_frame.shape[:2]
h2, w2 = query_frame.shape[:2]
org_imgs = np.zeros((max(h1, h2), w1+w2,3), np.uint8)
org_imgs[:h1, :w1] = train_bckp
org_imgs[:h2, w1:w1+w2] = query_bckp
for m in good:
# draw the keypoints
# print m.queryIdx, m.trainIdx, m.distance
color = tuple([np.random.randint(0, 255) for _ in xrange(3)])
cv2.line(org_imgs, (int(train_keypoints[m.queryIdx].pt[0]), int(train_keypoints[m.queryIdx].pt[1])) , (int(query_keypoints[m.trainIdx].pt[0] + w1), int(query_keypoints[m.trainIdx].pt[1])), color)
cv2.circle(org_imgs, (int(train_keypoints[m.queryIdx].pt[0]), int(train_keypoints[m.queryIdx].pt[1])) , 5, color, 1)
cv2.circle(org_imgs, (int(query_keypoints[m.trainIdx].pt[0] + w1), int(query_keypoints[m.trainIdx].pt[1])) , 5, color, 1)
cv2.imshow('original', org_imgs)
cv2.imshow(name, img)
def on_prepare(self):
self.camera = StereoCamera(self.config.camera.left_id,
self.config.camera.right_id,
self.config.camera.width,
self.config.camera.height)
for view in (self.left_view, self.right_view, self.depth_view):
view.set_default_size(self.camera.width,
self.camera.height)
view.show_all()
self.left_maps = cv2.initUndistortRectifyMap(
self.config.camera.left_intrinsics,
self.config.camera.left_distortion,
self.config.camera.R1, self.config.camera.P1,
self.camera.size, cv2.CV_16SC2)
self.right_maps = cv2.initUndistortRectifyMap(
self.config.camera.right_intrinsics,
self.config.camera.right_distortion,
self.config.camera.R2, self.config.camera.P2,
self.camera.size, cv2.CV_16SC2)
self.update = True
GLib.idle_add(self.on_update)
def hartleyRectify(points1, points2, imgSize, M1, M2, D1, D2, F = None):
F, mask = cv2.findFundamentalMat(points1, points2, cv2.FM_RANSAC, 3, 0.99)
#print 'mask\n', mask
retval, H1, H2 = cv2.stereoRectifyUncalibrated(
points1, points2, F, imgSize)
retval, M1i = cv2.invert(M1); retval, M2i = cv2.invert(M2)
R1, R2 = np.dot(np.dot(M1i, H1), M1), np.dot(np.dot(M2i, H2), M2)
map1x, map1y = cv2.initUndistortRectifyMap(M1, D1, R1, M1, imgSize, cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(M2, D2, R2, M2, imgSize, cv2.CV_32FC1)
return (map1x, map1y, map2x, map2y), F
def doThings(self):
sgbm = cv2.StereoSGBM()
sgbm.SADWindowSize, numberOfDisparitiesMultiplier, sgbm.preFilterCap, sgbm.minDisparity, \
sgbm.uniquenessRatio, sgbm.speckleWindowSize, sgbm.P1, sgbm.P2, \
sgbm.speckleRange = [v for v,_ in self.params.itervalues()]
sgbm.numberOfDisparities = numberOfDisparitiesMultiplier*16
sgbm.disp12MaxDiff = -1
sgbm.fullDP = False
R1, R2, P1, P2, Q, topValidRoi, bottomValidRoi = cv2.stereoRectify(self.M1, self.D1, self.M2, self.D2,
(self.top.shape[1],self.top.shape[0]), self.R, self.T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=0)
top_map1, top_map2 = cv2.initUndistortRectifyMap(self.M1, self.D1, R1, P1,
(self.top.shape[1],self.top.shape[0]), cv2.CV_16SC2)
bottom_map1, bottom_map2 = cv2.initUndistortRectifyMap(self.M2, self.D2, R2, P2,
(self.bottom.shape[1], self.bottom.shape[0]), cv2.CV_16SC2)
self.top_r = cv2.remap(self.top, top_map1, top_map2, cv2.cv.CV_INTER_LINEAR);
self.bottom_r = cv2.remap(self.bottom, bottom_map1, bottom_map2, cv2.cv.CV_INTER_LINEAR)
top_small = cv2.resize(self.top_r, (self.top_r.shape[1]/2,self.top_r.shape[0]/2))
bottom_small = cv2.resize(self.bottom_r, (self.bottom_r.shape[1]/2,self.bottom_r.shape[0]/2))
cv2.imshow('top', top_small);
cv2.imshow('bottom', bottom_small);
# top_r = cv2.equalizeHist(top_r)
top_r = cv2.blur(self.top_r, (5,5))
# bottom_r = cv2.equalizeHist(bottom_r)
bottom_r = cv2.blur(self.bottom_r, (5,5))
dispTop = sgbm.compute(top_r.T, bottom_r.T).T;
dispTopPositive = dispTop
dispTopPositive[dispTop<0] = 0
disp8 = (dispTopPositive / (sgbm.numberOfDisparities * 16.) * 255).astype(np.uint8);
disp_small = cv2.resize(disp8, (disp8.shape[1]/2, disp8.shape[0]/2));
cv2.imshow(self.winname, disp_small);
self.disp8 = disp8
self.xyz = cv2.reprojectImageTo3D(dispTop, Q, handleMissingValues=True)
# self.xyzrgb = np.zeros((self.xyz.shape[0],self.xyz.shape[1],4))
# import struct
# def color_to_float(color):
# if color.size == 1:
# color = [color]*3
# rgb = (color[2] << 16 | color[1] << 8 | color[0]);
# rgb_hex = hex(rgb)[2:-1]
# s = '0'*(8-len(rgb_hex)) + rgb_hex.capitalize()
## print color, rgb, hex(rgb)
# rgb_float = struct.unpack('!f', s.decode('hex'))[0]
## print rgb_float
# return rgb_float
# for i in range(self.xyz.shape[0]):
# for j in range(self.xyz.shape[1]):
# self.xyzrgb[i,j] = np.append(self.xyz[i,j], color_to_float(self.top[i,j]))
def rectify_images(img1, x1, img2, x2, K, d, F, shearing=False):
imsize = (img1.shape[1], img1.shape[0])
H1, H2 = epipolar.rectify_uncalibrated(x1, x2, F, imsize)
#x1_1d = np.empty((2*x1.shape[1],), dtype=float)
#x1_1d[0::2] = x1[0,:]
#x1_1d[1::2] = x1[1,:]
#x2_1d = np.empty((2*x2.shape[1],), dtype=float)
#x2_1d[0::2] = x2[0,:]
#x2_1d[1::2] = x2[1,:]
#success, cvH1, cvH2 = cv2.stereoRectifyUncalibrated(x1_1d, x2_1d, F, imsize)
if shearing:
S = epipolar.rectify_shearing(H1, H2, imsize)
H1 = S.dot(H1)
rH = la.inv(K).dot(H1).dot(K)
lH = la.inv(K).dot(H2).dot(K)
# TODO: lRect or rRect for img1/img2 ??
map1x, map1y = cv2.initUndistortRectifyMap(K, d, rH, K, imsize,
cv.CV_16SC2)
map2x, map2y = cv2.initUndistortRectifyMap(K, d, lH, K, imsize,
cv.CV_16SC2)
# Convert the images to RGBA (add an axis with 4 values)
img1 = np.tile(img1[:,:,np.newaxis], [1,1,4])
img1[:,:,3] = 255
img2 = np.tile(img2[:,:,np.newaxis], [1,1,4])
img2[:,:,3] = 255
rimg1 = cv2.remap(img1, map1x, map1y,
interpolation=cv.CV_INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT,
borderValue=(0,0,0,0))
rimg2 = cv2.remap(img2, map2x, map2y,
interpolation=cv.CV_INTER_LINEAR,
borderMode=cv2.BORDER_CONSTANT,
borderValue=(0,0,0,0))
# Put a red background on the invalid values
# TODO: Return a mask for valid/invalid values
# TODO: There is aliasing hapenning on the images border. We should
# invalidate a margin around the border so we're sure we have only valid
# pixels
rimg1[rimg1[:,:,3] == 0,:] = (255,0,0,255)
rimg2[rimg2[:,:,3] == 0,:] = (255,0,0,255)
return rimg1, rimg2
def __init__(self, params, winname, top, bottom):
self.top = top
self.bottom = bottom
top_small = cv2.resize(top, (top.shape[1] / 2, top.shape[0] / 2))
bottom_small = cv2.resize(bottom, (bottom.shape[1] / 2, bottom.shape[0] / 2))
cv2.imshow('top', top_small);
cv2.imshow('bottom', bottom_small);
extrinsic_filepath = config.PROJPATH + 'extrinsics.yml'
intrinsic_filepath = config.PROJPATH + 'intrinsics.yml'
self.R = np.asarray(cv2.cv.Load(extrinsic_filepath, name='R'))
self.T = np.asarray(cv2.cv.Load(extrinsic_filepath, name='T'))
self.R1 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='R1'))
self.R2 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='R2'))
self.P1 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='P1'))
self.P2 = np.asarray(cv2.cv.Load(extrinsic_filepath, name='P2'))
self.Q = np.asarray(cv2.cv.Load(extrinsic_filepath, name='Q'))
self.M1 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='M1'))
self.M2 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='M2'))
self.D1 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='D1'))
self.D2 = np.asarray(cv2.cv.Load(intrinsic_filepath, name='D2'))
self.do_tune = config.TUNE_DISPARITY_MAP
R1, R2, P1, P2, self.Q, topValidRoi, bottomValidRoi = cv2.stereoRectify(self.M1, self.D1, self.M2, self.D2,
(self.top.shape[1], self.top.shape[0]), self.R, self.T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=-1)
top_map1, top_map2 = cv2.initUndistortRectifyMap(self.M1, self.D1, R1, P1,
(self.top.shape[1], self.top.shape[0]), cv2.CV_16SC2)
bottom_map1, bottom_map2 = cv2.initUndistortRectifyMap(self.M2, self.D2, R2, P2,
(self.bottom.shape[1], self.bottom.shape[0]), cv2.CV_16SC2)
self.top_r = cv2.remap(self.top, top_map1, top_map2, cv2.cv.CV_INTER_LINEAR);
self.bottom_r = cv2.remap(self.bottom, bottom_map1, bottom_map2, cv2.cv.CV_INTER_LINEAR)
top_r_small = cv2.resize(self.top_r, (self.top_r.shape[1] / 2, self.top_r.shape[0] / 2))
bottom_r_small = cv2.resize(self.bottom_r, (self.bottom_r.shape[1] / 2, self.bottom_r.shape[0] / 2))
cv2.imshow('top rectified', top_r_small);
cv2.imshow('bottom rectified', bottom_r_small);
tx1,ty1,tx2,ty2 = topValidRoi
bx1,by1,bx2,by2 = bottomValidRoi
self.roi = (max(tx1, bx1), max(ty1, by1), min(tx2, bx2), min(ty2, by2))
self.top_r = cv2.blur(self.top_r, (5, 5))
self.bottom_r = cv2.blur(self.bottom_r, (5, 5))
# top_r = cv2.equalizeHist(self.top_r)
# bottom_r = cv2.equalizeHist(self.bottom_r)
super(SGBMTuner, self).__init__(params, winname)
def update(self, queue):
self.camera = PiCamera()
self.image = None
self.camera.resolution = (w, h)
self.camera.framerate = 60
self.camera.brightness = 70
self.camera.contrast = 100
self.rawCapture = PiRGBArray(self.camera, size=(w, h))
time.sleep(0.1)
mtx = np.matrix([[313.1251541, 0., 157.36763381],
[0., 311.84837219, 130.36209271],
[0., 0., 1.]])
dist = np.matrix([[-0.42159111, 0.44966352, -0.00877638, 0.00070653, -0.43508731]])
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 0, (w, h))
self.mapx, self.mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
self.stream = self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True)
self.stopped = False
for f in self.stream:
self.image = cv2.remap(f.array, self.mapx, self.mapy, cv2.INTER_LINEAR)
self.rawCapture.truncate(0)
if not q.full():
queue.put(self.image, False)
if self.stopped:
self.stream.close()
self.rawCapture.close()
self.camera.close()
return
def main(argv): camera = np.load(argv[0]) images_path = argv[1] output_dir = argv[2] if not os.path.exists(output_dir): os.makedirs(output_dir) images = glob.glob(images_path + '/*.jpg') for fname in images: img = cv2.imread(fname) h, w = img.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix( camera['mtx'], camera['dist'], (w,h), 0, # 0.6 (w,h) ) mapx,mapy = cv2.initUndistortRectifyMap( camera['mtx'], camera['dist'], None, newcameramtx, (w,h), 5 ) dst = cv2.remap(img,mapx,mapy,cv2.INTER_LINEAR) cv2.imwrite(output_dir + '/' + os.path.basename(fname), dst)
def correctDistortion(image):
size = image.shape[1],image.shape[0]
corners = findCorners(image)
patternPoints = np.zeros( (np.prod(boardSize), 3), np.float32 )
patternPoints[:,:2] = np.indices(boardSize).T.reshape(-1, 2)
imagePoints = np.array([corners.reshape(-1, 2)])
objectPoints = np.array([patternPoints])
cameraMatrix = np.zeros((3, 3))
distCoefs = np.zeros(4)
rc,cameraMatrix,distCoeffs,rvecs,tvecs = cv2.calibrateCamera(
objectPoints,
imagePoints,
boardSize,
cameraMatrix,
distCoefs
)
newCameraMatrix,newExtents = cv2.getOptimalNewCameraMatrix(cameraMatrix,distCoeffs,size,0.0)
mapx, mapy = cv2.initUndistortRectifyMap(
cameraMatrix,
distCoeffs,
None,
cameraMatrix,
size,
cv2.CV_32FC1
)
newImage = cv2.remap( image, mapx, mapy, cv2.INTER_LANCZOS4 )
return newImage
def captureImage(self, mirror=False, flush=False, flushValue=1): """ If mirror is set to True, the image will be displayed as a mirror, otherwise it will be displayed as the camera sees it """ if self.isConnected: self.reading = True if flush: for i in xrange(0, flushValue): self.capture.read() #grab() ret, image = self.capture.read() self.reading = False if ret: if self.useDistortion and \ self.cameraMatrix is not None and \ self.distortionVector is not None and \ self.distCameraMatrix is not None: mapx, mapy = cv2.initUndistortRectifyMap(self.cameraMatrix, self.distortionVector, R=None, newCameraMatrix=self.distCameraMatrix, size=(self.width, self.height), m1type=5) image = cv2.remap(image, mapx, mapy, cv2.INTER_LINEAR) image = cv2.transpose(image) if not mirror: image = cv2.flip(image, 1) self._success() return cv2.cvtColor(image, cv2.COLOR_BGR2RGB) else: self._fail() return None else: self._fail() return None
def dewarp(imagedir):
# Loading from json file
C = CameraParams.fromfile(os.path.join(imagedir, "params.json"))
K = C.K
D = C.D
print("Loaded camera parameters from " + os.path.join(imagedir, "params.json"))
for f in file_list(imagedir, ['jpg', 'jpeg', 'png']):
print(f)
colour = cv2.imread(f)
grey = cv2.cvtColor(colour, cv2.COLOR_BGR2GRAY)
h, w = grey.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(K, D, (w,h), 1, (w,h))
mapx, mapy = cv2.initUndistortRectifyMap(K, D, None, newcameramtx, (w,h), 5)
dewarped = cv2.remap(grey, mapx, mapy, cv2.INTER_LINEAR)
x, y, w, h = roi
dewarped = dewarped[y:y+h, x:x+w]
grey = cv2.resize(grey, (0,0), fx=0.5, fy=0.5)
dewarped = cv2.resize(dewarped, (0,0), fx=0.5, fy=0.5)
cv2.imshow("Original", grey )
cv2.imshow("Dewarped", dewarped)
cv2.waitKey(-1)
def undistort_image(img, cameraMatrix, distCoeffs, imageSize):
"""
Undistort image "img",
shot with a camera with intrinsics ("cameraMatrix", "distCoeffs", and "imageSize" (w, h)).
Apart from the undistorted image, a region-of-interest will also be returned.
See OpenCV's doc about the format of "cameraMatrix" and "distCoeffs".
"""
# Refine cameraMatrix, and calculate RegionOfInterest
cameraMatrix_new, roi = cv2.getOptimalNewCameraMatrix(
cameraMatrix, distCoeffs, imageSize,
1 ) # all source image pixels retained in undistorted image
# Undistort
mapX, mapY = cv2.initUndistortRectifyMap(
cameraMatrix, distCoeffs,
None, # optional rectification transformation
cameraMatrix_new, imageSize,
5 ) # type of the first output map (CV_32FC1)
img_undistorted = cv2.remap(
img, mapX, mapY, cv2.INTER_LINEAR )
# Crop the image
x,y, w,h = roi
img_undistorted = img_undistorted[y:y+h, x:x+w]
return img_undistorted, roi
def undistort(mtx, dist, objpoints, imgpoints, rvecs, tvecs, img):
#read in an image of choice
#usefule for the reading in and segmenting of board to make hough lines easier
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.png', dst)
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.png', dst)
mean_error = 0
for i in xrange(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print "total error: ", mean_error / len(objpoints)
def undistort(path, imagesArr, K, d):
print '\n-------- Undistort Images ---------'
for fname in imagesArr:
print 'Undistorting', os.path.basename(fname),
img = cv2.imread(fname)
if img is None:
print ' - Failed to load.'
continue
h, w = img.shape[:2]
# Calculate new optimal matrix to avoid losing pixels in the edges after undistortion
new_matrix, roi = cv2.getOptimalNewCameraMatrix(K, d, (w, h), 1)
# Generate undistorted image
#newimg = cv2.undistort(img, K, d, None, new_matrix)
# Alternative undistortion via remapping
mapx, mapy = cv2.initUndistortRectifyMap(K, d, None, new_matrix, (w, h), cv2.CV_32FC1)
newimg = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# Output undistorted image to the same location with postfix '_undistorted'
f = os.path.basename(fname).split('.')
newimg_path = os.path.join(path, ".".join(f[:-1]) + '_undistorted.' + f[-1])
cv2.imwrite(newimg_path, newimg)
print '----->', newimg_path
print 'Undistorted', len(imagesArr), 'images.'
print '-------- End of Undistortion ---------\n'
def calibrate_cameras(self):
"""Calibrate cameras based on found chessboard corners."""
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
flags = cv2.CALIB_FIX_ASPECT_RATIO + cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_SAME_FOCAL_LENGTH
calib = StereoCalibration()
(
calib.cam_mats["left"],
calib.dist_coefs["left"],
calib.cam_mats["right"],
calib.dist_coefs["right"],
calib.rot_mat,
calib.trans_vec,
calib.e_mat,
calib.f_mat,
) = cv2.stereoCalibrate(
self.object_points,
self.image_points["left"],
self.image_points["right"],
self.image_size,
criteria=criteria,
flags=flags,
)[
1:
]
(
calib.rect_trans["left"],
calib.rect_trans["right"],
calib.proj_mats["left"],
calib.proj_mats["right"],
calib.disp_to_depth_mat,
calib.valid_boxes["left"],
calib.valid_boxes["right"],
) = cv2.stereoRectify(
calib.cam_mats["left"],
calib.dist_coefs["left"],
calib.cam_mats["right"],
calib.dist_coefs["right"],
self.image_size,
calib.rot_mat,
calib.trans_vec,
flags=0,
)
for side in ("left", "right"):
(calib.undistortion_map[side], calib.rectification_map[side]) = cv2.initUndistortRectifyMap(
calib.cam_mats[side],
calib.dist_coefs[side],
calib.rect_trans[side],
calib.proj_mats[side],
self.image_size,
cv2.CV_32FC1,
)
# This is replaced because my results were always bad. Estimates are
# taken from the OpenCV samples.
width, height = self.image_size
focal_length = 0.8 * width
calib.disp_to_depth_mat = np.float32(
[[1, 0, 0, -0.5 * width], [0, -1, 0, 0.5 * height], [0, 0, 0, -focal_length], [0, 0, 1, 0]]
)
return calib
def computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, distortionCoefficients):
from copy import deepcopy
from numpy import identity
newImgSize = (int(round(width*undistortedImageMultiplication)), int(round(height*undistortedImageMultiplication)))
newCameraMatrix = deepcopy(intrinsicCameraMatrix)
newCameraMatrix[0,2] = newImgSize[0]/2.
newCameraMatrix[1,2] = newImgSize[1]/2.
return cv2.initUndistortRectifyMap(intrinsicCameraMatrix, array(distortionCoefficients), identity(3), newCameraMatrix, newImgSize, cv2.CV_32FC1)
def rectifyImage(self, raw, rectified):
"""
:param raw: input image
:type raw: :class:`CvMat` or :class:`IplImage`
:param rectified: rectified output image
:type rectified: :class:`CvMat` or :class:`IplImage`
Applies the rectification specified by camera parameters :math:`K` and and :math:`D` to image `raw` and writes the resulting image `rectified`.
"""
self.mapx = numpy.ndarray(shape=(self.height, self.width, 1),
dtype='float32')
self.mapy = numpy.ndarray(shape=(self.height, self.width, 1),
dtype='float32')
cv2.initUndistortRectifyMap(self.K, self.D, self.R, self.P,
(self.width, self.height), cv2.CV_32FC1, self.mapx, self.mapy)
cv2.remap(raw, self.mapx, self.mapy, cv2.INTER_CUBIC, rectified)
def generate_maps(self):
logging.debug("Generating maps")
logging.debug("Generating distortion map")
self.map_x, self.map_y = cv2.initUndistortRectifyMap(self.original_camera_matrix, self.coefficients, None,
self.new_camera_matrix, self.image_size, 5)
logging.debug("Generating distortion map - Complete")
#self._generate_reverse_map()
logging.debug("Generating maps - Complete.")
def undistort_brown_image(image, camera, new_camera, interpolation):
"""Remove radial distortion from a brown image."""
height, width = image.shape[:2]
K = camera.get_K_in_pixel_coordinates(width, height)
distortion = np.array([camera.k1, camera.k2, camera.p1, camera.p2, camera.k3])
new_K = new_camera.get_K_in_pixel_coordinates(width, height)
map1, map2 = cv2.initUndistortRectifyMap(
K, distortion, None, new_K, (width, height), cv2.CV_32FC1)
return cv2.remap(image, map1, map2, interpolation)
def capture_images(self, extrin, points, noise2d=0.0):
"""
Args:
extrin: Extrinsics
points: list of dictionaries, each dictionary representing a board
nosie2d: std. dev. of point detection
Returns:
a list of 2xN numpy array, each representing a captured board
"""
mapx,mapy = cv2.initUndistortRectifyMap(self.intrinsics.intri_mat, \
#np.concatenate( (self.intrinsics.radial_dist[0:2],self.intrinsics.tang_dist[:], np.asarray([ self.intrinsics.radial_dist[-1] ])) ,axis = 0), \
self.get_opencv_dist_coeffs(), \
np.eye(3), \
self.intrinsics.intri_mat, \
self.size,\
cv2.CV_32FC1)
img_pts = []
if isinstance(points[0], dict):
print "capture images points argument bad input type", type(points), " of ", type(points[0])
# for chessboard in points:
# img_pts_per_chessboard = {}
# for point_id in chessboard:
# pts = self.project_point(extrin, chessboard[point_id])
# #apply distortion
# if(pts[0,0] >=0 and pts[0,0] < self.size[0] and pts[1,0] >= 0 and pts[1,0] < self.size[1] ):
# # final_pts = np.zeros((2,1))
# x_ = mapx[pts[0,0],pts[1,0]]
# y_ = mapy[pts[0,0],pts[1,0]]
# final_pts[0] = x_ #col
# final_pts[1] = y_ #row
# final_pts = np.array([x_, y_]).reshape(2,1)
# # final_pts = np.array([pts[0,0], pts[1,0]]).reshape(2,1)
# img_pts_per_chessboard[point_id] = final_pts
# # add noise
# if noise2d > 0:
# # noises = np.concatenate((np.random.normal(0, noise2d, (2,1)),\
# # np.zeros((1,1))), axis=0)
# noises = np.random.normal(0, noise2d, (2,1))
# img_pts_per_chessboard[point_id] += noises
# img_pts.append( img_pts_per_chessboard )
elif isinstance(points[0], np.ndarray) and points[0].shape[0] == 3:
for chessboard in points: #3xN numpy array
#projectPoints expect Nx3 points
img_pts_per_chessboard,_ = cv2.projectPoints(chessboard.T, \
extrin.rot_vec, extrin.trans_vec, \
self.intrinsics.intri_mat, self.get_opencv_dist_coeffs())
if noise2d > 0:
noises = np.random.normal(0, noise2d, img_pts_per_chessboard.shape)
img_pts_per_chessboard += noises
img_pts.append(img_pts_per_chessboard.reshape(-1,2).T)
else:
print "capture images points argument bad input type", type(points), " of ", type(points[0])
import pdb; pdb.set_trace()
return img_pts
def computeStereoRectify(params):
imsize = (1280,960)
cameraMatrix1 = params['cam'][0]['KK']
distCoeffs1 = params['cam'][0]['distort']
cameraMatrix2 = params['cam'][1]['KK']
distCoeffs2 = params['cam'][1]['distort']
(R1, R2, P1, P2, Q, size1, size2) = cv2.stereoRectify(
cameraMatrix1, distCoeffs1,
cameraMatrix2, distCoeffs2,
imsize,
params['cam'][0]['E_R'],
params['cam'][0]['E_t'])
map1x, map1y = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imsize, cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, imsize, cv2.CV_32FC1)
return (R1, R2, P1, P2, Q, size1, size2, map1x, map1y, map2x, map2y)
def __init__(self, context, **kwargs):
super(ImageUndistorter, self).__init__(**kwargs)
self.UndistMapX, self.UndistMapY = cv2.initUndistortRectifyMap(
np.asarray(self.cameraMatrix),
np.asarray(self.distortCoefs),
None,
np.asarray(self.cameraMatrix),
tuple(self.imageSize),
cv2.CV_32FC1)
def plot_rectified_images(self, feat_mode="SURF"):
"""Plots rectified images
This method computes and plots a rectified version of the two
images side by side.
:param feat_mode: whether to use rich descriptors for feature
matching ("surf") or optic flow ("flow")
"""
self._extract_keypoints(feat_mode)
self._find_fundamental_matrix()
self._find_essential_matrix()
self._find_camera_matrices_rt()
R = self.Rt2[:, :3]
T = self.Rt2[:, 3]
#perform the rectification
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(self.K, self.d,
self.K, self.d,
self.img1.shape[:2],
R, T, alpha=1.0)
mapx1, mapy1 = cv2.initUndistortRectifyMap(self.K, self.d, R1, self.K,
self.img1.shape[:2],
cv2.CV_32F)
mapx2, mapy2 = cv2.initUndistortRectifyMap(self.K, self.d, R2, self.K,
self.img2.shape[:2],
cv2.CV_32F)
img_rect1 = cv2.remap(self.img1, mapx1, mapy1, cv2.INTER_LINEAR)
img_rect2 = cv2.remap(self.img2, mapx2, mapy2, cv2.INTER_LINEAR)
# draw the images side by side
total_size = (max(img_rect1.shape[0], img_rect2.shape[0]),
img_rect1.shape[1] + img_rect2.shape[1], 3)
img = np.zeros(total_size, dtype=np.uint8)
img[:img_rect1.shape[0], :img_rect1.shape[1]] = img_rect1
img[:img_rect2.shape[0], img_rect1.shape[1]:] = img_rect2
# draw horizontal lines every 25 px accross the side by side image
for i in range(20, img.shape[0], 25):
cv2.line(img, (0, i), (img.shape[1], i), (255, 0, 0))
cv2.imshow('imgRectified', img)
cv2.waitKey()
def getMap(self, refFrame):
"""
Returns the x and y maps used to remap the pixels in the remap function.
:param refFrame: An image with the same property as the calibration and target frames.
"""
h, w = refFrame.shape[:2]
mapX, mapY = cv2.initUndistortRectifyMap(self.cameraMatrix, self.distortionCoeffs, None,
self.optimalCameraMatrix, (w, h), 5)
mapX2, mapY2 = cv2.convertMaps(mapX, mapY, cv2.CV_16SC2)
return (mapX2, mapY2)
def compute_stereo_rectification_maps(stereo_rig, im_size, size_factor):
new_size = (int(im_size[1] * size_factor), int(im_size[0] * size_factor))
rotation1, rotation2, pose1, pose2 = \
cv2.stereoRectify(cameraMatrix1=stereo_rig.cameras[0].intrinsics.intrinsic_mat,
distCoeffs1=stereo_rig.cameras[0].intrinsics.distortion_coeffs,
cameraMatrix2=stereo_rig.cameras[1].intrinsics.intrinsic_mat,
distCoeffs2=stereo_rig.cameras[1].intrinsics.distortion_coeffs,
imageSize=(im_size[1], im_size[0]),
R=stereo_rig.cameras[1].extrinsics.rotation,
T=stereo_rig.cameras[1].extrinsics.translation,
flags=cv2.CALIB_ZERO_DISPARITY,
newImageSize=new_size
)[0:4]
map1x, map1y = cv2.initUndistortRectifyMap(stereo_rig.cameras[0].intrinsics.intrinsic_mat,
stereo_rig.cameras[0].intrinsics.distortion_coeffs,
rotation1, pose1, new_size, cv2.CV_32FC1)
map2x, map2y = cv2.initUndistortRectifyMap(stereo_rig.cameras[1].intrinsics.intrinsic_mat,
stereo_rig.cameras[1].intrinsics.distortion_coeffs,
rotation2, pose2, new_size, cv2.CV_32FC1)
return map1x, map1y, map2x, map2y
def calibrateImage(frame,camera_mtx,distortions):
h,w = frame.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(camera_mtx,distortions,(w,h),1,(w,h))
#undistort
mapx,mapy = cv2.initUndistortRectifyMap(camera_mtx,distortions,None,newcameramtx,(w,h),5)
undistorted = cv2.remap(frame,mapx,mapy,cv2.INTER_LINEAR)
#crop the image
x,y,w,h = roi
undistorted = undistorted[y:y+h,x:x+w]
return undistorted
camera_matrix[0, 2] = 3.2516069906172453e+02
camera_matrix[1, 1] = 5.4801781476172562e+02
camera_matrix[1, 2] = 2.4794113960783835e+02
camera_matrix[2, 2] = 1
dist_coeffs = np.array([ 3.7230261423972011e-02, -1.6171708069773008e-01, -3.5260752900266357e-04, 1.7161234226767313e-04, 1.0192711400840315e-01 ])
new_camera_matrix = np.zeros(shape=(3, 3))
new_camera_matrix[0, 0] = 518.8579
new_camera_matrix[0, 2] = 320
new_camera_matrix[1, 1] = 518.8579
new_camera_matrix[1, 2] = 240
new_camera_matrix[2, 2] = 1
R = np.identity(3, dtype=np.float)
map1, map2 = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, R, new_camera_matrix, (640, 480), cv2.CV_32FC1)
def load_model():
params = bts_parameters(
encoder=args.encoder,
height=args.input_height,
width=args.input_width,
batch_size=None,
dataset=None,
max_depth=args.max_depth,
num_gpus=None,
num_threads=None,
num_epochs=None,
)
model = BtsModel(params, 'test', image, None, focal=focals, bn_training=False)
#flags |= cv2.CALIB_SAME_FOCAL_LENGTH
#flags |= cv2.CALIB_FIX_K3
#flags |= cv2.CALIB_FIX_K4
#flags |= cv2.CALIB_FIX_K5
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(
objpoints, imgpointsL, imgpointsR, mtxL, distL, mtxR, distR,
ChessImaR.shape[::-1], criteria_stereo, flags)
# StereoRectify function
rectify_scale = 0 # if 0 image croped, if 1 image nor croped
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(
MLS, dLS, MRS, dRS, ChessImaR.shape[::-1], R, T, rectify_scale,
(0, 0)) # last paramater is alpha, if 0= croped, if 1= not croped
# initUndistortRectifyMap function
Left_Stereo_Map = cv2.initUndistortRectifyMap(
MLS, dLS, RL, PL, ChessImaR.shape[::-1], cv2.CV_16SC2
) # cv2.CV_16SC2 this format enables us the programme to work faster
Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
ChessImaR.shape[::-1],
cv2.CV_16SC2)
#*******************************************
#***** Parameters for the StereoVision *****
#*******************************************
# Create StereoSGBM and prepare all parameters
window_size = 3
min_disp = 2
num_disp = 130 - min_disp
stereo = cv2.StereoSGBM_create(minDisparity=min_disp,
numDisparities=num_disp,
blockSize=window_size,
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
mean_error += error
#print("total error: {}".format(mean_error / len(objpoints)))
# undistortion
path_file = os.path.join(path, 'left1.jpg')
img = cv2.imread(path_file)
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
if True:
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
else:
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
#cv2.imshow('distorted', img)
#cv2.imshow('calibresult', dst)
#cv2.waitKey(0)
file_name = 'calibration_1_cell.json'
json_file = os.path.join(path, file_name)
data = {
'K': mtx.tolist(),
def __init__(self):
self.print_usb_info()
self.try_ids = [0, 1, 2, 3]
self.try_ids = [0]
self.open_all(self.try_ids)
print(self.captures.keys())
# image_all()
self.id = self.try_ids[0]
names = {}
names[0] = "round"
names[1] = "black"
names[2] = "gray"
names[3] = "clips"
self.names = names
self.undistort = False
# captures = []
# cams = [id for id in try_ids]
mtx, dist = (None, None)
while True:
# print(captures)
self.cap = self.captures[self.id]
cap = self.cap
if cap is not None:
if not self.undistort:
self.im = self.cap_show(cap)
else:
self.im = self.cap_show(cap)
# dst = cv2.undistort(im, mtx, dist, None, newcameramtx)
# # crop the image
# x,y,w,h = roi
# dst = dst[y:y+h, x:x+w]
h, w = self.im.shape[:2]
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(
mtx, dist, None, newcameramtx, (w, h), 5)
dst = cv2.remap(self.im, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
# cv2.imwrite('calibresult.png',dst)
self.im = self.im_show(dst)
# print(self.im.shape)
key = cv2.waitKey(1) & 0xFF
if int(key) == 255:
# print(key)
continue
if key == ord("q"):
break
elif key == ord("b"):
print("it")
else:
key_int = int(key) - 48
print(self.captures.keys(), key_int)
if key_int in self.captures.keys():
print("Selecting source[{}] ".format(key_int))
self.id = key_int
else:
self.open_all([key_int])
# if 0 < key_int and key_int < len(captures) :
if key_int in self.captures.keys():
cap = self.captures[key_int]
ret, im = cap.read()
print(im.shape)
if im is not None:
print("Choosing source[{}] ".format(key_int))
self.id = key_int
else:
print("Source[{}] cannot be read from".format(
key_int))
self.close_all([self.id])
# if key in function_keys
if key == ord("s"):
self.save_image(self.im)
elif key == ord("d"):
def load_calib(name):
folder = r"D:\DEV\PYTHON\pyCV\calibration\_pics"
def load_matrix(folder, file):
path = os.path.join(folder, name, file)
mat = np.loadtxt(path)
return np.array(mat)
mtx = load_matrix(folder, "Intrinsic.txt")
dist = load_matrix(folder, "Distortion.txt")
print("Intrinsic: {}\nDistortion: {}".format(
mtx, dist))
return mtx, dist
mtx, dist = load_calib(names[self.id])
h, w = self.im.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(self.im, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
# cv2.imwrite('calibresult.png',dst)
self.im = dst
self.undistort = True
elif key == ord("u"):
self.undistort = False
elif key == ord("a"):
for q in range(4, 0, -1):
print("Taking 20 screenshots in {}".format(q))
self.wait_show()
for q in range(20):
self.wait_show()
self.save_image(self.im)
self.close_all()
import time
import RPi.GPIO as GPIO
from AlphaBot import AlphaBot
# load calibration file
with open('calibration.yaml') as f:
loadeddict = yaml.load(f)
camera_matrix = loadeddict.get('camera_matrix')
dist_coeffs = loadeddict.get('dist_coeff')
camera_matrix = np.matrix(camera_matrix)
dist_coeffs = np.matrix(dist_coeffs)
image_size = (640, 480)
map1, map2 = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None,
None, image_size, cv2.CV_16SC2)
#import 6x6 arUco tags lib
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000)
# marker sizes that we used were ~ 10cmx10cm
markerLength = 9.60 # Here, our measurement unit is centimetre.
arucoParams = aruco.DetectorParameters_create()
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(640, 480))
#*****
#capL = cv2.VideoCapture(1)
#ret, frame = capR.read()
#height, width, depth = frame.shape
#print height, width, depth
Q = np.loadtxt('./matrix_dir/Q.txt')
cameraMatrixL = np.loadtxt('./matrix_dir/stereo_cameraMatrixL.txt')
cameraMatrixR = np.loadtxt('./matrix_dir/stereo_cameraMatrixR.txt')
distCoeffsL = np.loadtxt('./matrix_dir/stereo_distCoeffsL.txt')
distCoeffsR = np.loadtxt('./matrix_dir/stereo_distCoeffsR.txt')
R1 = np.loadtxt('./matrix_dir/R1.txt')
R2 = np.loadtxt('./matrix_dir/R2.txt')
P1 = np.loadtxt('./matrix_dir/P1.txt')
P2 = np.loadtxt('./matrix_dir/P2.txt')
left_maps = cv2.initUndistortRectifyMap(cameraMatrixL, distCoeffsL, R1, P1, (640,480), cv2.CV_16SC2)
right_maps= cv2.initUndistortRectifyMap(cameraMatrixR, distCoeffsR, R2, P2, (640,480), cv2.CV_16SC2)
xyz_init = np.zeros((480,640,3))
dst = np.zeros((480, 640))
for x in range(640):
for y in range(480):
xyz_init[y][x][0] = x
xyz_init[y][x][1] = y
capR = cv2.VideoCapture(1)
capL = cv2.VideoCapture(0)
def update():
colors = ((1.0, 1.0, 1.0, 1.0))
cv2.imshow('rawcamerainput', cv_image)
cv2.waitKey(1)
def main(args):
ic = image_converter()
rospy.init_node('image_processor', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
if __name__ == '__main__':
npz_calib_file = np.load(
'/home/sine/catkin_ws/src/vatsal/src/leftcam_fisheye_correction.npz')
distCoeff = npz_calib_file['distCoeff']
intrinsic_matrix = npz_calib_file['intrinsic_matrix']
npz_calib_file.close()
newMat, ROI = cv2.getOptimalNewCameraMatrix(intrinsic_matrix,
distCoeff, (1280, 720),
alpha=5.5,
centerPrincipalPoint=1)
mapx, mapy = cv2.initUndistortRectifyMap(intrinsic_matrix,
distCoeff,
None,
newMat, (1280, 720),
m1type=cv2.CV_32FC1)
main(sys.argv)
def calibrate_cameras():
capL = cv2.VideoCapture(0)
capR = cv2.VideoCapture(2)
# capL.set(3, camera_width)
# capL.set(4, camera_height)
capL.set(5, fps)
# capR.set(3, camera_width)
# capR.set(4, camera_height)
capR.set(5, fps)
fpsLeft = capL.get(5)
fpsRight = capR.get(5)
print("Left Cam FPS: ", fpsLeft)
print("Right Cam FPS: ", fpsRight)
if not capL.isOpened():
print("Unable to read LEFT camera")
capL.release()
if not capR.isOpened():
print("Unable to read RIGHT camera")
capR.release()
# Global variables preset
_, testFrame = capL.read()
img_height, img_width, _ = testFrame.shape
resolution = (img_width, img_height)
print(resolution)
all_corners_left = []
all_ids_left = []
all_corners_right = []
all_ids_right = []
required_count = 75
frame_idx = 0
frame_spacing = 5
success = False
print('Start cycle')
with tqdm(total=required_count, file=sys.stdout) as pbar:
while True:
retL, frameL = capL.read()
retR, frameR = capR.read()
ret = retL and retR
if ret:
grayL = cv2.cvtColor(frameL, cv2.COLOR_BGR2GRAY)
grayR = cv2.cvtColor(frameR, cv2.COLOR_BGR2GRAY)
marker_corners_left, marker_ids_left, _ = cv2.aruco.detectMarkers(
grayL, charucoDictionary, parameters=detectorParams)
marker_corners_right, marker_ids_right, _ = cv2.aruco.detectMarkers(
grayR, charucoDictionary, parameters=detectorParams)
if (len(marker_corners_left) >
0) and (len(marker_corners_right) >
0) and (frame_idx % frame_spacing == 0):
ret_left, charuco_corners_left, charuco_ids_left = cv2.aruco.interpolateCornersCharuco(
marker_corners_left, marker_ids_left, grayL,
charucoBoard)
ret_right, charuco_corners_right, charuco_ids_right = cv2.aruco.interpolateCornersCharuco(
marker_corners_right, marker_ids_right, grayR,
charucoBoard)
if (charuco_corners_left is not None) and (
charuco_corners_right is not None) and (
charuco_ids_left is not None) and (
charuco_ids_right is not None) and (
len(charuco_corners_left) > 3) and (
len(charuco_corners_right) > 3):
all_corners_left.append(charuco_corners_left)
all_corners_right.append(charuco_corners_right)
all_ids_left.append(charuco_ids_left)
all_ids_right.append(charuco_ids_right)
pbar.set_description(
'Found {} ChArUco frames out of {} required frames'
.format(len(all_ids_right), required_count))
pbar.update(1)
cv2.aruco.drawDetectedMarkers(frameL, marker_corners_left,
marker_ids_left)
cv2.aruco.drawDetectedMarkers(frameR, marker_corners_right,
marker_ids_right)
fullFrame = np.concatenate((frameL, frameR), axis=1)
cv2.imshow('stereo view',
cv2.resize(fullFrame, (0, 0), fx=ft, fy=ft))
key = cv2.waitKey(1)
if key == 27:
break
frame_idx += 1
# print("Found: " + str(len(all_ids_right)) + " / " + str(required_count))
if len(all_ids_right) >= required_count:
success = True
cv2.destroyAllWindows()
break
print('End cycle')
if success:
print(
'Finished collecting data, \nStarting calibration... It might take few minutes.. !'
)
leftImgSize = grayL.shape
# print(leftImgSize)
rightImgSize = grayR.shape
# print(rightImgSize)
try:
err_left, camera_matrix_left, dist_coeffs_left, rvecs_left, tvecs_left = charuco_calibration(
all_corners_left, all_ids_left, leftImgSize, charucoBoard)
err_right, camera_matrix_right, dist_coeffs_right, rvecs_right, tvecs_right = charuco_calibration(
all_corners_right, all_ids_right, rightImgSize, charucoBoard)
print('Cam Left calibrated with error: ', err_left)
print('Cam Right calibrated with error: ', err_right)
print('...DONE')
except Exception as e:
print(e)
success = False
if success:
# Generate the corrections
new_camera_matrix_left, valid_pix_roi_left = cv2.getOptimalNewCameraMatrix(
camera_matrix_left, dist_coeffs_left, resolution, 0)
new_camera_matrix_right, valid_pix_roi_right = cv2.getOptimalNewCameraMatrix(
camera_matrix_right, dist_coeffs_right, resolution, 0)
mapx_left, mapy_left = cv2.initUndistortRectifyMap(
camera_matrix_left, dist_coeffs_left, None,
new_camera_matrix_left, resolution, 5)
mapx_right, mapy_right = cv2.initUndistortRectifyMap(
camera_matrix_right, dist_coeffs_right, None,
new_camera_matrix_right, resolution, 5)
save_json(
'Left', {
'camera_matrix': camera_matrix_left.tolist(),
'dist_coeffs': dist_coeffs_left.tolist(),
'err': err_left,
'mapx': mapx_left.tolist(),
'mapy': mapy_left.tolist()
})
save_json(
'Right', {
'camera_matrix': camera_matrix_right.tolist(),
'dist_coeffs': dist_coeffs_right.tolist(),
'err': err_right,
'mapx': mapx_right.tolist(),
'mapy': mapy_right.tolist()
})
while True:
retL, frameLeft = capL.read()
retR, frameRight = capR.read()
ret = retL and retR
if ret:
if (mapx_left is not None) and (mapy_left is not None):
rect_frameLeft = cv2.remap(frameLeft, mapx_left,
mapy_left, cv2.INTER_LINEAR)
if (mapx_right is not None) and (mapy_right is not None):
rect_frameRight = cv2.remap(frameRight, mapx_right,
mapy_right,
cv2.INTER_LINEAR)
rect_fullFrame = np.concatenate(
(rect_frameLeft, rect_frameRight), axis=1)
fullFrame = np.concatenate((frameLeft, frameRight), axis=1)
cv2.imshow('stereo view',
cv2.resize(fullFrame, (0, 0), fx=ft, fy=ft))
cv2.imshow(
'RECTIFIED stereo view',
cv2.resize(rect_fullFrame, (0, 0), fx=ft, fy=ft))
key = cv2.waitKey(1)
if key == 27:
cv2.destroyAllWindows()
break
else:
print("Camera reading error!")
break
# key = cv2.waitKey(1)
# if key == 27:
# break
capL.release()
capR.release()
cv2.destroyAllWindows()
def _get_stereo_rectify_params(self):
""" if using stereo, we need to findout the stereo parameters,
based on the extrinsic parameters for the two cameras
"""
camera_names = self._data_config['intrinsic'].keys()
camera5_mat = uts.intrinsic_vec_to_mat(
self._data_config['intrinsic']['Camera_5'],
self._data_config['image_size'])
camera6_mat = uts.intrinsic_vec_to_mat(
self._data_config['intrinsic']['Camera_6'],
self._data_config['image_size'])
distCoeff = np.zeros(4)
image_size = (self._data_config['image_size'][1],
self._data_config['image_size'][0])
# compare the two image
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(
cameraMatrix1=camera5_mat,
distCoeffs1=distCoeff,
cameraMatrix2=camera6_mat,
distCoeffs2=distCoeff,
imageSize=image_size,
R=self._data_config['extrinsic']['R'],
T=self._data_config['extrinsic']['T'],
flags=cv2.CALIB_ZERO_DISPARITY,
alpha=1)
# for warping image 5
map1x, map1y = cv2.initUndistortRectifyMap(cameraMatrix=camera5_mat,
distCoeffs=distCoeff,
R=R1,
newCameraMatrix=P1,
size=image_size,
m1type=cv2.CV_32FC1)
# for warping image 6
map2x, map2y = cv2.initUndistortRectifyMap(cameraMatrix=camera6_mat,
distCoeffs=distCoeff,
R=R2,
newCameraMatrix=P2,
size=image_size,
m1type=cv2.CV_32FC1)
res = {
'Camera_5_rot': R1,
'Camera_5_intr': P1,
'Camera_5_mapx': map1x,
'Camera_5_mapy': map1y,
'Camera_6_rot': R2,
'Camera_6_intr': P2,
'Camera_6_mapx': map2x,
'Camera_6_mapy': map2y
}
# get new intrinsic and rotation parameters after rectification
for name in camera_names:
res[name + '_intr'] = uts.intrinsic_mat_to_vec(res[name + '_intr'])
res[name + '_intr'][[0, 2]] /= self._data_config['image_size'][1]
res[name + '_intr'][[1, 3]] /= self._data_config['image_size'][0]
rect_extr_mat = np.eye(4)
rect_extr_mat[:3, :3] = res[name + '_rot']
res[name + '_ext'] = rect_extr_mat
self._data_config.update(res)
# 设置标志位为cv2.CALIB_FIX_INTRINSIC，这样就会固定输入的cameraMatrix和distCoeffs不变，只求解𝑅,𝑇,𝐸,𝐹
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(
objpoints, imgpointsL, imgpointsR, OmtxL, distL, OmtxR, distR,
ChessImaR.shape[::-1], criteria_stereo, flags)
# 利用stereoRectify()计算立体校正的映射矩阵
rectify_scale = 1 # 设置为0的话，对图片进行剪裁，设置为1则保留所有原图像像素
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(MLS, dLS, MRS, dRS,
ChessImaR.shape[::-1], R, T,
rectify_scale, (0, 0))
# 利用initUndistortRectifyMap函数计算畸变矫正和立体校正的映射变换，实现极线对齐。
Left_Stereo_Map = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL,
ChessImaR.shape[::-1],
cv2.CV_16SC2)
Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR,
ChessImaR.shape[::-1],
cv2.CV_16SC2)
# 立体校正效果显示
for i in range(5, 6): # 以第一对图片为例
t = str(i)
print(i)
frameR = cv2.imread('E:/Image/Right/R_' + t + '.jpg', 0)
frameL = cv2.imread('E:/Image/Left/L_' + t + '.jpg', 0)
Left_rectified = cv2.remap(frameL, Left_Stereo_Map[0], Left_Stereo_Map[1],
cv2.INTER_LANCZOS4, cv2.BORDER_CONSTANT,
with open("../data/extrinsics.yml", 'w') as extrStream:
yaml.dump_all(["R1", RL, "R2", RR, "P1", PL, "P2", PR, "Q", Q], extrStream)
print("extrinsics written")
print("R1\n", RL, "\nR2\n", RR, "\nP1\n", PL, "\nP2\n", PR, "\nQ\n", Q,
"\nroiL\n", roiL, "\nroiR\n", roiR)
# TODO: Move to depth-mapping module? Or own debug function
print("Undistorting images... ", end='')
# Invert image size to width*height
distImgSize = (stereo_imgsize[1], stereo_imgsize[0])
mapLx, mapLy = cv2.initUndistortRectifyMap(cameraMatrixL, distCoeffsL, RL,
PL, distImgSize, cv2.CV_16SC2, None,
None)
mapRx, mapRy = cv2.initUndistortRectifyMap(cameraMatrixR, distCoeffsR, RR,
PR, distImgSize, cv2.CV_16SC2, None,
None)
# Show remapped versions of calibration images
keyPressFlag = False
cv2.namedWindow("Remapped left")
cv2.namedWindow("Remapped right")
for imL, imR in zip(stereo_imgnames_l, stereo_imgnames_r):
imgL = cv2.imread(imL)
remappedImgL = cv2.remap(src=imgL, map1=mapLx, map2=mapLy,
interpolation=cv2.INTER_LINEAR)
cv2.imshow("Remapped left", remappedImgL)
def undistort_method2(self, img, cameramatrix):
# undistort using remaping
mapx, mapy = cv2.initUndistortRectifyMap(self.K, self.D, None,
cameramatrix, (w, h), 5)
dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
return dst
print('\n')
print('Reprojection Error=\n', ret)
# stereo rectification
R1, R2, P1, P2, Q, ROI1, ROI2 = cv2.stereoRectify(mtx_l,
dist_l,
mtx_r,
dist_r,
gray.shape[::-1],
R,
T,
flags=0,
alpha=-1)
# undistort rectifying mapping
map1_l, map2_l = cv2.initUndistortRectifyMap(mtx_l, dist_l, R1, P1,
gray.shape[::-1], cv2.CV_16SC2)
map1_r, map2_r = cv2.initUndistortRectifyMap(mtx_r, dist_r, R2, P2,
gray.shape[::-1], cv2.CV_16SC2)
# undistort the original image, take img#3 as an example
left3 = cv2.imread('../left/left03.jpg')
dst_l = cv2.remap(left3, map1_l, map2_l, cv2.INTER_LINEAR)
cv2.imwrite('rectifyresult/left03(rectified).jpg', dst_l)
if cv2.imwrite('rectifyresult/left03(rectified).jpg', dst_l) == True:
print('rectification of left camera has been done successfully.\n')
right3 = cv2.imread('../right/right03.jpg')
dst_r = cv2.remap(right3, map1_r, map2_r, cv2.INTER_LINEAR)
cv2.imwrite('rectifyresult/right03(rectified).jpg', dst_r)
if cv2.imwrite('rectifyresult/right03(rectified).jpg', dst_r) == True:
print('rectification of right camera has been done successfully.\n')
def __init__(self, K=K, D=D, alpha=0.0, height=FRAME_HEIGHT, width=FRAME_WIDTH):
dim = (width, height)
self.newK = cv2.getOptimalNewCameraMatrix(K, D, dim, alpha, dim)[0]
self.K = K
self.D = D
self.umap1, self.umap2 = cv2.initUndistortRectifyMap(K, D, None, self.newK, dim, cv2.CV_16SC2)
Q = np.zeros((4, 4))
Q[0] = [1., 0., 0., -1.0855335845947266e+03]
Q[1] = [0., 1., 0., -5.5919826126098633e+02]
Q[2] = [0., 0., 0., 8.9016364173284990e+02]
Q[3] = [0., 0., 4.6101647549769706e-03, 0.]
# 73==55==1809==980
# 54==41==1805==979
# roi1=73:1809,55:980
# roi2=54:1805,41:979
# roi1 125 63 1773 963
# roi2 17 50 1777 968
######## Precompute maps for cv::remap()
left_maps = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1,
(1920, 1080), cv2.CV_16SC2)
right_maps = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2,
(1920, 1080), cv2.CV_16SC2)
right_imagepath = '/home/westwell/Downloads/cali_1097/1534311097/right/'
left_imagepath = '/home/westwell/Downloads/cali_1097/1534311097/left/'
count = 10000
debug = True
for left_image in sorted(os.listdir(left_imagepath)):
image_order = left_image.split('_')[0]
right_image = image_order + '_R.png'
left_img = cv2.imread(left_imagepath + left_image, cv2.CV_8UC1)
right_img = cv2.imread(right_imagepath + right_image, cv2.CV_8UC1)
print right_imagepath + right_image, left_imagepath + left_image
left_img = cv2.resize(left_img, (0, 0), fx=0.75, fy=0.75)
right_img = cv2.resize(right_img, (0, 0), fx=0.75, fy=0.75)
print left_img.shape
def main_worker(id, video_file, camera_model, K, D, R, T, measurements, quit_event):
# Setup video displays
video_disp = Display({'name': 'Camera_{}'.format(id)})
# Get input video
video = Video(video_file)
# Setup the undistortion stuff
if camera_model == 'P':
# Harcoded image size as
# this is a test script
img_size = (1920, 1080)
# First create scaled intrinsics because we will undistort
# into region beyond original image region
new_K = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0.35)[0]
# Then calculate new image size according to the scaling
# Unfortunately the Python API doesn't directly provide the
# the new image size. They forgot?
new_img_size = (int(img_size[0] + (new_K[0, 2] - K[0, 2])), int(
img_size[1] + (new_K[1, 2] - K[1, 2])))
# Standard routine of creating a new rectification
# map for the given intrinsics and mapping each
# pixel onto the new map with linear interpolation
map1, map2 = cv2.initUndistortRectifyMap(
K, D, None, new_K, new_img_size, cv2.CV_16SC2)
elif camera_model == 'F':
# Harcoded image size
img_size = (1920, 1080)
# First create scaled intrinsics because we will undistort
# into region beyond original image region. The alpha
# parameter in pinhole model is equivalent to balance parameter here.
new_K = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(
K, D, img_size, np.eye(3), balance=1)
# Then calculate new image size according to the scaling
# Well if they forgot this in pinhole Python API,
# can't complain about Fisheye model. Note the reversed
# indexing here too.
new_img_size = (int(img_size[0] + (new_K[0, 2] - K[0, 2])), int(
img_size[1] + (new_K[1, 2] - K[1, 2])))
# Standard routine of creating a new rectification
# map for the given intrinsics and mapping each
# pixel onto the new map with linear interpolation
map1, map2 = cv2.fisheye.initUndistortRectifyMap(
K, D, np.eye(3), new_K, new_img_size_1, cv2.CV_16SC2)
# Set up foreground and background separation
fgbg = cv2.createBackgroundSubtractorMOG2()
# Averaging kernel that will be used in opening
kernel = np.ones((6, 6), np.uint8)
# Code commented out because not using
# confidence currently, but could be
# used again with changes later
# # Will be used for histogram comparison
# # (Confidence measure)
# ball_image_file = 'ball_image.jpg'
# ball_image = cv2.imread(ball_image_file)
# 2D ball detection and 3D ball tracking setup
ball_position_frame = None
ball_wc = [0, 0, 0]
while not video.end_reached() and not quit_event.value:
# Get each frame
frame = video.next_frame()
# Undistort the current frame
img_undistorted = cv2.remap(
frame, map1, map2, cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)
# Convert to HSV and threshold range of ball
img_hsv = cv2.cvtColor(img_undistorted, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(img_hsv, np.array(
(15, 190, 200)), np.array((25, 255, 255)))
# Foreground and background separation mask
fgmask = fgbg.apply(img_undistorted)
mask_color_bgs = cv2.bitwise_and(mask, mask, mask=fgmask)
masked_and_opened = cv2.morphologyEx(
mask_color_bgs, cv2.MORPH_OPEN, kernel)
# Hough transform to detect ball (circle)
circles = cv2.HoughCircles(masked_and_opened, cv2.HOUGH_GRADIENT, dp=3,
minDist=2500, param1=300, param2=5, minRadius=3, maxRadius=30)
if circles is not None:
# Make indexing easier and
# convert everything to int
circles = circles[0, :]
circles = np.round(circles).astype("int")
# Take only the first
# (and hopefully largest)
# circle detected
x, y, r = circles[0]
ball_position_frame = [x - r, y - r, 2 * r, 2 * r]
else:
ball_position_frame = None
# Determine the correct ball radius
mask_ball_radius = cv2.bitwise_and(fgmask, fgmask, mask=cv2.inRange(
img_hsv, np.array((10, 150, 180)), np.array((40, 255, 255))))
if ball_position_frame:
x1, y1, w1, h1 = ball_position_frame
ball_crop_temp = mask_ball_radius[(
y1 + h1 // 2 - 50):(y1 + h1 // 2 + 50), (x1 + w1 // 2 - 50):(x1 + w1 // 2 + 50)]
height, width = ball_crop_temp.shape
if height and width:
# Successfully cropped image
ball_crop = ball_crop_temp
cnts = cv2.findContours(
ball_crop.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
if len(cnts) > 0:
# contour detected
c = max(cnts, key=cv2.contourArea)
ellipse = cv2.fitEllipse(c)
width = min(ellipse[1])
ball_position_frame = [
ball_position_frame[0], ball_position_frame[1], 2 * width, 2 * width]
# Code commented out because not using
# confidence currently, but could be
# used again with changes later
# # Calculate confidence
# confidence = histogram_comparison(ball_image, img_undistorted, ball_position_frame)
# print confidence
if ball_position_frame:
x1, y1, w1, h1 = ball_position_frame
pixels_per_mm = (
K[0, 0] + K[1, 1]) / 2 / DEFAULT_FOCAL_LENGTH
z = PING_PONG_DIAMETER * \
DEFAULT_FOCAL_LENGTH / (w1 / pixels_per_mm)
x = ((x1 - K[0, 2]) /
pixels_per_mm) * z / DEFAULT_FOCAL_LENGTH
y = ((y1 - K[1, 2]) /
pixels_per_mm) * z / DEFAULT_FOCAL_LENGTH
ball_cc = np.array([x, y, z]) / 1000
ball_wc = np.dot(R.T, ball_cc - T.ravel())
# Push measurements to be processed/visualized
measurement = {
'id': id,
'frame_num': video.get_cur_frame_num(),
'ball_ic': ball_position_frame,
'ball_wc': ball_wc
}
measurements.put(measurement)
# Update video display
video_disp.refresh(img_undistorted)
# Add quitting event
if video_disp.can_quit():
break
# Setting this will signal
# the other parallel process
# to exit too.
quit_event.value = 1
cv2.CALIB_SAME_FOCAL_LENGTH | cv2.CALIB_RATIONAL_MODEL | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5,
criteria=term)
# 5. 矫正一张图像看看，是否完成了极线矫正
start_time = time.time()
fname1 = './images/left/left01.jpg'
fname2 = './images/right/right01.jpg'
img1 = cv2.imread(fname1)
img2 = cv2.imread(fname2)
gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = \
cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (w, h), R, T)
map1_1, map1_2 = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, (w, h), cv2.CV_32FC1) #cv2.CV_16SC2
map2_1, map2_2 = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2, (w, h), cv2.CV_32FC1)
result1 = cv2.remap(img1, map1_1, map1_2, cv2.INTER_LINEAR)
result2 = cv2.remap(img2, map2_1, map2_2, cv2.INTER_LINEAR)
print("变形处理时间%f(s)" % (time.time() - start_time))
result = np.concatenate((result1, result2), axis=1)
result[::20, :] = 0
cv2.namedWindow("rectification",0)
cv2.imshow("rectification",result)
# cv2.imwrite("E:/pics/rec13.png", result)
# 8. 计算视差图并显示
#视差计算
def SGBM(imgL, imgR):
def getCalibration(self, camera_index):
#---------------------- SET THE PARAMETERS
frame_width = 1920
frame_height = 1080
nRows = 9
nCols = 6
dimension = 25 #- mm
workingFolder = "/Users/thekey/Desktop/PuzzleSolver/RoboPuzzler/ObjectDetection/CameraCalibration/calibrationimages"
imageType = 'jpg'
safefolder = 'CameraCalibration'
#------------------------------------------
self.makeCalibrationimages(frame_width, frame_height, camera_index)
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
dimension, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((nRows * nCols, 3), np.float32)
objp[:, :2] = np.mgrid[0:nCols, 0:nRows].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
# Find the images files
filename = workingFolder + "/*." + imageType
images = glob.glob(filename)
print(len(images))
if len(images) < 9:
print(
"Not enough images were found: at least 9 shall be provided!!!"
)
sys.exit()
else:
nPatternFound = 0
imgNotGood = images[1]
for fname in images:
if 'calibresult' in fname: continue
#-- Read the file and convert in greyscale
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
print("Reading image ", fname)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(
gray, (nCols, nRows), None)
# If found, add object points, image points (after refining them)
if ret == True:
print(
"Pattern found! Press ESC to skip or ENTER to accept")
#--- Sometimes, Harris cornes fails with crappy pictures, so
corners2 = cv2.cornerSubPix(gray, corners, (11, 11),
(-1, -1), criteria)
# Draw and display the corners
cv2.drawChessboardCorners(img, (nCols, nRows), corners2,
ret)
cv2.imshow('img', img)
# cv2.waitKey(0)
k = cv2.waitKey(0) & 0xFF
if k == 27: #-- ESC Button
print("Image Skipped")
imgNotGood = fname
continue
print("Image accepted")
nPatternFound += 1
objpoints.append(objp)
imgpoints.append(corners2)
# cv2.waitKey(0)
else:
imgNotGood = fname
cv2.destroyAllWindows()
mtx = []
dist = []
if (nPatternFound > 1):
print("Found %d good images" % (nPatternFound))
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, gray.shape[::-1], None, None)
# Undistort an image
img = cv2.imread(imgNotGood)
h, w = img.shape[:2]
print("Image to undistort: ", imgNotGood)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
mtx, dist, (w, h), 1, (w, h))
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None,
newcameramtx, (w, h), 5)
dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
print("ROI: ", x, y, w, h)
cv2.imwrite(safefolder + "/calibresult.png", dst)
print("Calibrated picture saved as calibresult.png")
print("Calibration Matrix Created ")
print("Disortion: ", dist)
#--------- Save result
filename = "cameraMatrix.txt"
np.savetxt(filename, mtx, delimiter=',')
filename = "cameraDistortion.txt"
np.savetxt(filename, dist, delimiter=',')
mean_error = 0
for i in range(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i],
tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2,
cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print("total error: ", mean_error / len(objpoints))
else:
print(
"In order to calibrate you need at least 9 good pictures... try again"
)
return mtx, dist
line_y = [i for i in range(0, 480, 480 // line_num)]
line_x1 = [640 * 2 - 1 for i in range(480 // line_num)]
start = tuple(zip(line_x0, line_y))
end = tuple(zip(line_x1, line_y))
for i in range(len(left_content)):
leftFrame = cv2.imread(left_content[i])
leftFrame = cv2.resize(leftFrame, (640, 480))
rightFrame = cv2.imread(right_content[i])
rightFrame = cv2.resize(rightFrame, (640, 480))
height, width, channel = leftFrame.shape # We will use the shape for remap
# Undistortion and Rectification part!
leftMapX, leftMapY = cv2.initUndistortRectifyMap(
K1, D1, R1, P1, (width, height), cv2.CV_32FC1)
left_rectified = cv2.remap(leftFrame, leftMapX, leftMapY,
cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)
rightMapX, rightMapY = cv2.initUndistortRectifyMap(
K2, D2, R2, P2, (width, height), cv2.CV_32FC1)
right_rectified = cv2.remap(rightFrame, rightMapX, rightMapY,
cv2.INTER_LINEAR, cv2.BORDER_CONSTANT)
merge = np.hstack((left_rectified, right_rectified))
for index in range(len(start)):
s = start[index]
e = end[index]
show_remap = cv2.line(merge, s, e, (255, 0, 0), thickness=1)
cv2.imshow('remap', show_remap)
delimiter=',')
np.savetxt(os.path.join(path, 'results', 'cam_mtx.txt'),
results['intrinsic_matrix'],
fmt='%.8f',
delimiter=',')
np.savetxt(os.path.join(path, 'results', 'cam_dist.txt'),
results['distortion_coefficients'],
fmt='%.8f',
delimiter=',')
h, w = cv2.imread(images_path_list[0], 0).shape[:2]
newcammtx, roi = cv2.getOptimalNewCameraMatrix(
results['intrinsic_matrix'], results['distortion_coefficients'], (w, h),
alpha, (w, h))
mapx, mapy = cv2.initUndistortRectifyMap(results['intrinsic_matrix'],
results['distortion_coefficients'],
None, newcammtx, (w, h), 5)
print("New Camera Matrix: \n")
print(newcammtx)
np.savetxt(os.path.join(path, 'results', 'cam_mtx_new.txt'),
newcammtx,
fmt='%.8f',
delimiter=',')
np.savetxt(os.path.join(path, 'results', 'roi.txt'),
roi,
fmt='%i',
delimiter=',')
for image in images_path_list:
img = cv2.imread(image)
def Calibrate(): #claibration of the camera from previously taken images
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
cbrow = 6
cbcol = 8
lobjp = np.zeros((cbrow * cbcol, 3), np.float32)
robjp = np.zeros((cbrow * cbcol, 3), np.float32)
lobjp[:, :2] = np.mgrid[0:cbcol, 0:cbrow].T.reshape(-1, 2)
robjp[:, :2] = np.mgrid[0:cbcol, 0:cbrow].T.reshape(-1, 2)
lnewcameramtx = []
rnewcameramtx = []
lret = []
lmtx = []
ldist = []
lroi = []
lrot = []
ltrn = []
rret = []
rmtx = []
rdist = []
rroi = []
rrot = []
rtrn = []
imagesizee = []
# Arrays to store object points and image points from all the images.
lobjpoints = [] # 3d point in real world space
robjpoints = [] # 3d point in real world space
limgpoints = [] # 2d points in image plane.
rimgpoints = [] # 2d points in image plane.
left = cv2.VideoCapture(1)
leftimages = ["D:\\cal\Cal\Leftside.jpg", "D:\\cal\Cal\Leftside1.jpg"]
rightimages = ["D:\\cal\Cal\Rightside.jpg", "D:\\cal\Cal\Rightside1.jpg"]
for fname in range(0, 2, 1):
leftimg = cv2.imread(leftimages[fname])
rightimg = cv2.imread(rightimages[fname])
print("files: " + leftimages[fname] + " and " + rightimages[fname])
leftimg = cv2.cvtColor(leftimg, cv2.COLOR_RGB2GRAY)
rightimg = cv2.cvtColor(rightimg, cv2.COLOR_RGB2GRAY)
imagesizee = leftimg.shape[:2]
# Find the chess board corners
r, lcorners = cv2.findChessboardCorners(
leftimg, (cbcol, cbrow), None) #detecting chessboard corners
r2, rcorners = cv2.findChessboardCorners(rightimg, (cbcol, cbrow),
None)
if r2 == True:
print("right chackboard found !")
robjpoints.append(robjp)
cv2.cornerSubPix(rightimg, rcorners, (11, 11), (-1, -1), criteria)
rimgpoints.append(rcorners)
rret, rmtx, rdist, rrot, rtrn = cv2.calibrateCamera(
robjpoints, rimgpoints, rightimg.shape[::-1], None,
None) #calibrating camera individually
h, w = rightimg.shape[:2]
rnewcameramtx, rroi = cv2.getOptimalNewCameraMatrix(
rmtx, rdist, (w, h), 1, (w, h)) #getting refined camera matrix
# If found, add object points, image points (after refining them)
if r == True:
print("left chackboard found !")
lobjpoints.append(lobjp)
cv2.cornerSubPix(leftimg, lcorners, (11, 11), (-1, -1), criteria)
limgpoints.append(lcorners)
lret, lmtx, ldist, lrot, ltrn = cv2.calibrateCamera(
lobjpoints, limgpoints, rightimg.shape[::-1], None, None)
h, w = leftimg.shape[:2]
lnewcameramtx, lroi = cv2.getOptimalNewCameraMatrix(
lmtx, ldist, (w, h), 1, (w, h))
(
_, _, _, _, _, rotationMatrix, translationVector, _, _
) = cv2.stereoCalibrate(
lobjpoints, limgpoints, rimgpoints, lmtx, ldist, rmtx, rdist,
tuple(imagesizee),
None, None, None, None, cv2.CALIB_FIX_INTRINSIC
) #calibrating stereovision of the two views together (supposed to be used for stereocamera)
(leftRectification, rightRectification, leftProjection, rightProjection,
dispartityToDepthMap, leftROI, rightROI) = cv2.stereoRectify(
lmtx, ldist, rmtx, rdist, tuple(imagesizee), rotationMatrix,
translationVector, None, None, None, None, None,
cv2.CALIB_ZERO_DISPARITY,
0.25) #refining the calibration and getting desparity to depth map
leftMapX, leftMapY = cv2.initUndistortRectifyMap(lmtx, ldist,
leftRectification,
leftProjection,
imagesizee, cv2.CV_32FC1)
rightMapX, rightMapY = cv2.initUndistortRectifyMap(rmtx, rdist,
rightRectification,
rightProjection,
imagesizee,
cv2.CV_32FC1)
print("Finished Calibrating, saving")
np.savez("D://cal/calibration.npz",
lmtx=lmtx,
rmtx=rmtx,
ldist=ldist,
rdist=rdist,
lnewcameramtx=lnewcameramtx,
rnewcameramtx=rnewcameramtx,
leftROI=leftROI,
rightROI=rightROI,
dispartityToDepthMap=dispartityToDepthMap)
return lmtx, rmtx, ldist, rdist, lnewcameramtx, rnewcameramtx, leftROI, rightROI, dispartityToDepthMap
def get_mapping(camera_matrix, distortion, rectification, projection, shape):
map_x, map_y = cv2.initUndistortRectifyMap(camera_matrix, distortion,
rectification, projection,
shape, cv2.CV_32FC1)
return map_x, map_y
def calibration(**kwargs):
for k, v in kwargs.items():
setattr(opt, k, v)
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
objpoints_r = []
imgpoints_r = []
images = glob.glob('../left/*.jpg')
images_r = glob.glob('../right/*.jpg')
images.sort()
images_r.sort()
for fname, fname_r in zip(images, images_r):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img_r = cv2.imread(fname_r)
gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)
ret_r, corners_r = cv2.findChessboardCorners(gray_r, (7, 6), None)
# If found, add object points, image points (after refining them)
if ret == True and ret_r == True:
objpoints.append(objp)
objpoints_r.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
corners2_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11),
(-1, -1), criteria)
imgpoints.append(corners2)
imgpoints_r.append(corners2_r)
# Draw and display the corners
if opt.disp_calib:
cv2.imshow('img', img)
cv2.waitKey(500)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
img = cv2.imread('../left/left' + str(opt.sample) + '.jpg')
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1,
(w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
if opt.disp_calib:
cv2.imwrite('../calibresult/left' + str(opt.sample) + '.png', dst)
ret, mtx_r, dist_r, rvecs, tvecs = cv2.calibrateCamera(
objpoints_r, imgpoints_r, gray_r.shape[::-1], None, None)
img_r = cv2.imread('../right/right' + str(opt.sample) + '.jpg')
h, w = img_r.shape[:2]
newcameramtx_r, roi = cv2.getOptimalNewCameraMatrix(
mtx_r, dist_r, (w, h), 1, (w, h))
# undistort
dst_r = cv2.undistort(img_r, mtx_r, dist_r, None, newcameramtx_r)
# crop the image
x, y, w, h = roi
dst_r = dst_r[y:y + h, x:x + w]
if opt.disp_calib:
cv2.imwrite('../calibresult/right' + str(opt.sample) + '.png', dst)
if not opt.stereo_calib:
exit(0)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objpoints, imgpoints, imgpoints_r, mtx,
dist, mtx_r, dist_r, gray.shape[::-1])
if opt.matlab:
try:
R = opt.R[opt.sample]
T = opt.T[opt.sample]
except:
print('Please modify config to add R and T for ' + opt.sample)
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
gray.shape[::-1], R, T)
# 矫正映射
left_map1, left_map2 = cv2.initUndistortRectifyMap(cameraMatrix1,
distCoeffs1, R1, P1,
gray.shape[::-1],
cv2.INTER_NEAREST)
right_map1, right_map2 = cv2.initUndistortRectifyMap(
cameraMatrix2, distCoeffs2, R2, P2, gray.shape[::-1],
cv2.INTER_NEAREST)
img = cv2.imread('../left/left' + str(opt.sample) + '.jpg')
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img = cv2.imread(('../right/right' + str(opt.sample) + '.jpg'))
gray_r = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
#矫正原始图像 矫正后基本处于行对准状态
imgL = cv2.remap(gray, left_map1, left_map2, cv2.INTER_LINEAR)
imgR = cv2.remap(gray_r, right_map1, right_map2, cv2.INTER_LINEAR)
if opt.disp_stereo_calib:
cv2.imwrite(
'../result/stereo_calibresult/left' + str(opt.sample) + '.png',
imgL)
cv2.imwrite(
'../result/stereo_calibresult/right' + str(opt.sample) + '.png',
imgR)
plt.subplot(121)
plt.title('left')
plt.imshow(imgL, cmap='gray')
plt.axis('off')
plt.subplot(122)
plt.title('right')
plt.imshow(imgR, cmap='gray')
plt.axis('off')
plt.show()
if not opt.disparity:
exit(0)
cv2.namedWindow("depth")
cv2.namedWindow("disparity")
cv2.moveWindow("depth", 0, 0)
cv2.moveWindow("disparity", 600, 0)
def callbackFunc(e, x, y, f, p):
if e == cv2.EVENT_LBUTTONDOWN:
print(threeD[y][x])
cv2.setMouseCallback("depth", callbackFunc, None)
# BM算法得到视差图
stereo = cv2.StereoSGBM_create(numDisparities=16 * opt.num,
blockSize=opt.blockSize)
disparity = stereo.compute(imgL, imgR)
disp = cv2.normalize(disparity,
disparity,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
# 将图片扩展至3d空间中，其z方向的值则为当前的距离 视差图转为3D坐标
threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32) / 16., Q)
#FourD = cv2.triangulatePoints()
#print(threeD.shape)
cv2.imshow("disparity", disp)
cv2.imshow("depth", imgL)
key = cv2.waitKey(0)
if key == ord("q"):
exit(0)
elif key == ord("s"):
cv2.imwrite("../result/disparity/disparity" + opt.sample + ".png",
disp)
def init_calibration(calibration_file, image_size):
cameraMarix_left = cameraMatrix_right = map_left_y = map_left_x = map_right_y = map_right_x = np.array(
[])
config = configparser.ConfigParser()
config.read(calibration_file)
check_data = True
resolution_str = ''
if image_size.width == 2208:
resolution_str = '2K'
elif image_size.width == 1920:
resolution_str = 'FHD'
elif image_size.width == 1280:
resolution_str = 'HD'
elif image_size.width == 672:
resolution_str = 'VGA'
else:
resolution_str = 'HD'
check_data = False
T_ = np.array([
-float(config['STEREO']['Baseline'] if 'Baseline' in
config['STEREO'] else 0),
float(config['STEREO']['TY_' + resolution_str] if 'TY_' +
resolution_str in config['STEREO'] else 0),
float(config['STEREO']['TZ_' + resolution_str] if 'TZ_' +
resolution_str in config['STEREO'] else 0)
])
left_cam_cx = float(config['LEFT_CAM_' + resolution_str]['cx'] if 'cx' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_cy = float(config['LEFT_CAM_' + resolution_str]['cy'] if 'cy' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_fx = float(config['LEFT_CAM_' + resolution_str]['fx'] if 'fx' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_fy = float(config['LEFT_CAM_' + resolution_str]['fy'] if 'fy' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_k1 = float(config['LEFT_CAM_' + resolution_str]['k1'] if 'k1' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_k2 = float(config['LEFT_CAM_' + resolution_str]['k2'] if 'k2' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_p1 = float(config['LEFT_CAM_' + resolution_str]['p1'] if 'p1' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_p2 = float(config['LEFT_CAM_' + resolution_str]['p2'] if 'p2' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_p3 = float(config['LEFT_CAM_' + resolution_str]['p3'] if 'p3' in
config['LEFT_CAM_' + resolution_str] else 0)
left_cam_k3 = float(config['LEFT_CAM_' + resolution_str]['k3'] if 'k3' in
config['LEFT_CAM_' + resolution_str] else 0)
right_cam_cx = float(config['RIGHT_CAM_' + resolution_str]['cx'] if 'cx' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_cy = float(config['RIGHT_CAM_' + resolution_str]['cy'] if 'cy' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_fx = float(config['RIGHT_CAM_' + resolution_str]['fx'] if 'fx' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_fy = float(config['RIGHT_CAM_' + resolution_str]['fy'] if 'fy' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_k1 = float(config['RIGHT_CAM_' + resolution_str]['k1'] if 'k1' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_k2 = float(config['RIGHT_CAM_' + resolution_str]['k2'] if 'k2' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_p1 = float(config['RIGHT_CAM_' + resolution_str]['p1'] if 'p1' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_p2 = float(config['RIGHT_CAM_' + resolution_str]['p2'] if 'p2' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_p3 = float(config['RIGHT_CAM_' + resolution_str]['p3'] if 'p3' in
config['RIGHT_CAM_' + resolution_str] else 0)
right_cam_k3 = float(config['RIGHT_CAM_' + resolution_str]['k3'] if 'k3' in
config['RIGHT_CAM_' + resolution_str] else 0)
R_zed = np.array([
float(config['STEREO']['RX_' + resolution_str] if 'RX_' +
resolution_str in config['STEREO'] else 0),
float(config['STEREO']['CV_' + resolution_str] if 'CV_' +
resolution_str in config['STEREO'] else 0),
float(config['STEREO']['RZ_' + resolution_str] if 'RZ_' +
resolution_str in config['STEREO'] else 0)
])
R, _ = cv2.Rodrigues(R_zed)
cameraMatrix_left = np.array([[left_cam_fx, 0, left_cam_cx],
[0, left_cam_fy, left_cam_cy], [0, 0, 1]])
cameraMatrix_right = np.array([[right_cam_fx, 0, right_cam_cx],
[0, right_cam_fy, right_cam_cy], [0, 0, 1]])
distCoeffs_left = np.array([[left_cam_k1], [left_cam_k2], [left_cam_p1],
[left_cam_p2], [left_cam_k3]])
distCoeffs_right = np.array([[right_cam_k1], [right_cam_k2],
[right_cam_p1], [right_cam_p2],
[right_cam_k3]])
T = np.array([[T_[0]], [T_[1]], [T_[2]]])
R1 = R2 = P1 = P2 = np.array([])
R1, R2, P1, P2 = cv2.stereoRectify(cameraMatrix1=cameraMatrix_left,
cameraMatrix2=cameraMatrix_right,
distCoeffs1=distCoeffs_left,
distCoeffs2=distCoeffs_right,
R=R,
T=T,
flags=cv2.CALIB_ZERO_DISPARITY,
alpha=0,
imageSize=(image_size.width,
image_size.height),
newImageSize=(image_size.width,
image_size.height))[0:4]
map_left_x, map_left_y = cv2.initUndistortRectifyMap(
cameraMatrix_left, distCoeffs_left, R1, P1,
(image_size.width, image_size.height), cv2.CV_32FC1)
map_right_x, map_right_y = cv2.initUndistortRectifyMap(
cameraMatrix_right, distCoeffs_right, R2, P2,
(image_size.width, image_size.height), cv2.CV_32FC1)
cameraMatrix_left = P1
cameraMatrix_right = P2
return cameraMatrix_left, cameraMatrix_right, map_left_x, map_left_y, map_right_x, map_right_y
(wr, hr), 1, (wr, hr))
udImgR = cv.undistort(imgR, Rc['M2'], Rc['dist2'], None,
newCameraMtxR) #undistorted Right Image
newCameraMtxL, roiL = cv.getOptimalNewCameraMatrix(Rc['M1'], Rc['dist1'],
(wl, hl), 1, (wl, hl))
udImgL = cv.undistort(imgL, Rc['M1'], Rc['dist1'], None, newCameraMtxL)
rectify_scale = 0 # 0=full crop, 1=no crop
R1, R2, P1, P2, Q, roi1, roi2 = cv.stereoRectify(Rc["M1"],
Rc["dist1"],
Rc["M2"],
Rc["dist2"], (640, 480),
Rc["R"],
Rc["T"],
alpha=rectify_scale)
left_maps = cv.initUndistortRectifyMap(Rc["M1"], Rc["dist1"], R1, P1,
(640, 480), cv.CV_16SC2)
right_maps = cv.initUndistortRectifyMap(Rc["M2"], Rc["dist2"], R2, P2,
(640, 480), cv.CV_16SC2)
udImgL = cv.remap(imgL, left_maps[0], left_maps[1], cv.INTER_LANCZOS4)
udImgR = cv.remap(imgR, right_maps[0], right_maps[1], cv.INTER_LANCZOS4)
udImgR = cv.cvtColor(udImgR, cv.COLOR_BGR2GRAY) # converting to grayScale
udImgL = cv.cvtColor(udImgL, cv.COLOR_BGR2GRAY)
good = []
pts1 = []
pts2 = []
sift = cv.xfeatures2d.SIFT_create()
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(udImgL, None)
kp2, des2 = sift.detectAndCompute(udImgR, None)
import cv2
import numpy as np
left_camera_matrix = np.array([[401.0750, 0., 0], [0., 396.4960, 0],
[322.1910, 178.9279, 1.]]).T
left_distortion = np.array([[-0.05783, 0.00739, 0.0000, 0.00000, 0.190000]])
right_camera_matrix = np.array([[401.1001, 0., 0], [0., 396.447, 0],
[312.6818, 183.3658, 1.]]).T
right_distortion = np.array([[-0.0216, 0.27, 0.0000, 0.00000, -0.19000]])
# om = np.array([0.0586, 0.8007, 0.13853])
#R = cv2.Rodrigues(om)[0]
R = np.array([[0.9057, 0.00196, -0.4246], [-0.0182, 0.9976, -0.0106],
[0.4244, -0.01709, 0.9052]])
T = np.array([-1.1088, -0.0716, -0.2885]).T
size = (640, 360)
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
left_camera_matrix, left_distortion, right_camera_matrix, right_distortion,
size, R, T)
left_map1, left_map2 = cv2.initUndistortRectifyMap(left_camera_matrix,
left_distortion, R1, P1,
size, cv2.CV_16SC2)
right_map1, right_map2 = cv2.initUndistortRectifyMap(right_camera_matrix,
right_distortion, R2, P2,
size, cv2.CV_16SC2)
def stereoRectificationProcess(rectify_scale, cameraMatrix1, distCoeffs1,
cameraMatrix2, distCoeffs2, R, T, E, F):
# call cv2.stereoRectify to get the rectification parameters
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify( \
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (640, 480), R, T, alpha=rectify_scale)
# prepare to remap the webcams
l_maps = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1,
(640, 480), cv2.CV_16SC2)
r_maps = cv2.initUndistortRectifyMap(cameraMatrix2, distCoeffs2, R2, P2,
(640, 480), cv2.CV_16SC2)
with open('lmaps.txt', 'wb') as f:
np.savetxt(f, l_maps[0])
np.savetxt(f, l_maps[1])
focal_length_x = cameraMatrix1[0][0]
#focal_length_y = cameraMatrix1[1][1]
"""
for l, r in zip(l_goodpair, r_goodpair):
#l_gray = cv2.cvtColor(l, cv2.COLOR_BGR2GRAY)
#r_gray = cv2.cvtColor(r, cv2.COLOR_BGR2GRAY)
l_imgremap = cv2.remap(l, l_maps[0], l_maps[1], cv2.INTER_LINEAR)
r_imgremap = cv2.remap(r, r_maps[0], r_maps[1], cv2.INTER_LINEAR)
cv2.imshow('left image_remap', l_imgremap)
cv2.imshow('right image_remap', r_imgremap)
cv2.waitKey(5000)
cv2.destroyAllWindows()
"""
# read in webcams' frames and prepare for disparity computing parameter tuning
lcap = cv2.VideoCapture(1)
rcap = cv2.VideoCapture(2)
cv2.namedWindow('disparity_parameters')
text = np.zeros((5, 500), dtype=np.uint8)
cv2.imshow('disparity_parameters', text)
cv2.createTrackbar('minDisparity', 'disparity_parameters', 16, 100,
minDispsCallBack)
cv2.createTrackbar('numDisparities', 'disparity_parameters', 1, 20,
numDispsCallBack) # divisible by 16
cv2.createTrackbar('blockSize', 'disparity_parameters', 7, 30,
bSizeCallBack) # odd number, 1 < 3 < blockSize < 11
cv2.createTrackbar('SADWindowSize', 'disparity_parameters', 3, 30,
wSizeCallBack)
#cv2.createTrackbar('P1', 'disparity_parameters', 1, 1, p1SizeCallBack)
#cv2.createTrackbar('P2', 'disparity_parameters', 1, 1, p2SizeCallBack)
cv2.createTrackbar('disp12MaxDiff', 'disparity_parameters', 1, 30,
disp12CallBack)
cv2.createTrackbar('uniquenessRatio', 'disparity_parameters', 1, 30,
uniqCallBack)
cv2.createTrackbar('speckleWindowSize', 'disparity_parameters', 100, 200,
spWCallBack) # 55 < speckleWindow < 200
cv2.createTrackbar('speckleRange', 'disparity_parameters', 1, 32,
spRCallBack) # 1 <= speckleRange <= 2
while (True):
lret, lframe = lcap.read()
rret, rframe = rcap.read()
# show disparity map and tune the paramters in real-time
tuneDisparity(lframe, rframe, l_maps, r_maps, focal_length_x)
key = cv2.waitKey(5) & 0xFF
if key == 27 or key == ord('q'):
print('bye')
break
lcap.release()
rcap.release()
cv2.destroyAllWindows()
#dst = cv2.undistort(img,mtx,dist,None,newcameramtx)
dst = cv2.undistort(img,mtx,dist,None,mtx)
out = np.concatenate([img, dst], axis=1)
cv2.imshow('out', out)
cv2.imshow('img',img)
cv2.imwrite('calibresult11.png', out)
cv2.imshow('result', dst)
cv2.waitKey()
#print "方法一:dst的大小为:", dst1.shape
exit()
# undistort方法二
print("-------------------使用重映射的方式-----------------------")
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, np.diag([1.0, 1.0, 1.0]), newcameramtx, (w,h), cv2.CV_32FC1) # 获取映射方程
print(mapx, mapy)
#dst = cv2.remap(img,mapx,mapy,cv2.INTER_LINEAR) # 重映射
dst = cv2.remap(img,mapx,mapy,cv2.INTER_CUBIC) # 重映射后，图像变小了
x,y,w,h = roi
#dst2 = dst[y:y+h,x:x+w]
dst2 = dst
cv2.imwrite('calibresult11_2.jpg', dst2)
print "方法二:dst的大小为:", dst2.shape # 图像比方法一的小
print("-------------------计算反向投影误差-----------------------")
tot_error = 0
for i in xrange(len(obj_points)):
img_points2, _ = cv2.projectPoints(obj_points[i],rvecs[i],tvecs[i],mtx,dist)
error = cv2.norm(img_points[i],img_points2, cv2.NORM_L2)/len(img_points2)
tot_error += error
# In[3]:
# for i in range(len(left_)):
# plot_figures({'left_1': left_[i], 'right_1': right_[i]}, 1, 2)
# ### undistort and rectify
# In[4]:
h, w = left_[1].shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx_left, distCoeffs_left,
(w, h), 1, (w, h))
x, y, w, h = roi
mapx, mapy = cv2.initUndistortRectifyMap(mtx_left, distCoeffs_left, None,
newcameramtx, (w, h), 5)
left_6_rectified = cv2.remap(left_[6].copy(), mapx, mapy,
cv2.INTER_LINEAR)[y:y + h, x:x + w]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx_right, distCoeffs_right,
(w, h), 1, (w, h))
x, y, w, h = roi
mapx, mapy = cv2.initUndistortRectifyMap(mtx_right, distCoeffs_right, None,
newcameramtx, (w, h), 5)
right_6_rectified = cv2.remap(right_[6].copy(), mapx, mapy, cv2.INTER_LINEAR)
right_6_rectified = right_6_rectified[y:y + h + 10, x:x + w]
left_6_rectified = left_6_rectified[18:y + h - 44, 30:x + w - 20]
plot_figures(
{
'left_6_rectified': left_6_rectified,
D1 = fs.getNode('D1').mat()
M2 = fs.getNode('M2').mat()
D2 = fs.getNode('D2').mat()
fs = cv.FileStorage('extrinsics.yml', cv.FILE_STORAGE_READ)
R = fs.getNode('R').mat()
T = fs.getNode('T').mat()
R1 = fs.getNode('R1').mat()
P1 = fs.getNode('P1').mat()
R2 = fs.getNode('R2').mat()
P2 = fs.getNode('P2').mat()
Q = fs.getNode('Q').mat()
cap = cv.VideoCapture('http://192.168.43.90:8080/?action=stream')
size = (320, 240)
left_map1, left_map2 = cv.initUndistortRectifyMap(M1, D1, R1, P1, size,
cv.CV_16SC2)
right_map1, right_map2 = cv.initUndistortRectifyMap(M2, D2, R2, P2, size,
cv.CV_16SC2)
while True:
ret, frame = cap.read()
frame = cv.resize(frame, (640, 240), interpolation=cv.CV_8SC1)
frame_left = frame[0:240, 0:320]
frame_right = frame[0:240, 320:640]
left_rectified = cv.remap(frame_left, left_map1, left_map2,
cv.INTER_LINEAR)
right_rectified = cv.remap(frame_right, right_map1, right_map2,
cv.INTER_LINEAR)
rectified = np.concatenate([left_rectified, right_rectified], axis=1)
