749 lines
26 KiB
Python
749 lines
26 KiB
Python
|
"""
|
||
|
|
A set of functions and scripts to demonstrate camera calibration and
|
||
|
|
registration.
|
||
|
|
|
||
|
|
Notes:
|
||
|
|
* Parts of this module uses opencv calibration described here:
|
||
|
|
https://docs.opencv.org/4.0.0/d9/d0c/group__calib3d.html
|
||
|
|
* and the Chruco board described here:
|
||
|
|
https://docs.opencv.org/4.0.0/d0/d3c/classcv_1_1aruco_1_1CharucoBoard.html
|
||
|
|
|
||
|
|
Copyright (C) Microsoft Corporation. All rights reserved.
|
||
|
|
"""
|
||
|
|
|
||
|
|
# Standard imports.
|
||
|
|
import os
|
||
|
|
import fnmatch
|
||
|
|
import json
|
||
|
|
import math
|
||
|
|
import time
|
||
|
|
import warnings
|
||
|
|
|
||
|
|
from dataclasses import dataclass
|
||
|
|
from typing import Dict
|
||
|
|
from typing import List
|
||
|
|
from typing import Tuple
|
||
|
|
|
||
|
|
# 3rd party imports.
|
||
|
|
import cv2
|
||
|
|
import cv2.aruco as aruco
|
||
|
|
from cv2 import aruco_CharucoBoard
|
||
|
|
from cv2 import aruco_DetectorParameters as detect_params
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
class CalibrationError(Exception):
|
||
|
|
"""Error during calibration.
|
||
|
|
"""
|
||
|
|
|
||
|
|
class RegistrationError(Exception):
|
||
|
|
"""Error during registration.
|
||
|
|
"""
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
@dataclass
|
||
|
|
class RTMatrix():
|
||
|
|
"""dataclass for containing rotation and translation between coordinates"""
|
||
|
|
|
||
|
|
rotation:List
|
||
|
|
translation:List
|
||
|
|
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def write_json(json_file:str, data:dict)-> None:
|
||
|
|
"""Helper function for writing out json files.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
json_file (str): full path
|
||
|
|
data (dict): Blob of data to write.
|
||
|
|
"""
|
||
|
|
with open(json_file, "w") as j:
|
||
|
|
json.dump(data, j, indent=4)
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def r_as_matrix(rotation:np.array):
|
||
|
|
"""Convert a 3vec rotation array to a rotation matrix.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
rotation (np.array): 3 vector array representing rotation.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
[np.array]: Rotation matrix.
|
||
|
|
"""
|
||
|
|
rmat = np.zeros(shape=(3,3))
|
||
|
|
cv2.Rodrigues(rotation, rmat)
|
||
|
|
return rmat
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def read_opencv_calfile(calfile:str) -> Tuple[np.ndarray,
|
||
|
|
np.ndarray,
|
||
|
|
np.ndarray]:
|
||
|
|
"""Read a calibration file generated by opencv.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
calfile (str): full path to an opencv calibration yaml file.
|
||
|
|
|
||
|
|
Raises:
|
||
|
|
ValueError: Function will explicitly fail when the cv2 calibration file
|
||
|
|
fails to open.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
Tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||
|
|
k_matrix: The camera intrinsics matrix.
|
||
|
|
dist: the distortion matrix.
|
||
|
|
img_size: Image size as ndarray objects, in the order width, height.
|
||
|
|
"""
|
||
|
|
fs_calib = cv2.FileStorage(calfile, cv2.FILE_STORAGE_READ)
|
||
|
|
if not fs_calib.isOpened():
|
||
|
|
raise ValueError(f"failed to open {fs_calib}")
|
||
|
|
|
||
|
|
k_matrix = fs_calib.getNode("K").mat()
|
||
|
|
dist = fs_calib.getNode("dist").mat()
|
||
|
|
img_size = fs_calib.getNode("img_size").mat()
|
||
|
|
fs_calib.release()
|
||
|
|
return k_matrix, dist, img_size
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def write_opencv_calfile(calfile:str,
|
||
|
|
k_matrix:np.array,
|
||
|
|
dist8:List,
|
||
|
|
img_size:np.array) -> None:
|
||
|
|
"""Write out an opencv calibration file for a single camera.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
calfile (str): Full path of calibration file.
|
||
|
|
k_matrix (np.array): the camera intrinsics matrix.
|
||
|
|
dist8 (List): The distortion coefficients.
|
||
|
|
img_size (np.array): 2d array of the image size, in the order width, height.
|
||
|
|
"""
|
||
|
|
fs_calib = cv2.FileStorage(calfile, cv2.FILE_STORAGE_WRITE)
|
||
|
|
fs_calib.write("K", k_matrix)
|
||
|
|
fs_calib.write("dist", dist8)
|
||
|
|
fs_calib.write("img_size", img_size)
|
||
|
|
fs_calib.release()
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def write_calibration_blob(calibrations:List[str],
|
||
|
|
rmat_b_to_a:np.array,
|
||
|
|
tvec_b_to_a:np.array,
|
||
|
|
out_dir:str):
|
||
|
|
"""Write all calibration and registration values to a json blob.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
calibrations (List[str]): All calibration files.
|
||
|
|
rmat_b_to_a (np.array): Rotation matrix array.
|
||
|
|
tvec_b_to_a (np.array): Translation vector array.
|
||
|
|
out_dir (str): Ouput directory to place the calibration_blob.json file.
|
||
|
|
"""
|
||
|
|
|
||
|
|
blob = {"CalibrationInformation":{"Cameras":[]}}
|
||
|
|
|
||
|
|
for idx, calibration_file in enumerate(calibrations):
|
||
|
|
if idx == 0:
|
||
|
|
# RT for the camera used as the origin for all others.
|
||
|
|
reshape_r = [1,0,0,0,1,0,0,0,1]
|
||
|
|
reshape_t = [0,0,0]
|
||
|
|
else:
|
||
|
|
reshape_r = rmat_b_to_a.reshape(9, 1).squeeze(1).tolist()
|
||
|
|
reshape_t = tvec_b_to_a.squeeze(1).tolist()
|
||
|
|
|
||
|
|
camera_matrix, dist, img_size = read_opencv_calfile(calibration_file)
|
||
|
|
intrinsics = [camera_matrix[0][2]/img_size[0][0], #Px
|
||
|
|
camera_matrix[1][2]/img_size[1][0], #Py
|
||
|
|
camera_matrix[0][0]/img_size[0][0], #Fx
|
||
|
|
camera_matrix[1][1]/img_size[1][0], #Fy
|
||
|
|
dist[0][0], #K1
|
||
|
|
dist[1][0], #K2
|
||
|
|
dist[4][0], #K3
|
||
|
|
dist[5][0], #K4
|
||
|
|
dist[6][0], #K5
|
||
|
|
dist[7][0], #K6
|
||
|
|
0, #Cx always Zero. (BrownConrady)
|
||
|
|
0, #Cy always Zero. (BrownConrady)
|
||
|
|
dist[3][0], #P2/Tx
|
||
|
|
dist[2][0]] #P1/Ty
|
||
|
|
|
||
|
|
model_type = "CALIBRATION_LensDistortionModelBrownConrady"
|
||
|
|
intrinsics_data = {"ModelParameterCount":len(intrinsics),
|
||
|
|
"ModelParameters":intrinsics,
|
||
|
|
"ModelType":model_type}
|
||
|
|
|
||
|
|
extrinsics = {"Rotation":reshape_r, "Translation":reshape_t}
|
||
|
|
calibration = {"Intrinsics":intrinsics_data,
|
||
|
|
"Rt":extrinsics,
|
||
|
|
"SensorHeight":img_size[1].tolist(),
|
||
|
|
"SensorWidth":img_size[0].tolist()}
|
||
|
|
|
||
|
|
blob["CalibrationInformation"]["Cameras"].append(calibration)
|
||
|
|
|
||
|
|
os.makedirs(out_dir, exist_ok=True)
|
||
|
|
json_file = os.path.join(out_dir, "calibration_blob.json")
|
||
|
|
write_json(json_file, blob)
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def read_board_parameters(json_file: str) -> Tuple[Dict, aruco_CharucoBoard]:
|
||
|
|
"""Read charuco board from a json file.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
json_file (str): fullpath of the board json_file.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
Tuple[dict, aruco_CharucoBoard]:
|
||
|
|
target: Target data from json_file.
|
||
|
|
board: A single charuco board object.
|
||
|
|
"""
|
||
|
|
|
||
|
|
with open(json_file) as j_file:
|
||
|
|
targets = json.load(j_file)
|
||
|
|
|
||
|
|
target = targets["shapes"][0]
|
||
|
|
aruco_dict = aruco.Dictionary_get(target["aruco_dict_name"])
|
||
|
|
board = aruco.CharucoBoard_create(target["squares_x"],
|
||
|
|
target["squares_y"],
|
||
|
|
target["square_length"]/1000,
|
||
|
|
target["marker_length"]/1000,
|
||
|
|
aruco_dict)
|
||
|
|
|
||
|
|
return target, board
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def get_image_points(board:aruco_CharucoBoard,
|
||
|
|
marker_ids:np.ndarray) -> np.ndarray:
|
||
|
|
"""
|
||
|
|
Generate markers 3d and 2d positions like getBoardObjectAndImagePoints but for
|
||
|
|
Charuco Parameters.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
board (aruco_CharucoBoard): A board object from opencv.
|
||
|
|
marker_ids (np.ndarray): List of detected charuco marker Ids.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
np.ndarray: numpy array (n*1*3) markers 3d positions.
|
||
|
|
"""
|
||
|
|
|
||
|
|
object_points = board.chessboardCorners[marker_ids, :]
|
||
|
|
return object_points
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def detect_markers(img: np.ndarray,
|
||
|
|
template: str,
|
||
|
|
params:detect_params = None) -> Tuple[List[np.ndarray],
|
||
|
|
List[np.ndarray],
|
||
|
|
aruco_CharucoBoard]:
|
||
|
|
"""Detect board markers.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
img (np.ndarray): Board image.
|
||
|
|
template (str): fullpath of the board json_file.
|
||
|
|
params (aruco_DetectorParameters, optional): a cv2 object
|
||
|
|
aruco_DetectorParameters. Defaults to None.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
Tuple[List[np.ndarray], List[np.ndarray], aruco_CharucoBoard]:
|
||
|
|
charuco_corners: List of detected charuco marker corners.
|
||
|
|
charuco_ids: List of detected charuco marker Ids.
|
||
|
|
board: charucoboard object.
|
||
|
|
"""
|
||
|
|
# detect markers
|
||
|
|
_, board = read_board_parameters(template)
|
||
|
|
|
||
|
|
if params is None:
|
||
|
|
params = aruco.DetectorParameters_create()
|
||
|
|
params.cornerRefinementMethod = aruco.CORNER_REFINE_NONE
|
||
|
|
|
||
|
|
aruco_corners, aruco_ids, _ = aruco.detectMarkers(img,
|
||
|
|
board.dictionary,
|
||
|
|
None,
|
||
|
|
None,
|
||
|
|
params)
|
||
|
|
if len(aruco_corners) > 0:
|
||
|
|
_, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
|
||
|
|
aruco_corners,
|
||
|
|
aruco_ids,
|
||
|
|
img,
|
||
|
|
board)
|
||
|
|
if charuco_corners is None:
|
||
|
|
charuco_corners = []
|
||
|
|
charuco_ids = []
|
||
|
|
warnings.warn("No charuco corners detected in image.")
|
||
|
|
else:
|
||
|
|
charuco_corners = []
|
||
|
|
charuco_ids = []
|
||
|
|
warnings.warn("No charuco corners detected in image.")
|
||
|
|
|
||
|
|
return charuco_corners, charuco_ids, board
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def detect_markers_many_images(imgnames:List[str], template: str):
|
||
|
|
"""
|
||
|
|
Run detect_markers on a large set of png or jpeg images in a single directory,
|
||
|
|
with the assumption that all images are viewing the same board.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
imgnames (List[str]):Full path to images.
|
||
|
|
template (str): Template file json of the board.
|
||
|
|
|
||
|
|
Raises:
|
||
|
|
CalibrationError: Not all image sizes are equal.
|
||
|
|
CalibrationError: Insufficient number of markers detected. Inspect images
|
||
|
|
for poor quality.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
[Tuple]: [List[List[np.ndarray]],
|
||
|
|
List[List[np.ndarray]],
|
||
|
|
List[List[np.ndarray]],
|
||
|
|
Tuple[np.array, np.array],
|
||
|
|
aruco_CharucoBoard]
|
||
|
|
ccorners_all: All chauco corners detected in every image.
|
||
|
|
cids_all: All charuco marker ids detected in every image.
|
||
|
|
p3d: Image points.
|
||
|
|
img_size: [width; height].
|
||
|
|
board: Charucoboard object.
|
||
|
|
"""
|
||
|
|
ccorners_all = []
|
||
|
|
cids_all = []
|
||
|
|
p3d = []
|
||
|
|
img_sizes_all = []
|
||
|
|
|
||
|
|
for imgfile in imgnames:
|
||
|
|
img = cv2.imread(imgfile, cv2.IMREAD_GRAYSCALE)
|
||
|
|
if img is not None:
|
||
|
|
ccorners, cids, board = detect_markers(img, template)
|
||
|
|
|
||
|
|
if len(ccorners) > 3:
|
||
|
|
ccorners_all.append(ccorners)
|
||
|
|
cids_all.append(cids)
|
||
|
|
m3d = get_image_points(board, cids)
|
||
|
|
p3d.append(m3d)
|
||
|
|
sizes = np.array([img.shape[1], img.shape[0]])
|
||
|
|
img_sizes_all.append(sizes)
|
||
|
|
|
||
|
|
# check all images sizes are identical.
|
||
|
|
rows_equal = [elem[0]==img_sizes_all[0][0] for elem in img_sizes_all]
|
||
|
|
cols_equal = [elem[1]==img_sizes_all[0][1] for elem in img_sizes_all]
|
||
|
|
|
||
|
|
if not all(rows_equal) or not all(cols_equal):
|
||
|
|
raise CalibrationError("Not all image sizes in data set are the same.")
|
||
|
|
|
||
|
|
img_size = (img_sizes_all[0][0], img_sizes_all[0][1])
|
||
|
|
return ccorners_all, cids_all, p3d, img_size, board
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def estimate_pose(img: np.array,
|
||
|
|
template: str,
|
||
|
|
opencv_calfile: str) -> Tuple[bool,
|
||
|
|
np.ndarray,
|
||
|
|
np.ndarray]:
|
||
|
|
"""Estimate camera pose using board.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
img (np.array): Board image.
|
||
|
|
template (str): fullpath of the board json_file.
|
||
|
|
opencv_calfile (str): fullpath of the opencv cal file.
|
||
|
|
|
||
|
|
Raises:
|
||
|
|
ValueError: Throw an error if the calibration file fails to load.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
Tuple[bool, np.ndarray, np.ndarray]: Returns success of calibration and
|
||
|
|
extrinsics.
|
||
|
|
retval: Return True of the optimizer converged.
|
||
|
|
rvec: rotation array
|
||
|
|
tvec: translation array 1*3
|
||
|
|
"""
|
||
|
|
|
||
|
|
k_matrix, dist, _ = read_opencv_calfile(opencv_calfile)
|
||
|
|
|
||
|
|
rvec = np.full((1, 3), 0.01)
|
||
|
|
tvec = np.full((1, 3), 0.01)
|
||
|
|
|
||
|
|
charuco_corners, charuco_ids, board = detect_markers(img, template)
|
||
|
|
if len(charuco_corners) > 0:
|
||
|
|
retval, rvec, tvec = aruco.estimatePoseCharucoBoard(charuco_corners,
|
||
|
|
charuco_ids,
|
||
|
|
board,
|
||
|
|
k_matrix,
|
||
|
|
dist,
|
||
|
|
rvec,
|
||
|
|
tvec)
|
||
|
|
else:
|
||
|
|
retval = False
|
||
|
|
rvec = []
|
||
|
|
tvec = []
|
||
|
|
return retval, rvec, tvec
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def pose_as_dataclass(array_a:np.array,
|
||
|
|
template:str,
|
||
|
|
calib_a:str,
|
||
|
|
img_a:str)-> RTMatrix:
|
||
|
|
"""Get RT of camera to board.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
array_a (np.array): Numpy array of the image.
|
||
|
|
template (str): Template to estimate pose.
|
||
|
|
calib_a (str): calibration file for this camera.
|
||
|
|
img_a (str): Path to image.
|
||
|
|
|
||
|
|
Raises:
|
||
|
|
RegistrationError: Failed to estimate pose of board.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
RTMatrix: Rotation and translation as a dataclass.
|
||
|
|
"""
|
||
|
|
|
||
|
|
[retval_a, rvec_a, tvec_a] = estimate_pose(array_a, template, calib_a)
|
||
|
|
if retval_a is False:
|
||
|
|
raise RegistrationError(f"Could not estimate pose for image @ {img_a}")
|
||
|
|
|
||
|
|
pose_a = RTMatrix(rvec_a, tvec_a)
|
||
|
|
return pose_a
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def unproject(points:np.array, k_mat:np.array, dist:np.array) -> np.array:
|
||
|
|
"""
|
||
|
|
Take the 2D distorted markers in an image and unproject into normalized 3D
|
||
|
|
coordinates.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
points (np.array): Location of markers in image.
|
||
|
|
k_mat (np.array): Camera Matrix.
|
||
|
|
dist (np.array): Distortion coefficients.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
np.array: 3D Normalized coordinates of markers after unprojection.
|
||
|
|
"""
|
||
|
|
|
||
|
|
principal_point = [k_mat[0][2], k_mat[1][2]]
|
||
|
|
focal_length = [k_mat[0][0], k_mat[1][1]]
|
||
|
|
|
||
|
|
term_criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT,
|
||
|
|
400,
|
||
|
|
0.000001)
|
||
|
|
x_u = cv2.undistortPointsIter(points,
|
||
|
|
k_mat,
|
||
|
|
dist,
|
||
|
|
np.eye(3),
|
||
|
|
k_mat,
|
||
|
|
term_criteria)
|
||
|
|
|
||
|
|
rays = (x_u-principal_point)/focal_length
|
||
|
|
return rays
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def registration_error(array_a:np.array,
|
||
|
|
template:str,
|
||
|
|
calib_a:str,
|
||
|
|
pose_b:RTMatrix,
|
||
|
|
rmat_b_to_a:np.array,
|
||
|
|
tvec_b_to_a:np.array) -> Tuple[float, float]:
|
||
|
|
"""
|
||
|
|
Calculate the registration error between two cameras. The registration error
|
||
|
|
is computed by taking the rotation and translation of the board to camera B
|
||
|
|
coordinates, applying the registration, then projecting into 2D camera A
|
||
|
|
coordinates, then calculating this against the reprojection error of the
|
||
|
|
detected markers from camera A.
|
||
|
|
|
||
|
|
camera B camera A (prj)
|
||
|
|
------------ ------------
|
||
|
|
| 3D points| -----> | 3D points|
|
||
|
|
------------ ------------
|
||
|
|
^ |
|
||
|
|
| |
|
||
|
|
| v camera A (detected)
|
||
|
|
------------ ------------ diff with ------------
|
||
|
|
| 3D board | | 2D points| <---------> | 2D points|
|
||
|
|
------------ ------------ ------------
|
||
|
|
|
||
|
|
Args:
|
||
|
|
array_a (np.array): Image from camera A.
|
||
|
|
template (str): Template of the board.
|
||
|
|
calib_a (str): Path to camera calibration file for camera A.
|
||
|
|
pose_b (RTMatrix): Rotation and translation of board to camera B.
|
||
|
|
rmat_b_to_a (np.array): Rotation matrix of camera B to camera A.
|
||
|
|
tvec_b_to_a (np.array): Translation vector of camera B to camera A.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
Tuple[float, float]: Root mean square of all reprojected points in pixels
|
||
|
|
and radians.
|
||
|
|
"""
|
||
|
|
|
||
|
|
# Get 2d image coordinates of markers detected in camera A.
|
||
|
|
corners_a, ids_a, board = detect_markers(array_a, template)
|
||
|
|
|
||
|
|
# 3D board points in coordinates of camera B.
|
||
|
|
points_board = board.chessboardCorners[ids_a, :]
|
||
|
|
squoze = points_board.squeeze(1)
|
||
|
|
markers_cam_b = np.matmul(r_as_matrix(pose_b.rotation),
|
||
|
|
squoze.transpose()) + pose_b.translation
|
||
|
|
|
||
|
|
# Multiply by registration to get markers 3D coordinates in camera A.
|
||
|
|
pts_in_cam_a = np.matmul(rmat_b_to_a, markers_cam_b) + tvec_b_to_a
|
||
|
|
|
||
|
|
# Registration computed 3D points to 2D image plane A.
|
||
|
|
k_mat, dist, _ = read_opencv_calfile(calib_a)
|
||
|
|
pts, _ = cv2.projectPoints(pts_in_cam_a,
|
||
|
|
np.eye(3),
|
||
|
|
np.zeros((3,1)),
|
||
|
|
k_mat,
|
||
|
|
dist)
|
||
|
|
|
||
|
|
# Express difference between measured and projected as radians.
|
||
|
|
angles = []
|
||
|
|
measured = unproject(corners_a, k_mat, dist)
|
||
|
|
prediction = unproject(pts, k_mat, dist)
|
||
|
|
|
||
|
|
for idx, elem in enumerate(measured):
|
||
|
|
meas = [elem[0][0], elem[0][1], 1]
|
||
|
|
pred = [prediction[idx][0][0], prediction[idx][0][1], 1]
|
||
|
|
|
||
|
|
dot_product = np.dot(meas, pred)
|
||
|
|
norm_a = np.linalg.norm(meas)
|
||
|
|
norm_b = np.linalg.norm(pred)
|
||
|
|
theta = math.acos(dot_product / (norm_a * norm_b))
|
||
|
|
angles.append(theta)
|
||
|
|
|
||
|
|
# Get Root Mean Square of measured points in camera A to registration
|
||
|
|
# calculated points.
|
||
|
|
num_points = pts.shape[0]
|
||
|
|
squares = [elem**2 for elem in angles]
|
||
|
|
rms_radians = math.sqrt(sum(squares)/num_points)
|
||
|
|
print(f"RMS (Radians): {rms_radians}")
|
||
|
|
|
||
|
|
rms_pixels = np.sqrt(np.sum((corners_a-pts)**2)/num_points)
|
||
|
|
return rms_pixels, rms_radians
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def calibrate_camera(
|
||
|
|
imdir:str,
|
||
|
|
template:str,
|
||
|
|
init_calfile:str = None,
|
||
|
|
rms_thr:float = 1.0,
|
||
|
|
postfix:str = "",
|
||
|
|
min_detections:int = 30,
|
||
|
|
min_images:int = 30,
|
||
|
|
min_quality_images:int = 30,
|
||
|
|
per_view_threshold:int = 1
|
||
|
|
) -> Tuple[float,
|
||
|
|
np.array,
|
||
|
|
np.array,
|
||
|
|
np.array,
|
||
|
|
List,
|
||
|
|
List]:
|
||
|
|
""" Calibrate a camera using charuco detector and opencv bundler.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
imdir (str): Image directory.
|
||
|
|
template (str): Fullpath of the board json_file.
|
||
|
|
init_calfile (str, optional): Calibration file. Defaults to None.
|
||
|
|
rms_thr (float, optional): Reprojection threshold. Defaults to 1.0.
|
||
|
|
postfix (str, optional): Calibration filename suffix. Defaults to "".
|
||
|
|
min_detections (int, optional): Required minimum number of detections.
|
||
|
|
Defaults to 30.
|
||
|
|
min_images (int, optional): Minimum number of images. Defaults to 30.
|
||
|
|
min_quality_images (int, optional): Minimum number of images with sufficient
|
||
|
|
reprojection quality. Defaults to 30.
|
||
|
|
per_view_threshold (int, optional): RMS reprojection error threshold to
|
||
|
|
distinguish quality images. Defaults to 1.
|
||
|
|
|
||
|
|
Raises:
|
||
|
|
CalibrationError: Not enough images for calibration.
|
||
|
|
CalibrationError: Not enough detections for calibration.
|
||
|
|
CalibrationError: Not enough images with low rms for calibration.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
Tuple[float, np.array, np.array, np.array, List, List]:
|
||
|
|
rms: the overall RMS re-projection error in pixels.
|
||
|
|
k_matrix: camera matrix.
|
||
|
|
dist: Lens distortion coeffs. OpenCV model with 8 distortion is equivalent
|
||
|
|
to Brown-Conrady model. (used in K4A).
|
||
|
|
img_size: [width; height].
|
||
|
|
rvecs: list of rotation vectors.
|
||
|
|
tvecs: list of translation vectors.
|
||
|
|
"""
|
||
|
|
|
||
|
|
# Check validity of initial calibration file.
|
||
|
|
if init_calfile is not None and os.path.exists(init_calfile) is False:
|
||
|
|
FileExistsError(f"Initial calibration does not exist: {init_calfile}")
|
||
|
|
|
||
|
|
# output cal file
|
||
|
|
calfile = os.path.join(imdir, f"calib{postfix}.yml")
|
||
|
|
|
||
|
|
imgnames = []
|
||
|
|
for file in os.listdir(imdir):
|
||
|
|
if fnmatch.fnmatch(file, "*.png") or fnmatch.fnmatch(file, "*.jpg"):
|
||
|
|
imgnames.append(os.path.join(imdir, file))
|
||
|
|
|
||
|
|
num_images = len(imgnames)
|
||
|
|
if num_images < min_images:
|
||
|
|
msg = f"Not Enough images. {num_images} found, {min_images} required."
|
||
|
|
raise CalibrationError(msg)
|
||
|
|
|
||
|
|
ccorners_all, cids_all, p3d, img_size, board = \
|
||
|
|
detect_markers_many_images(imgnames, template)
|
||
|
|
|
||
|
|
# check number of times corners were successfully detected.
|
||
|
|
num_det = len(ccorners_all)
|
||
|
|
if num_det < min_detections:
|
||
|
|
msg = f"Insufficent detections. {num_det} found, {min_detections} required."
|
||
|
|
raise CalibrationError(msg)
|
||
|
|
|
||
|
|
# initial calibration
|
||
|
|
if init_calfile is None:
|
||
|
|
# get image size of any image
|
||
|
|
k_matrix = cv2.initCameraMatrix2D(p3d, ccorners_all, img_size) # (w,h)
|
||
|
|
dist = np.zeros((8, 1), dtype=np.float32)
|
||
|
|
else:
|
||
|
|
k_matrix, dist, img_data = read_opencv_calfile(init_calfile)
|
||
|
|
img_arr = img_data.astype(int)
|
||
|
|
img_size = (img_arr[0][0], img_arr[1][0])
|
||
|
|
|
||
|
|
flags = cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_USE_INTRINSIC_GUESS
|
||
|
|
criteria = cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 100, 1e-6
|
||
|
|
|
||
|
|
start = time.perf_counter()
|
||
|
|
rms, k_matrix, dist, rvecs, tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors = \
|
||
|
|
aruco.calibrateCameraCharucoExtended(ccorners_all,
|
||
|
|
cids_all,
|
||
|
|
board,
|
||
|
|
img_size,
|
||
|
|
k_matrix,
|
||
|
|
dist,
|
||
|
|
flags=flags,
|
||
|
|
criteria=criteria)
|
||
|
|
|
||
|
|
# Check Quality of each image.
|
||
|
|
n_low_rms = [err[0] for err in perViewErrors if err[0] <= per_view_threshold]
|
||
|
|
num_good_images = len(n_low_rms)
|
||
|
|
|
||
|
|
# Report which indexes are failing in perViewErrors.
|
||
|
|
failing_idxs = []
|
||
|
|
[failing_idxs.append(str(index)) for (index, err) in enumerate(perViewErrors) if err[0] > per_view_threshold]
|
||
|
|
warning_failing_indexes = "Failing image indices: " + ", ".join(failing_idxs)
|
||
|
|
|
||
|
|
if len(failing_idxs) != 0:
|
||
|
|
warnings.warn(warning_failing_indexes)
|
||
|
|
|
||
|
|
if num_good_images < min_quality_images:
|
||
|
|
msg = f"Insufficent number of quality images. " \
|
||
|
|
f"{num_good_images} found, {min_quality_images} required."
|
||
|
|
raise CalibrationError(msg)
|
||
|
|
|
||
|
|
dist8 = dist[:8, :]
|
||
|
|
img_size_as_array = np.array([np.array([img_size[0]]),
|
||
|
|
np.array([img_size[1]])])
|
||
|
|
if rms < rms_thr:
|
||
|
|
print("calibrate_camera took {} sec".format(time.perf_counter()-start))
|
||
|
|
write_opencv_calfile(calfile, k_matrix, dist8, img_size_as_array)
|
||
|
|
write_json(os.path.join(imdir, "report.json"), {"RMS_pixels":rms})
|
||
|
|
else:
|
||
|
|
print("calibrate_camera failed \n")
|
||
|
|
|
||
|
|
return rms, k_matrix, dist, img_size, rvecs, tvecs, num_good_images
|
||
|
|
|
||
|
|
#-------------------------------------------------------------------------------
|
||
|
|
def register(img_a: str,
|
||
|
|
img_b: str,
|
||
|
|
template: str,
|
||
|
|
calib_a: str,
|
||
|
|
calib_b: str,
|
||
|
|
out_dir: str,
|
||
|
|
rms_threshold:float=1) -> Tuple[np.array, np.array, float]:
|
||
|
|
"""Get rotation and translation of camera b in terms of camera a.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
img_a (str): Full path to image taken by camera A.
|
||
|
|
img_b (str): Full path to image taken by camera B.
|
||
|
|
template (str): Full path to template image of board.
|
||
|
|
calib_a (str): Full path to opencv calibration file of camera A.
|
||
|
|
calib_b (str): Full path to opencv calibration file of camera B.
|
||
|
|
out_dir (str): Output directory for full calibration blob.
|
||
|
|
rms_threshold (float): Threshold to fail RMS at in radians.
|
||
|
|
|
||
|
|
Raises:
|
||
|
|
FileExistsError: Raise if image file A is not found.
|
||
|
|
FileExistsError: Raise if image file B is not found.
|
||
|
|
FileExistsError: Raise if template file is not found.
|
||
|
|
FileExistsError: Raise if calibration parameters for camera A is not found.
|
||
|
|
FileExistsError: Raise if calibration parameters for camera B is not found.
|
||
|
|
RegistrationError: Raise if OpenCV fails to load image file A.
|
||
|
|
RegistrationError: Raise if OpenCV fails to load image file B.
|
||
|
|
RegistrationError: Raise if reprojection error is too large.
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
Tuple[np.array, np.array, float]: Return Rotation Translation of camera B
|
||
|
|
to A, and rms.
|
||
|
|
rmat_b_to_a: Numpy array of the rotation matrix from B to A.
|
||
|
|
tmat_b_to_a: Numpy array of the translation vector from B to A.
|
||
|
|
rms: Reprojection error expressed as Root mean square of pixel diffs
|
||
|
|
between markers.
|
||
|
|
"""
|
||
|
|
|
||
|
|
# File exists checks.
|
||
|
|
if not os.path.exists(img_a):
|
||
|
|
raise FileExistsError(f"Image file not found for camera A @ {img_a}")
|
||
|
|
if not os.path.exists(img_b):
|
||
|
|
raise FileExistsError(f"Image file not found for camera B @ {img_b}")
|
||
|
|
if not os.path.exists(template):
|
||
|
|
raise FileExistsError(f"Board template parameters not found @ {template}")
|
||
|
|
if not os.path.exists(calib_a):
|
||
|
|
raise FileExistsError(f"Calib params for camera A not found @ {calib_a}")
|
||
|
|
if not os.path.exists(calib_b):
|
||
|
|
raise FileExistsError(f"Calib params for camera B not found @ {calib_b}")
|
||
|
|
|
||
|
|
array_a = cv2.imread(img_a)
|
||
|
|
array_b = cv2.imread(img_b)
|
||
|
|
|
||
|
|
# Check image was read by opencv.
|
||
|
|
if array_a is None:
|
||
|
|
raise RegistrationError(f"OpenCV could not interpret Camera A @ {img_a}")
|
||
|
|
if array_b is None:
|
||
|
|
raise RegistrationError(f"OpenCV could not interpret Camera B @ {img_b}")
|
||
|
|
|
||
|
|
# Get Rt of camera A to board.
|
||
|
|
pose_a = pose_as_dataclass(array_a, template, calib_a, img_a)
|
||
|
|
rmat_a = r_as_matrix(pose_a.rotation)
|
||
|
|
|
||
|
|
# Get Rt of camera B to board.
|
||
|
|
pose_b = pose_as_dataclass(array_b, template, calib_b, img_b)
|
||
|
|
rmat_b = r_as_matrix(pose_b.rotation)
|
||
|
|
|
||
|
|
# Get perspective of camera B to board.
|
||
|
|
rmat_b_to_a = np.matmul(rmat_a, rmat_b.transpose())
|
||
|
|
tvec_b_to_a = -np.matmul(rmat_b_to_a, pose_b.translation) + pose_a.translation
|
||
|
|
|
||
|
|
print(f"Translation camera B to A:\n{tvec_b_to_a}")
|
||
|
|
print(f"Rotation camera B to A:\n{rmat_b_to_a}")
|
||
|
|
|
||
|
|
# Find registration error.
|
||
|
|
print("Find forward reprojection error (camera B to camera A).")
|
||
|
|
(rms1_pixels, rms1_rad) = registration_error(array_a,
|
||
|
|
template,
|
||
|
|
calib_a,
|
||
|
|
pose_b,
|
||
|
|
rmat_b_to_a,
|
||
|
|
tvec_b_to_a)
|
||
|
|
if rms1_rad > rms_threshold:
|
||
|
|
raise RegistrationError("Registration error from A to B too large.")
|
||
|
|
|
||
|
|
# Find reverse registration error.
|
||
|
|
print("Find reverse reprojection error (camera A to camera B).")
|
||
|
|
(rms2_pixels, rms2_rad) = registration_error(array_b,
|
||
|
|
template,
|
||
|
|
calib_b,
|
||
|
|
pose_a,
|
||
|
|
rmat_b_to_a.transpose(),
|
||
|
|
tvec_b_to_a*-1)
|
||
|
|
if rms2_rad > rms_threshold:
|
||
|
|
raise RegistrationError("Registration error from B to A too large.")
|
||
|
|
|
||
|
|
# Write to calibration blob.
|
||
|
|
write_calibration_blob([calib_a, calib_b], rmat_b_to_a, tvec_b_to_a, out_dir)
|
||
|
|
rms_report = {"RMS_B_to_A_pixels": rms1_pixels,
|
||
|
|
"RMS_B_to_A_radians": rms1_rad,
|
||
|
|
"RMS_A_to_B_pixels": rms2_pixels,
|
||
|
|
"RMS_A_to_B_radians": rms2_rad}
|
||
|
|
write_json(os.path.join(out_dir, "report.json"), rms_report)
|
||
|
|
|
||
|
|
return rmat_b_to_a, tvec_b_to_a, rms1_pixels, rms1_rad, rms2_pixels, rms2_rad
|