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)