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")
