def calibrate_intrinsic_parameters(calibration_data, calibration_results_file):
"""Calibrate intrinsic parameters of the camera given different images
taken for the Charuco board from different views, the resulting parameters
are saved to the provided filename.
Args:
calibration_data (str): directory of the stored images of the
Charuco board.
calibration_results_file (str): filepath that will be used to write
the calibration results in.
"""
handler = CharucoBoardHandler()
camera_matrix, dist_coeffs, error = handler.calibrate(
calibration_data, visualize=False
)
camera_info = dict()
camera_info["camera_matrix"] = dict()
camera_info["camera_matrix"]["rows"] = 3
camera_info["camera_matrix"]["cols"] = 3
camera_info["camera_matrix"]["data"] = camera_matrix.flatten().tolist()
camera_info["distortion_coefficients"] = dict()
camera_info["distortion_coefficients"]["rows"] = 1
camera_info["distortion_coefficients"]["cols"] = 5
camera_info["distortion_coefficients"][
"data"
] = dist_coeffs.flatten().tolist()
with open(calibration_results_file, "w") as outfile:
yaml.dump(
camera_info, outfile, default_flow_style=False,
)
return
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"-i",
type=str,
required=True,
help="""Path to the image in which the points are visualized.""",
)
parser.add_argument(
"-c",
type=str,
required=True,
help="""Path to the camera calibration file.""",
)
parser.add_argument(
"--detect-board-position",
action="store_true",
help="""Detect the board position from the image instead of using the
pose from the calibration file.""",
)
args = parser.parse_args()
camera_info = CameraCalibrationFile(args.c)
camera_matrix = camera_info["camera_matrix"]
distortion_coeffs = camera_info["distortion_coefficients"]
handler = CharucoBoardHandler(
BOARD_SIZE_X,
BOARD_SIZE_Y,
BOARD_SQUARE_SIZE,
BOARD_MARKER_SIZE,
camera_matrix,
distortion_coeffs,
)
points = np.vstack([
x.flatten() for x in np.meshgrid(
np.arange(-0.3, 0.4, 0.1),
np.arange(-0.3, 0.4, 0.1),
[0.0],
)
]).T
image = cv2.imread(args.i)
if args.detect_board_position:
_, _, rvec, tvec = handler.detect_board(image)
board_offset = np.array([0.1, 0.14, 0])
points = points + board_offset
else:
# get rvec and tvec from projection matrix
pose_mat = camera_info["projection_matrix"]
tvec = pose_mat[0:3, 3]
rvec = cv2.Rodrigues(pose_mat[:3, :3])[0]
imgpoints, _ = cv2.projectPoints(
points,
rvec,
tvec,
camera_matrix,
distortion_coeffs,
)
for imgpoint in imgpoints:
cv2.drawMarker(image, tuple(imgpoint[0].astype(int)),
tuple([0, 0, 255]))
cv2.imshow("foo", image)
cv2.waitKey()
def calibrate_extrinsic_parameters(
calibration_results_file,
charuco_centralized_image_filename,
extrinsic_calibration_filename,
impose_cube=True,
):
"""Calibrate extrinsic parameters of the camera given one image taken for
the Charuco board centered at (0, 0, 0) the resulting parameters are
saved to the provided filename and a virtual cube is imposed on the
board for verification.
Args:
calibration_results_file (str): filepath that will be used to read
the intrinsic calibration results.
charuco_centralized_image_filename (str): filename of the image
taken for the Charuco board centered at (0, 0, 0).
extrinsic_calibration_filename (str): filepath that will be used
to write the extrinsic calibration results in.
impose_cube (bool): boolean whether to output a virtual cube
imposed on the first square of the board or not.
"""
with open(calibration_results_file) as file:
calibration_data = yaml.safe_load(file)
def config_matrix(data):
return np.array(data["data"]).reshape(data["rows"], data["cols"])
camera_matrix = config_matrix(calibration_data["camera_matrix"])
dist_coeffs = config_matrix(calibration_data["distortion_coefficients"])
handler = CharucoBoardHandler(
BOARD_SIZE_X,
BOARD_SIZE_Y,
BOARD_SQUARE_SIZE,
BOARD_MARKER_SIZE,
camera_matrix,
dist_coeffs,
)
rvec, tvec = handler.detect_board_in_image(
charuco_centralized_image_filename, visualize=False)
# projection_matrix = np.zeros((4, 4))
projection_matrix = utils.rodrigues_to_matrix(rvec)
projection_matrix[0:3, 3] = tvec[:, 0]
projection_matrix[3, 3] = 1
calibration_data["projection_matrix"] = dict()
calibration_data["projection_matrix"]["rows"] = 4
calibration_data["projection_matrix"]["cols"] = 4
calibration_data["projection_matrix"]["data"] = projection_matrix.flatten(
).tolist()
with open(extrinsic_calibration_filename, "w") as outfile:
yaml.dump(
calibration_data,
outfile,
default_flow_style=False,
)
if impose_cube:
new_object_points = (np.array(
[
[0, 0, 0],
[0, 1, 0],
[1, 0, 0],
[1, 1, 0],
[0, 0, 1],
[0, 1, 1],
[1, 0, 1],
[1, 1, 1],
],
dtype=np.float32,
) * 0.04)
world_origin_points = (np.array(
[
[0, 0, 0],
[0, 1, 0],
[1, 0, 0],
[0, 0, 1],
],
dtype=np.float32,
) * 0.1)
# cube
point_pairs = (
(0, 4),
(1, 5),
(2, 6),
(3, 7),
(0, 1),
(0, 2),
(1, 3),
(2, 3),
(4, 5),
(4, 6),
(5, 7),
(6, 7),
)
img = cv2.imread(charuco_centralized_image_filename)
imgpoints, _ = cv2.projectPoints(
new_object_points,
rvec,
tvec,
camera_matrix,
dist_coeffs,
)
for p1, p2 in point_pairs:
cv2.line(
img,
tuple(imgpoints[p1, 0]),
tuple(imgpoints[p2, 0]),
[200, 200, 0],
thickness=2,
)
cv2.imshow("Imposed Cube", img)
cv2.waitKey(0)
cv2.destroyAllWindows()
def calibrate_mean_extrinsic_parameters(
camera_matrix,
dist_coeffs,
charuco_centralized_image_dir,
extrinsic_calibration_filename,
impose_cube=True,
):
"""Calibrate extrinsic parameters of the camera given several imaeges taken for
the Charuco board centered at (0, 0, 0). transform the extrinsic parameters into the
fixed 'world' coordinate system.
the resulting parameters are averaged for all images and saved to the provided filename.
a virtual cube on the board as well as the world coordinates axes are imposed for verification.
Args:
camera_matrix, dist_coeffs: output of the intrinsic calibration (either read from file or directly obtained from
intrinsic calibration function.
charuco_centralized_image_dir (str): directory containing images
taken for the Charuco board centered at (0, 0, 0).
extrinsic_calibration_filename (str): filepath that will be used
to write the extrinsic calibration results in.
impose_cube (bool): boolean whether to output a virtual cube
imposed on the first square of the board or not.
"""
handler = CharucoBoardHandler(
BOARD_SIZE_X,
BOARD_SIZE_Y,
BOARD_SQUARE_SIZE,
BOARD_MARKER_SIZE,
camera_matrix,
dist_coeffs,
)
file_pattern = "*.png"
pattern = os.path.join(charuco_centralized_image_dir, file_pattern)
ind = 0
projection_matrix = np.zeros((len(glob.glob(pattern)), 4, 4))
for filename in glob.glob(pattern):
img = cv2.imread(filename)
rvec, tvec = handler.detect_board_in_image(filename, visualize=False)
# geometric data of the calibration board with respect to the 'world' coordinates
# inclination angle of the board (CAD data)
alpha = 22
alphr = np.radians(alpha)
# half-width of the calibration board (CAD data)
Dx = 0.105
# projected (on the base) half-height of the calibration board (CAD data)
Dy = 0.16054
# thickness of the base-plate (measured)
T1 = 0.00435
# thickness of the calibration board plate (measured)
T2 = 0.0045
# y-distance of the pattern axis from the plate edge (measured)
dy = 0.0378
# x-distance of the pattern axis from the plate edge (measured)
dx = 0.0047
# x-distance of the pattern origin to the world origin
tx = Dx - dx
# y-distance of the pattern origin to the world origin
ty = (Dy - T2 * np.sin(alphr) -
T1 * np.tan(alphr)) * np.cos(alphr) - dy
# z-distance of the pattern origin to the world origin
tz = (T2 +
(Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.sin(alphr) +
T1 / np.cos(alphr))
# resulting translation vector
Tvec = np.array([tx, ty, -tz], dtype="float32")
# rotation around the x-axis (pattern coordinate system)
xrot = np.array([1, 0, 0]) * np.radians(-alpha)
xMat = cv2.Rodrigues(xrot)[0]
# (really dirty method to get) rotation angle of the calibration board
zrot = int(filename[-6:-4])
zrot = (zrot - 1) * 10
zrot = np.radians(zrot)
zrot = np.matmul(xMat, np.array([0, 0, 1])) * zrot
zMat = cv2.Rodrigues(zrot)[0]
# absolute world vectors equivalent to cv2 tvec and rvec:
tvecW = np.matmul(cv2.Rodrigues(rvec)[0], Tvec) + tvec.T
rvecW = cv2.Rodrigues(
np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)))[0]
# embed()
projection_matrix[ind, 0:4, 0:4] = utils.rodrigues_to_matrix(rvecW)
projection_matrix[ind, 0:3, 3] = tvecW
projection_matrix[ind, 3, 3] = 1
ind += 1
if impose_cube:
new_object_points = (np.array(
[
[0, 0, 0],
[0, 1, 0],
[1, 0, 0],
[1, 1, 0],
[0, 0, 1],
[0, 1, 1],
[1, 0, 1],
[1, 1, 1],
],
dtype=np.float32,
) * 0.04)
world_origin_points = (np.array(
[
[0, 0, 0],
[0, 1, 0],
[1, 0, 0],
[0, 0, 1],
],
dtype=np.float32,
) * 0.1)
world_origin_points = (zMat @ xMat @ world_origin_points.T).T
world_origin_points = world_origin_points + Tvec
# cube
point_pairs = (
(0, 4),
(1, 5),
(2, 6),
(3, 7),
(0, 1),
(0, 2),
(1, 3),
(2, 3),
(4, 5),
(4, 6),
(5, 7),
(6, 7),
)
img = cv2.imread(filename)
imgpoints, _ = cv2.projectPoints(
new_object_points,
rvec,
tvec,
camera_matrix,
dist_coeffs,
)
for p1, p2 in point_pairs:
cv2.line(
img,
tuple(imgpoints[p1, 0]),
tuple(imgpoints[p2, 0]),
[200, 200, 0],
thickness=2,
)
# world origin
point_pairs = (
(0, 1),
(0, 2),
(0, 3),
)
imgpoints, _ = cv2.projectPoints(
world_origin_points,
rvec,
tvec,
camera_matrix,
dist_coeffs,
)
for p1, p2 in point_pairs:
cv2.line(
img,
tuple(imgpoints[p1, 0]),
tuple(imgpoints[p2, 0]),
[200, 200, 0],
thickness=2,
)
cv2.imshow("Imposed Cube", img)
cv2.waitKey(100)
cv2.destroyAllWindows()
projection_matrix_std = np.std(projection_matrix, axis=0)
projection_matrix = np.mean(projection_matrix, axis=0)
print("Mean proj matrix:")
print(projection_matrix)
print("Std proj matrix:")
print(projection_matrix_std)
print("Rel std proj matrix:")
print(projection_matrix_std / projection_matrix)
# save all the data
calibration_data = dict()
calibration_data["camera_matrix"] = dict()
calibration_data["camera_matrix"]["rows"] = 3
calibration_data["camera_matrix"]["cols"] = 3
calibration_data["camera_matrix"]["data"] = camera_matrix.flatten().tolist(
)
calibration_data["distortion_coefficients"] = dict()
calibration_data["distortion_coefficients"]["rows"] = 1
calibration_data["distortion_coefficients"]["cols"] = 5
calibration_data["distortion_coefficients"]["data"] = dist_coeffs.flatten(
).tolist()
calibration_data["projection_matrix"] = dict()
calibration_data["projection_matrix"]["rows"] = 4
calibration_data["projection_matrix"]["cols"] = 4
calibration_data["projection_matrix"]["data"] = projection_matrix.flatten(
).tolist()
calibration_data["projection_matrix_std"] = dict()
calibration_data["projection_matrix_std"]["rows"] = 4
calibration_data["projection_matrix_std"]["cols"] = 4
calibration_data["projection_matrix_std"][
"data"] = projection_matrix_std.flatten().tolist()
with open(extrinsic_calibration_filename, "w") as outfile:
yaml.dump(
calibration_data,
outfile,
default_flow_style=False,
)
def calibrate_mean_extrinsic_parameters(
camera_matrix,
dist_coeffs,
image_files,
impose_cube=True,
):
"""Calibrate extrinsic parameters of the camera.
Calibrate extrinsic parameters given several images taken of the Charuco
board at defined poses. Transform the extrinsic parameters into the fixed
'world' coordinate system. The resulting parameters are averaged for all
images and saved to the provided filename.
Args:
camera_matrix, dist_coeffs: output of the intrinsic calibration
(either read from file or directly obtained from
intrinsic calibration function.
image_files (list): list of image files.
taken for the Charuco board centered at (0, 0, 0).
to write the extrinsic calibration results in.
impose_cube (bool): boolean whether to output a virtual cube
imposed on the first square of the board or not.
Returns:
The camera parameters including the camera pose.
"""
handler = CharucoBoardHandler(
BOARD_SIZE_X,
BOARD_SIZE_Y,
BOARD_SQUARE_SIZE,
BOARD_MARKER_SIZE,
camera_matrix,
dist_coeffs,
)
camera_params = CameraParameters()
ind = 0
pose_matrix = np.zeros((len(image_files), 4, 4))
for i, filename in enumerate(image_files):
# verify that images are given in the expected order
assert "{:04d}".format(i + 1) in filename
img = cv2.imread(filename)
camera_params.image_height = img.shape[0]
camera_params.image_width = img.shape[1]
rvec, tvec = handler.detect_board_in_image(filename, visualize=False)
# geometric data of the calibration board with respect to the 'world'
# coordinates
# inclination angle of the board (CAD data)
alpha = 22
alphr = np.radians(alpha)
# half-width of the calibration board (CAD data)
Dx = 0.105
# projected (on the base) half-height of the calibration board (CAD
# data)
Dy = 0.16054
# thickness of the base-plate (measured)
T1 = 0.00435
# thickness of the calibration board plate (measured)
T2 = 0.0045
# y-distance of the pattern axis from the plate edge (measured)
dy = 0.0378
# x-distance of the pattern axis from the plate edge (measured)
dx = 0.0047
# x-distance of the pattern origin to the world origin
tx = Dx - dx
# y-distance of the pattern origin to the world origin
ty = (Dy - T2 * np.sin(alphr) -
T1 * np.tan(alphr)) * np.cos(alphr) - dy
# z-distance of the pattern origin to the world origin
tz = (T2 +
(Dy - T2 * np.sin(alphr) - T1 * np.tan(alphr)) * np.sin(alphr) +
T1 / np.cos(alphr))
# resulting translation vector
Tvec = np.array([tx, ty, -tz], dtype="float32")
# rotation around the x-axis (pattern coordinate system)
xrot = np.array([1, 0, 0]) * np.radians(-alpha)
xMat = cv2.Rodrigues(xrot)[0]
# get rotation angle of the calibration board
zrot = i + 1
zrot = (zrot - 1) * 10
zrot = np.radians(zrot)
zrot = np.matmul(xMat, np.array([0, 0, 1])) * zrot
zMat = cv2.Rodrigues(zrot)[0]
# absolute world vectors equivalent to cv2 tvec and rvec:
tvecW = np.matmul(cv2.Rodrigues(rvec)[0], Tvec) + tvec.T
rvecW = cv2.Rodrigues(
np.matmul(cv2.Rodrigues(rvec)[0], np.matmul(zMat, xMat)))[0]
# embed()
pose_matrix[ind, 0:4, 0:4] = utils.rodrigues_to_matrix(rvecW)
pose_matrix[ind, 0:3, 3] = tvecW
pose_matrix[ind, 3, 3] = 1
ind += 1
if impose_cube:
new_object_points = (np.array(
[
[0, 0, 0],
[0, 1, 0],
[1, 0, 0],
[1, 1, 0],
[0, 0, 1],
[0, 1, 1],
[1, 0, 1],
[1, 1, 1],
],
dtype=np.float32,
) * 0.04)
world_origin_points = (np.array(
[
[0, 0, 0],
[0, 1, 0],
[1, 0, 0],
[0, 0, 1],
],
dtype=np.float32,
) * 0.1)
world_origin_points = (zMat @ xMat @ world_origin_points.T).T
world_origin_points = world_origin_points + Tvec
# cube
point_pairs = (
(0, 4),
(1, 5),
(2, 6),
(3, 7),
(0, 1),
(0, 2),
(1, 3),
(2, 3),
(4, 5),
(4, 6),
(5, 7),
(6, 7),
)
img = cv2.imread(filename)
imgpoints, _ = cv2.projectPoints(
new_object_points,
rvec,
tvec,
camera_matrix,
dist_coeffs,
)
for p1, p2 in point_pairs:
cv2.line(
img,
tuple(imgpoints[p1, 0]),
tuple(imgpoints[p2, 0]),
[200, 200, 0],
thickness=2,
)
# world origin
point_pairs = (
(0, 1),
(0, 2),
(0, 3),
)
imgpoints, _ = cv2.projectPoints(
world_origin_points,
rvec,
tvec,
camera_matrix,
dist_coeffs,
)
for p1, p2 in point_pairs:
cv2.line(
img,
tuple(imgpoints[p1, 0].astype(int)),
tuple(imgpoints[p2, 0].astype(int)),
[200, 200, 0],
thickness=2,
)
cv2.imshow("Imposed Cube", img)
cv2.waitKey(100)
cv2.destroyAllWindows()
camera_params.camera_matrix = camera_matrix
camera_params.dist_coeffs = dist_coeffs
camera_params.tf_world_to_camera_std = np.std(pose_matrix, axis=0)
camera_params.tf_world_to_camera = np.mean(pose_matrix, axis=0)
print("Mean proj matrix:")
print(camera_params.tf_world_to_camera)
print("Std proj matrix:")
print(camera_params.tf_world_to_camera_std)
print("Rel std proj matrix:")
print(camera_params.tf_world_to_camera_std /
camera_params.tf_world_to_camera)
return camera_params
def main():
"""Execute an action depending on arguments passed by the user."""
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"action",
choices=["create_board", "detect_live", "detect_image", "calibrate"],
help="""Action that is executed.""",
)
parser.add_argument(
"--filename",
type=str,
help="""Filename used for saving or loading images (depending on the
action).
""",
)
parser.add_argument(
"--calibration-data",
type=str,
help="""Path to the calibration data directory (only used for action
'calibrate').
""",
)
parser.add_argument(
"--camera-info",
type=str,
help="""Camera info file, including intrinsic parameters.""",
)
parser.add_argument(
"--no-gui",
action="store_true",
help="""Set to disable any GUI-based visualization.""",
)
args = parser.parse_args()
camera_matrix = None
distortion_coeffs = None
if args.camera_info:
camera_info = CameraCalibrationFile(args.camera_info)
camera_matrix = camera_info["camera_matrix"]
distortion_coeffs = camera_info["distortion_coefficients"]
handler = CharucoBoardHandler(
BOARD_SIZE_X,
BOARD_SIZE_Y,
BOARD_SQUARE_SIZE,
BOARD_MARKER_SIZE,
camera_matrix,
distortion_coeffs,
)
if args.action == "create_board":
if not args.filename:
raise RuntimeError("Filename not specified.")
handler.save_board(args.filename)
elif args.action == "detect_live":
handler.detect_board_in_camera_stream()
elif args.action == "detect_image":
if not args.filename:
raise RuntimeError("Filename not specified.")
handler.detect_board_in_image(args.filename, visualize=not args.no_gui)
elif args.action == "calibrate":
pattern = os.path.join(args.calibration_data, args.filename)
files = glob.glob(pattern)
handler.calibrate(
files,
visualize=not args.no_gui,
)