def calibrate_charuco(allCorners, allIds, board, video_params): print("calibrating...") tstart = time() cameraMat = np.eye(3) distCoeffs = np.zeros(5) dim = (video_params['width'], video_params['height']) calib_flags = cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 + \ cv2.CALIB_FIX_PRINCIPAL_POINT error, cameraMat, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( allCorners, allIds, board, dim, cameraMat, distCoeffs, flags=calib_flags) tend = time() tdiff = tend - tstart print("calibration took {} minutes and {:.1f} seconds".format( int(tdiff / 60), tdiff - int(tdiff / 60) * 60)) out = dict() out['error'] = error out['camera_mat'] = cameraMat.tolist() out['dist_coeff'] = distCoeffs.tolist() out['width'] = video_params['width'] out['height'] = video_params['height'] out['fps'] = video_params['fps'] return out
def calcCameraIntrinsicFromCharucoData(self, corners_all, ids_all, image_size): # Make sure at least one image was found if len(corners_all) < 1: # Calibration failed because there were no images, warn the user print( "Calibration was unsuccessful. No images of charucoboards were found." ) # Exit for failure return False, None, None, None, None if len(corners_all) == ids_all: print("lengths of the two arrays must be the same.") # Exit for failure return False, None, None, None, None print('Caliculating with {} frames'.format(len(corners_all))) start = time.time() # Now that we've seen all of our images, perform the camera calibration # based on the set of points we've discovered calibration, cameraMatrix, distCoeffs, nView_rvecs, nView_tvecs = aruco.calibrateCameraCharuco( charucoCorners=corners_all, charucoIds=ids_all, board=self.CHARUCO_BOARD, imageSize=image_size, cameraMatrix=None, distCoeffs=None) t = time.time() - start print('Caliculatiion took {} seconds'.format(t)) return calibration, cameraMatrix, distCoeffs, nView_rvecs, nView_tvecs
def get_calibration_matrices(board, img_arr): """ :param board: Charucoboard object to calibrate against :param img_arr: array of images (from different viewpoints) :return ret_val: unknown usage :return camera_matrix: 3X3 camera calibration matrix :return dist_coeffs: camera's distortion coefficients """ dictionary = AeroCubeMarker.get_dictionary() all_charuco_corners = [] all_charuco_IDs = [] img_size = None for img in img_arr: # Convert to grayscale before performing operations gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) corners, IDs, _ = aruco.detectMarkers(gray, dictionary) _, charuco_corners, charuco_IDs = aruco.interpolateCornersCharuco( corners, IDs, gray, board) all_charuco_corners.append(charuco_corners) all_charuco_IDs.append(charuco_IDs) # Get matrix shape of grayscale image img_size = gray.shape ret_val, camera_matrix, dist_coeffs, _, _ = aruco.calibrateCameraCharuco( all_charuco_corners, all_charuco_IDs, board, img_size, None, None) return ret_val, camera_matrix, dist_coeffs
def calibrate_camera_charuco(corners, ids, charuco_board, image_size): # Now that we've seen all of our images, perform the camera calibration # based on the set of points we've discovered calibration, camera_matrix, dist_coeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( charucoCorners=corners, charucoIds=ids, board=charuco_board, imageSize=image_size, cameraMatrix=None, distCoeffs=None) return calibration, camera_matrix, dist_coeffs, rvecs, tvecs
def calibrate_camera(): """ Calibrate our camera """ # Display Charuco Calibration Board charuco = charucoBoard.draw(SCREEN_RESOLUTION) show_fullscreen_image(charuco) # Step 1: Initialize Camera # stream = cv2.VideoCapture(0) # stream.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) # stream.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) stream = rs.RealSenseCamera() time.sleep(2) # Step 2: Identify Charuco Corners and IDs corners, ids = get_charuco_corners(stream) # Step 3: Calculate and Save Camera Matrix & Distortion Coefficients print('Finished collecting data, computing...') try: err, camera_matrix, dist_coeffs, _, _ = aruco.calibrateCameraCharuco( corners, ids, charucoBoard, CAMERA_RESOLUTION, None, None) print('Calibrated with error: ', err) save_json({ 'camera_matrix': camera_matrix.tolist(), 'dist_coeffs': dist_coeffs.tolist(), 'err': err }) print('...DONE') except Exception as e: print(e) # Step 4: Generate Undistortion / Rectify Map camera_matrix, dist_coeffs = load_camera_props(CAMERA_CONFIG_PATH) mapx, mapy = get_undistort_maps(camera_matrix, dist_coeffs) # Step 5: Show Calibrated Image while True: #cap_success, frame = stream.read() frame = stream.read_rgb() frame = undistort_image(frame, mapx, mapy) cv2.imshow('Calibrated Image', frame) if cv2.waitKey(1) & 255 == ord('q'): break # stream.stop() cv2.destroyAllWindows()
def get_camera_intrinsics(all_corners, all_ids, im_size): try: _, cam_mat, dist_coeffs, _, _ = aruco.calibrateCameraCharuco( charucoCorners=all_corners, charucoIds=all_ids, board=charuco_board, imageSize=im_size, cameraMatrix=None, distCoeffs=None) print("K:\n {}".format(np.around(cam_mat[0:3], decimals=4))) print("Distortion coefficients:\n {}".format( np.around(dist_coeffs[0], decimals=3))) return {"K": cam_mat.tolist(), "distortion": dist_coeffs.tolist()} except: print("Intrinsic calibration failed. Recalibrating...")
def calibrate_camera(): """ Calibrate our camera """ # Step 1: Initialize Camera stream = VideoStream(usePiCamera=True, resolution=CAMERA_RESOLUTION).start() time.sleep(2) # Step 2: Identify Charuco Corners and IDs corners, ids = get_charuco_corners(stream) # Step 3: Calculate and Save Camera Matrix & Distortion Coefficients print('Finished collecting data, computing...') try: err, camera_matrix, dist_coeffs, _, _ = aruco.calibrateCameraCharuco( corners, ids, charucoBoard, CAMERA_RESOLUTION, None, None) print('Calibrated with error: ', err) save_json({ 'camera_matrix': camera_matrix.tolist(), 'dist_coeffs': dist_coeffs.tolist(), 'err': err }) print('...DONE') except Exception as e: print(e) # Step 4: Generate Undistortion / Rectify Map camera_matrix, dist_coeffs = load_camera_props(CAMERA_CONFIG_PATH) mapx, mapy = get_undistort_maps(camera_matrix, dist_coeffs) # Step 5: Show Calibrated Image while True: frame = cv2.flip(stream.read(), flipCode=-1) frame = undistort_image(frame, mapx, mapy) cv2.imshow('Calibrated Image', frame) if cv2.waitKey(1) & 255 == ord('q'): break stream.stop() cv2.destroyAllWindows()
def get_fake_calibration_parameters() -> CalibrationParameters: """ HACK: Generate fake calibration parameters """ dictionary = aruco.getPredefinedDictionary(MarkerType.ARUCO_6X6) seen_corners = [] seen_ids = [] image_size = (200, 200) board = aruco.CharucoBoard_create(6, 6, 0.025, 0.0125, dictionary) image = board.draw(image_size) for _ in range(15): corners, ids, _ = aruco.detectMarkers(image, dictionary) _, corners, ids = aruco.interpolateCornersCharuco(corners, ids, image, board) seen_corners.append(corners) seen_ids.append(ids) ret, mtx, dist, _, _ = aruco.calibrateCameraCharuco( seen_corners, seen_ids, board, image_size, None, None ) return CalibrationParameters(mtx, dist, (1280, 720))
def get_fake_calibration_parameters(size: int, iterations: int = 15 ) -> CalibrationParameters: """ HACK: Generate fake calibration parameters """ dictionary = aruco.getPredefinedDictionary(MarkerDict.DICT_6X6_1000) seen_corners = [] seen_ids = [] image_size = (size, size) board = aruco.CharucoBoard_create(6, 6, 0.025, 0.0125, dictionary) for i in range(iterations): image = board.draw(image_size) corners, ids, _ = aruco.detectMarkers(image, dictionary) _, corners, ids = aruco.interpolateCornersCharuco( corners, ids, image, board) seen_corners.append(corners) seen_ids.append(ids) ret, mtx, dist, _, _ = aruco.calibrateCameraCharuco( seen_corners, seen_ids, board, image_size, None, None) return CalibrationParameters(mtx, dist)
def intrinsic_calib(self): cameraMatrixInit = np.array([[1000., 0., self.im_size[0] / 2.], [0., 1000., self.im_size[1] / 2.], [0., 0., 1.]]) try: _, cameramatrix, distcoeffs, _, _ = aruco.calibrateCameraCharuco( charucoCorners=self.corners_all, charucoIds=self.ids_all, board=charuco_board, imageSize=self.im_size, cameraMatrix=cameraMatrixInit, distCoeffs=None) print("K: {}".format(cameramatrix[0:3])) print("Dist coeffs: {}".format(distcoeffs[0])) self.camera_matrix[:, :] = cameramatrix self.distCoeffs[:, :] = distcoeffs self.calibOutput.append({ "K": cameramatrix.tolist(), "d": distcoeffs.tolist() }) except: print("Intrinsic calibration failed. Recalibrating...") self.frame_counter = 0
def calibrate(self): self.loadimages() # Ready to calibrate. print "Calibrating..." try: retval, self.cameraMatrix, self.distCoefs, self.rvecs, self.tvecs = aruco.calibrateCameraCharuco( self.calibrationCorners, self.calibrationIds, self.board, self.grayimage.shape, None, None) #print(retval, cameraMatrix, distCoeffs, rvecs, tvecs) self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix( self.cameraMatrix, self.distCoefs, (self.imageWidth, self.imageHeight), 1, (self.imageWidth, self.imageHeight)) #print(self.newcameramtx, self.roi) print "Calibration successful" self.calibrated = True except: print "Calibration failed" exit() return
def calibrate_charuco(dirpath, image_format, marker_length, square_length): aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_1000) board = aruco.CharucoBoard_create(5, 7, square_length, marker_length, aruco_dict) arucoParams = aruco.DetectorParameters_create() counter, corners_list, id_list = [], [], [] img_dir = pathlib.Path(dirpath) first = 0 # Find the ArUco markers inside each image for img in img_dir.glob(f'*{image_format}'): print(f'using image {img}') image = cv2.imread(str(img)) img_gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) corners, ids, rejected = aruco.detectMarkers(img_gray, aruco_dict, parameters=arucoParams) resp, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=img_gray, board=board) # If a Charuco board was found, let's collect image/corner points # Requiring at least 20 squares if resp > 20: # Add these corners and ids to our calibration arrays corners_list.append(charuco_corners) id_list.append(charuco_ids) # Actual calibration ret, mtx, dist, rvecs, tvecs = aruco.calibrateCameraCharuco( charucoCorners=corners_list, charucoIds=id_list, board=board, imageSize=img_gray.shape, cameraMatrix=None, distCoeffs=None) return [ret, mtx, dist, rvecs, tvecs]
def savecalibration(self): # Show number of valid captures print("{} valid captures".format(self.capturecount)) # Make sure we were able to calibrate on at least one charucoboard if len(self.corners_all) == 0: print( "Calibration was unsuccessful. We couldn't detect charucoboards in the video." ) print( "Make sure that the calibration pattern is the same as the one we are looking for (ARUCO_DICT)." ) exit() print("Generating calibration...") # Now that we've seen all of our images, perform the camera calibration calibration, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( charucoCorners=self.corners_all, charucoIds=self.ids_all, board=CHARUCO_BOARD, imageSize=self.image_size, cameraMatrix=None, distCoeffs=None) # Print matrix and distortion coefficient to the console print("Camera intrinsic parameters matrix:\n{}".format(cameraMatrix)) print("\nCamera distortion coefficients:\n{}".format(distCoeffs)) # Save the calibrationq f = open('./CameraCalibration.pckl', 'wb') pickle.dump((cameraMatrix, distCoeffs, rvecs, tvecs), f) f.close() # Print to console our success print('Calibration successful. Calibration file created: {}'.format( 'CameraCalibration.pckl'))
allCharucoCorners = [] allCharucoIds = []; if len(res[0]) > 0: res2 = aruco.interpolateCornersCharuco(res[0], res[1], gray, aruco_board) if res2[1] is not None and res2[2] is not None and len(res2[1])>3: allCharucoCorners.append(res2[1]) allCharucoIds.append(res2[2]) aruco.drawDetectedMarkers(gray, res[0], res[1]) cv2.imshow('frame', gray) cv2.waitKey(0) cal = aruco.calibrateCameraCharuco(allCharucoCorners, allCharucoIds, aruco_board, size, None, None) mtx = cal[1] dist = cal[2] rvecs = cal[3][0] tvecs = cal[4][0] cv2.destroyAllWindows() cap = cv2.VideoCapture(0) lastIDPosition = {} # goal: calculate position of viewer position = np.array([0, 0, 0]) while(True): # capture frames start_time = time.time() # start time of the loop
# Make sure we were able to calibrate on at least one charucoboard by checking # if we ever determined the image size if not image_size: # Calibration failed because we didn't see any charucoboards of the PatternSize used print( "Calibration was unsuccessful. We couldn't detect charucoboards in any of the images supplied. Try changing the patternSize passed into Charucoboard_create(), or try different pictures of charucoboards." ) # Exit for failure exit() # Now that we've seen all of our images, perform the camera calibration # based on the set of points we've discovered calibration, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( charucoCorners=corners_all, charucoIds=ids_all, board=CHARUCO_BOARD, imageSize=image_size, cameraMatrix=None, distCoeffs=None) # Print matrix and distortion coefficient to the console print(cameraMatrix) print(distCoeffs) # Save values to be used where matrix+dist is required, for instance for posture estimation # I save files in a pickle file, but you can use yaml or whatever works for you f = open('calibration.pckl', 'wb') pickle.dump((cameraMatrix, distCoeffs, rvecs, tvecs), f) f.close() # Print to console our success
def calibrateCamera(self, rows=7, columns=5, lengthSquare=0.0354, lengthMarker=0.0177): # Create charuco board with actual measured dimensions from print out board = aruco.CharucoBoard_create( squaresX=columns, squaresY=rows, squareLength=lengthSquare, markerLength=lengthMarker, dictionary=self.arucoDict) # Sub pixel corner detection criteria subPixCriteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001) # Storage variables cornerList = [] idList = [] # Get image paths from calibration folder paths = glob.glob(self.calibrationDir + '*' + self.imgExtension) # Empty imageSize variable to be determined at runtime imageSize = None # Loop through all images for filePath in paths: # Read image and convert to gray img = cv2.imread(filePath) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=self.arucoDict) # Sub pixel detection for corner in corners: cv2.cornerSubPix( image=gray, corners=corner, winSize = (3,3), zeroZone = (-1,-1), criteria = subPixCriteria) # Get charuco corners and ids from detected aruco markers response, charucoCorners, charucoIDs = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=board) # If at least 20 corners were found if response > 20: # Add the corners and ids to calibration list cornerList.append(charucoCorners) idList.append(charucoIDs) # If image size is still None, set it to the image size if not imageSize: imageSize = gray.shape[::-1] else: # Error message print('Error in: ' + str(filePath)) # Make sure at least one image was found if len(paths) < 1: print('No images of charucoboards were found.') return # Make sure at least one charucoboard was found if not imageSize: print('Images supplied were not regonized by calibration') return # Start a timer startTime = time.time() # Run calibration _, self.mtx, self.dist, _, _ = aruco.calibrateCameraCharuco( charucoCorners=cornerList, charucoIds=idList, board=board, imageSize=imageSize, cameraMatrix=None, distCoeffs=None) # Print how long it took print("Calibration took: ", round(time.time()-startTime),' s') # Display matrix and distortion coefficients print('Image size: ', imageSize) print(self.mtx) print(self.dist) # Pickle the results f = open('resources/calibration.pckl', 'wb') pickle.dump((self.mtx, self.dist), f) f.close()
def calibrate_camera(camera_name): """ Calibrate our camera """ required_count = 50 resolution = (640, 480) cap = cv2.VideoCapture(0) #stream = VideoStream(usePiCamera=True, resolution=resolution).start() #time.sleep(2) # Warm up the camera all_corners = [] all_ids = [] frame_idx = 0 frame_spacing = 10 success = False calibration_board = charucoBoard.draw((1680, 1050)) show_calibration_frame(calibration_board) while True: ret, frame = cap.read() #frame = stream.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) marker_corners, marker_ids, _ = aruco.detectMarkers( gray, charucoDictionary) if len(marker_corners) > 0 and frame_idx % frame_spacing == 0: ret, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( marker_corners, marker_ids, gray, charucoBoard) if charuco_corners is not None and charuco_ids is not None and len( charuco_corners) > 3: all_corners.append(charuco_corners) all_ids.append(charuco_ids) aruco.drawDetectedMarkers(gray, marker_corners, marker_ids) cv2.imshow( 'frame', gray) # If we're showing the calibration image, we can't preview if cv2.waitKey(1) & 255 == ord('q'): break frame_idx += 1 print("Found: " + str(len(all_ids)) + " / " + str(required_count)) if len(all_ids) >= required_count: success = True break hide_calibration_frame() if success: print('Finished collecting data, computing...') try: err, camera_matrix, dist_coeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( all_corners, all_ids, charucoBoard, resolution, None, None) print('Calibrated with error: ', err) save_json( { 'camera_matrix': camera_matrix.tolist(), 'dist_coeffs': dist_coeffs.tolist(), 'err': err }, camera_name) print('...DONE') except Exception as e: print(e) success = False # Generate the corrections new_camera_matrix, valid_pix_roi = cv2.getOptimalNewCameraMatrix( camera_matrix, dist_coeffs, resolution, 0) mapx, mapy = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, new_camera_matrix, resolution, 5) while True: ret, frame = cap.read() #frame = stream.read() if mapx is not None and mapy is not None: frame = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR) cv2.imshow('frame', frame) if cv2.waitKey(1) & 255 == ord('q'): break cap.release() #stream.stop() cv2.destroyAllWindows()
def cal(): # ChAruco board variables CHARUCOBOARD_ROWCOUNT = 9 CHARUCOBOARD_COLCOUNT = 6 ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_250) # Create constants to be passed into OpenCV and Aruco methods CHARUCO_BOARD = aruco.CharucoBoard_create( squaresX=CHARUCOBOARD_COLCOUNT, squaresY=CHARUCOBOARD_ROWCOUNT, squareLength=0.028, markerLength=0.014, dictionary=ARUCO_DICT) # Create the arrays and variables we'll use to store info like corners and IDs from images processed corners_all = [] # Corners discovered in all images processed ids_all = [] # Aruco ids corresponding to corners discovered image_size = None # Determined at runtime # This requires a set of images or a video taken with the camera you want to calibrate # I'm using a set of images taken with the camera with the naming convention: # 'camera-pic-of-charucoboard-<NUMBER>.jpg' # All images used should be the same size, which if taken with the same camera shouldn't be a problem images = glob.glob(f'{FOLDER}/*.{IM_TYPE}') # Loop through images glob'ed for iname in images: # Open the image img = cv2.imread(iname) img = cv2.flip(img, 1) # Grayscale the image gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = aruco.detectMarkers( image=gray, dictionary=ARUCO_DICT) # Outline the aruco markers found in our query image img = aruco.drawDetectedMarkers( image=img, corners=corners) # Get charuco corners and ids from detected aruco markers response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=CHARUCO_BOARD) # If a Charuco board was found, let's collect image/corner points # Requiring at least 20 squares if response > 20: # Add these corners and ids to our calibration arrays corners_all.append(charuco_corners) ids_all.append(charuco_ids) # Draw the Charuco board we've detected to show our calibrator the board was properly detected img = aruco.drawDetectedCornersCharuco( image=img, charucoCorners=charuco_corners, charucoIds=charuco_ids) # If our image size is unknown, set it now if not image_size: image_size = gray.shape[::-1] # Reproportion the image, maxing width or height at 1000 proportion = max(img.shape) / 1000.0 img = cv2.resize( img, (int(img.shape[1]/proportion), int(img.shape[0]/proportion))) # Pause to display each image, waiting for key press cv2.imshow('Charuco board', img) cv2.waitKey(0) else: print("Not able to detect a charuco board in image: {}".format(iname)) # Destroy any open CV windows cv2.destroyAllWindows() # Make sure at least one image was found if len(images) < 1: # Calibration failed because there were no images, warn the user print("Calibration was unsuccessful. No images of charucoboards were found. Add images of charucoboards and use or alter the naming conventions used in this file.") # Exit for failure exit() # Make sure we were able to calibrate on at least one charucoboard by checking # if we ever determined the image size if not image_size: # Calibration failed because we didn't see any charucoboards of the PatternSize used print("Calibration was unsuccessful. We couldn't detect charucoboards in any of the images supplied. Try changing the patternSize passed into Charucoboard_create(), or try different pictures of charucoboards.") # Exit for failure exit() # Now that we've seen all of our images, perform the camera calibration # based on the set of points we've discovered calibration, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( charucoCorners=corners_all, charucoIds=ids_all, board=CHARUCO_BOARD, imageSize=image_size, cameraMatrix=None, distCoeffs=None) return cameraMatrix, distCoeffs, rvecs, tvecs
def calibrate(image_folder, output_file, squaresX=6, squaresY=10, squareLength=0.025, markerLength=0.02, dictionary='DICT_4X4_250'): image_filenames = glob.glob(image_folder + '*.jpg') image_filenames.sort() if len(image_filenames) == 0: return None else: dictionary = toDict( dictionary ) # toDict converts string to OpenCV Charuco Dictionary integer # The calibration part is a modified version of https://github.co# toDict converts string to OpenCV Charuco Dictionary integerm/kyle-elsalhi/opencv-examples/blob/master/CalibrationByCharucoBoard/CalibrateCamera.py # Create constants to be passed into OpenCV and Aruco methods CHARUCO_BOARD = aruco.CharucoBoard_create(squaresX=squaresX, squaresY=squaresY, squareLength=squareLength, markerLength=markerLength, dictionary=dictionary) # Create the arrays and variables we'll use to store info like corners and IDs from images processed corners_all = [] # Corners discovered in all images processed ids_all = [] # Aruco ids corresponding to corners discovered image_size = None # Determined at runtime # Loop through images nrImages = len(image_filenames) for ii, iname in enumerate(image_filenames): print("Detecting markers: {:07.3f}%".format(100.0 * ii / (nrImages - 1)), end='\r') # Open the image img = cv2.imread(iname) # imgscale the image gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # Find aruco markers in the query image corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=dictionary) # Outline the aruco markers found in our query image img = aruco.drawDetectedMarkers(image=img, corners=corners) # Get charuco corners and ids from detected aruco markers response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( markerCorners=corners, markerIds=ids, image=gray, board=CHARUCO_BOARD) # If a Charuco board was found, let's collect image/corner points # Requiring at least 6 squares if response > 0.50 * (len(CHARUCO_BOARD.ids)): # Add these corners and ids to our calibration arrays corners_all.append(charuco_corners) ids_all.append(charuco_ids) # If our image size is unknown, set it now if not image_size: image_size = gray.shape[::-1] # Reproportion the image, maxing width or height at 1000 proportion = max(img.shape) / 1000.0 img = cv2.resize(img, (int(img.shape[1] / proportion), int(img.shape[0] / proportion))) else: print("Not able to detect a charuco board in image: {}".format( iname)) # Make sure at least one image was found if len(image_filenames) < 1: print( "Calibration was unsuccessful. No images of charucoboards were found. Add images of charucoboards and use or alter the naming conventions used in this file." ) exit() # Make sure we were able to calibrate on at least one charucoboard by checking # if we ever determined the image size if not image_size: # Calibration failed because we didn't see any charucoboards of the PatternSize used print( "Calibration was unsuccessful. We couldn't detect charucoboards in any of the images supplied. Try changing the patternSize passed into Charucoboard_create(), or try different pictures of charucoboards." ) # Exit for failure exit() # Now that we've seen all of our images, perform the camera calibration # based on the set of points we've discovered print("\nCalculating camera parameters ...") calibration, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( charucoCorners=corners_all, charucoIds=ids_all, board=CHARUCO_BOARD, imageSize=image_size, cameraMatrix=None, distCoeffs=None) # Write results to json file and terminal camCal = { 'cameraMatrix': cameraMatrix.tolist(), 'distCoeffs': distCoeffs.tolist() } with open(output_file, 'w') as outfile: json.dump(camCal, outfile) print("Wrote {} to {}".format(camCal, output_file))
key = cv2.waitKey(0) if key == ord('q'): break elif key == ord('d'): del cornerpoints[n] del aruco_ids[n] else: continue cv2.destroyAllWindows() if aruco_ids: print("Starting Calibration") calibration, cameraMatrix, distCoeffs, rvecs, tvecs = arc.calibrateCameraCharuco( charucoCorners=cornerpoints, charucoIds=aruco_ids, board=board, imageSize=image_size, cameraMatrix=None, distCoeffs=None) # img = cv2.circle(img, tuple(cornerpoints[0][0][0]), 5, (100, 200, 50), 5) # cv2.imshow("qwekn", img) # cv2.waitKey(0) # cv2.destroyAllWindows() # Print matrix and distortion coefficient to the console print(cameraMatrix) np.save("camMat", cameraMatrix) print(distCoeffs) np.save("distCoeff", distCoeffs) else:
# print('ids 1:', len(ids)) # if ids were found, then if len(ids) > 0: # print(corners, ids, rejectedImgPoints) ret, chcorners, chids = aruco.interpolateCornersCharuco(corners, ids, gray, board) calcorners.append(chcorners) calids.append(chids) im2 = im.copy() aruco.drawDetectedCornersCharuco(im2, chcorners, chids) # aruco.drawDetectedMarkers(im2, rejectedImgPoints, borderColor=(100, 0, 240)) cv2.imshow('markers', im2) cv2.waitKey() ret, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco(calcorners, calids, board, gray.shape[::-1], None, None) print('rms', ret) print(cameraMatrix) print(distCoeffs) cam_params = { 'marker_type': 'aruco', 'cameraMatrix': cameraMatrix, 'distCoeffs': distCoeffs, 'image_size': imgs[0].shape[:2], 'marker_size': (x,y), 'marker_scale:': sqr } # im = cv2.cvtColor(imgs[0], cv2.COLOR_GRAY2BGR) # print(im.shape) getPose(imgs[9], board, cameraMatrix, distCoeffs)
frame_idx += 1 print("Found: " + str(len(all_ids)) + " / " + str(required_count)) if len(all_ids) >= required_count: success = True break hide_calibration_frame() if success: print('Finished collecting data, computing...') try: err, camera_matrix, dist_coeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( all_corners, all_ids, charucoBoard, resolution, None, None) print('Calibrated with error: ', err) save_json({ 'camera_matrix': camera_matrix.tolist(), 'dist_coeffs': dist_coeffs.tolist(), 'err': err }) print('...DONE') except Exception as e: print(e) success = False
def calibrate_camera(): # Generate Charuco board dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50) squareSize = 0.03907 #0.03 markerSize = squareSize / 1.5 #0.02 board = aruco.CharucoBoard_create(7, 5, squareSize, markerSize, dictionary) img = board.draw((int(1000 * 1.414), 1000)) cv2.imwrite('charuco.png', img) cap = cv2.VideoCapture(2) # Camera resolution res = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)), int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))) ret, frame = cap.read() print(frame.shape) allCharucoCorners = [] allCharucoIds = [] frame_idx = 0 frame_spacing = 20 # Only check every fifth frame required_count = 60 success = False while True: # Capture and image ret, frame = cap.read() # Convert to gray scale grayframe = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Detect markers marker_corners, marker_ids, _ = aruco.detectMarkers( grayframe, dictionary) # If detected any markers if len(marker_corners) > 0 and frame_idx % frame_spacing == 0: aruco.drawDetectedMarkers(grayframe, marker_corners, marker_ids) # Detect corners ret, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco( marker_corners, marker_ids, grayframe, board) #If detected any corners if charuco_corners is not None and charuco_ids is not None and len( charuco_corners) > 3: allCharucoCorners.append(charuco_corners) allCharucoIds.append(charuco_ids) # Draw detected corners aruco.drawDetectedCornersCharuco(grayframe, charuco_corners, charuco_ids) cv2.imshow('grayframe', grayframe) key = cv2.waitKey(1) & 0xFF if key == ord('q'): break frame_idx += 1 print("Found: " + str(len(allCharucoIds)) + "/" + str(required_count)) if len(allCharucoIds) > required_count: success = True break if success: print("Finished") try: err, camera_matrix, dist_coeffs, rvecs, tvecs = aruco.calibrateCameraCharuco( allCharucoCorners, allCharucoIds, board, res, None, None) print('Calibrated with error', err) print(camera_matrix) save_json({ 'camera_matrix': camera_matrix.tolist(), 'dist_coeffs': dist_coeffs.tolist(), 'err': err }) except: success = False print("failed!") return # Generate the corrections new_camera_matrix, valid_pix_roi = cv2.getOptimalNewCameraMatrix( camera_matrix, dist_coeffs, res, 0) #new_camera_matrix = camera_matrix mapx, mapy = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs, None, new_camera_matrix, res, 5) while True: ret, frame = cap.read() if mapx is not None and mapy is not None: frame = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR) cv2.imshow('frame', frame) if cv2.waitKey(1) & 255 == ord('q'): break cap.release() cv2.destroyAllWindows()
'r') and res2[1] is not None and res2[2] is not None and len( res2[1]) > 8: all_corners.append(res2[1]) all_ids.append(res2[2]) record_count += 1 print("Record: " + str(record_count)) # If [c] pressed, start the calculation if key == ord('c'): # Close all cv2 windows cv2.destroyAllWindows() # Check if recordings have been made if (record_count != 0): print("Calculate calibration [2/4] --> Use " + str(record_count) + " records"), # Calculate the camera calibration ret, mtx, dist, rvecs, tvecs = aruco.calibrateCameraCharuco( all_corners, all_ids, board, img_gray.shape, None, None) print("Save calibration [3/4]") # Save the calibration information into a file np.savez_compressed(name, ret=ret, mtx=mtx, dist=dist, rvecs=rvecs, tvecs=tvecs) print("Done [4/4]") else: print("Interrupted since there are no records...") break # If [ESC] pressed, close the application if key == 27: print("Application closed without calculation")