def save_data(self, dir_name: str, file_prefix: str):
"""
Saves calibration parameters to a directory.
:param dir_name: directory to save to
:param file_prefix: prefix for all files
"""
if not os.path.isdir(dir_name):
os.makedirs(dir_name)
intrinsics_file = sksio.get_intrinsics_file_name(dir_name, file_prefix)
np.savetxt(intrinsics_file, self.camera_matrix, fmt='%.8f')
dist_coeff_file = sksio.get_distortion_file_name(dir_name, file_prefix)
np.savetxt(dist_coeff_file, self.dist_coeffs, fmt='%.8f')
handeye_file = sksio.get_handeye_file_name(dir_name, file_prefix)
np.savetxt(handeye_file, self.handeye_matrix, fmt='%.8f')
p2m_file = sksio.get_pattern2marker_file_name(dir_name, file_prefix)
np.savetxt(p2m_file, self.pattern2marker_matrix, fmt='%.8f')
for i in enumerate(self.rvecs):
extrinsics_file = sksio.get_extrinsics_file_name(
dir_name, file_prefix, i[0])
extrinsics = sksu.extrinsic_vecs_to_matrix(self.rvecs[i[0]],
self.tvecs[i[0]])
np.savetxt(extrinsics_file, extrinsics, fmt='%.8f')
def compute_mono_3d_err(ids, object_points, image_points, rvecs, tvecs,
camera_matrix, distortion):
"""
Function to compute mono RMS reconstruction error over multiple views.
Here, to triangulate, we take the i^th camera as left camera, and
the i+1^th camera as the right camera, compute l2r, and triangulate.
:param ids: Vector of ndarray of integer point ids
:param object_points: Vector of Vector of 1x3 of type float32
:param image_points: Vector of Vector of 1x2 of type float32
:param rvecs: Vector of [3x1] ndarray, Rodrigues rotations for each camera
:param tvecs: Vector of [3x1] ndarray, translations for each camera
:param camera_matrix: [3x3] ndarray
:param distortion: [1x5] ndarray
:return: SSE re-reprojection error, number_samples
"""
sse = 0
number_of_samples = 0
number_of_frames = len(object_points)
# We are going to triangulate between a frame and the next frame.
for i in range(0, number_of_frames):
j = i + 1
if j == number_of_frames:
j = 0
_, common_object_points, common_left_image_points, \
common_right_image_points = \
vu.filter_common_points_per_image(ids[i],
object_points[i],
image_points[i],
ids[j],
image_points[j],
10)
left_camera_to_world = vu.extrinsic_vecs_to_matrix(rvecs[i], tvecs[i])
right_camera_to_world = vu.extrinsic_vecs_to_matrix(rvecs[j], tvecs[j])
left_to_right = np.matmul(right_camera_to_world,
np.linalg.inv(left_camera_to_world))
l2r_rmat = left_to_right[0:3, 0:3]
l2r_tvec = left_to_right[0:3, 3]
c_obj = [common_object_points]
c_li = [common_left_image_points]
c_ri = [common_right_image_points]
r_v = [rvecs[i]]
t_v = [tvecs[i]]
err, samp = compute_stereo_3d_error(l2r_rmat, l2r_tvec, c_obj, c_li,
camera_matrix, distortion, c_ri,
camera_matrix, distortion, r_v,
t_v)
sse = sse + err
number_of_samples = number_of_samples + samp
LOGGER.debug("Mono RMS reconstruction: sse=%s, num=%s", str(sse),
str(number_of_samples))
return sse, number_of_samples
def compute_stereo_2d_err(l2r_rmat,
l2r_tvec,
left_object_points,
left_image_points,
left_camera_matrix,
left_distortion,
right_object_points,
right_image_points,
right_camera_matrix,
right_distortion,
left_rvecs,
left_tvecs,
return_residuals=False):
"""
Function to compute stereo re-projection error, over multiple views.
:param l2r_rmat: [3x3] ndarray, rotation for l2r transform
:param l2r_tvec: [3x1] ndarray, translation for l2r transform
:param left_object_points: Vector of Vector of 1x3 of type float32
:param left_image_points: Vector of Vector of 1x2 of type float32
:param left_camera_matrix: [3x3] ndarray
:param left_distortion: [1x5] ndarray
:param right_object_points: Vector of Vector of 1x3 of type float32
:param right_image_points: Vector of Vector of 1x2 of type float32
:param right_camera_matrix: [3x3] ndarray
:param right_distortion: [1x5] ndarray
:param left_rvecs: Vector of [3x1] ndarray, Rodrigues rotations, left camera
:param left_tvecs: Vector of [3x1] ndarray, translations, left camera
:param return_residuals: if True returns vector of residuals for LM,
otherwise, returns SSE.
:return: re-reprojection error, number_samples
"""
left_to_right = mu.construct_rigid_transformation(l2r_rmat, l2r_tvec)
lse = 0
rse = 0
residuals = []
number_of_samples = 0
number_of_frames = len(left_object_points)
for i in range(0, number_of_frames):
projected_left, _ = cv2.projectPoints(left_object_points[i],
left_rvecs[i], left_tvecs[i],
left_camera_matrix,
left_distortion)
world_to_left_camera = vu.extrinsic_vecs_to_matrix(
left_rvecs[i], left_tvecs[i])
world_to_right_camera = np.matmul(left_to_right, world_to_left_camera)
rvec, tvec = vu.extrinsic_matrix_to_vecs(world_to_right_camera)
projected_right, _ = cv2.projectPoints(right_object_points[i], rvec,
tvec, right_camera_matrix,
right_distortion)
number_of_samples = number_of_samples \
+ len(left_image_points[i]) \
+ len(right_image_points[i])
diff_left = left_image_points[i] - projected_left
diff_right = right_image_points[i] - projected_right
if return_residuals:
residuals.append(diff_left.reshape((-1)))
residuals.append(diff_right.reshape((-1)))
else:
lse = lse + np.sum(np.square(diff_left))
rse = rse + np.sum(np.square(diff_right))
if return_residuals:
return np.hstack(residuals)
return lse + rse, number_of_samples
def stereo_video_calibration_expt(left_ids, left_object_points,
left_image_points, right_ids,
right_object_points, right_image_points,
image_size):
"""
Experimental.
:param left_ids: Vector of ndarrays containing integer point ids.
:param left_object_points: Vector of Vectors of 1x3 object points, float32
:param left_image_points: Vector of Vectors of 1x2 object points, float32
:param right_ids: Vector of ndarrays containing integer point ids.
:param right_object_points: Vector of Vectors of 1x3 object points, float32
:param right_image_points: Vector of Vectors of 1x2 object points, float32
:param image_size: (x, y) tuple, size in pixels, e.g. (1920, 1080)
:return:
"""
# First do standard OpenCV calibration, using the wrapper in this project.
s_reproj, s_recon, \
l_c, l_d, l_rvecs, l_tvecs, \
r_c, r_d, r_rvecs, r_tvecs, \
l2r_r, l2r_t, \
essential, fundamental = \
vc.stereo_video_calibration(left_ids,
left_object_points,
left_image_points,
right_ids,
right_object_points,
right_image_points,
image_size)
# For stereo reconstruction error we need common points.
_, common_object_points, common_left_image_points, \
common_right_image_points \
= vu.filter_common_points_all_images(left_ids,
left_object_points,
left_image_points,
right_ids,
right_image_points, 10)
l2r_rvec = (cv2.Rodrigues(l2r_r.T))[0]
# Now compute a set of left extrinsics, where the parameters
# are all relative to the first camera.
camera_rvecs = copy.deepcopy(l_rvecs)
camera_tvecs = copy.deepcopy(l_tvecs)
number_of_frames = len(left_object_points)
first_world_to_camera = \
vu.extrinsic_vecs_to_matrix(camera_rvecs[0], camera_tvecs[0])
first_camera_to_world = np.linalg.inv(first_world_to_camera)
for i in range(1, number_of_frames):
extrinsic = \
vu.extrinsic_vecs_to_matrix(camera_rvecs[i], camera_tvecs[i])
relative_to_first = np.matmul(extrinsic, first_camera_to_world)
camera_rvecs[i], camera_tvecs[i] = \
vu.extrinsic_matrix_to_vecs(relative_to_first)
# Now we have to create a flat vector of parameters to optimise.
# Optimsing l2r_r (3), l2r_t(3), l_c (4), r_c (4) + 6 DOF per camera.
number_of_parameters = 3 + 3 + 4 + 4 + (number_of_frames * 6)
x_0 = np.zeros(number_of_parameters)
x_0[0] = l2r_rvec[0][0]
x_0[1] = l2r_rvec[1][0]
x_0[2] = l2r_rvec[2][0]
x_0[3] = l2r_t[0][0]
x_0[4] = l2r_t[1][0]
x_0[5] = l2r_t[2][0]
x_0[6] = l_c[0][0]
x_0[7] = l_c[1][1]
x_0[8] = l_c[0][2]
x_0[9] = l_c[1][2]
x_0[10] = r_c[0][0]
x_0[11] = r_c[1][1]
x_0[12] = r_c[0][2]
x_0[13] = r_c[1][2]
for i in range(0, number_of_frames):
x_0[14 + i * 6 + 0] = camera_rvecs[i][0]
x_0[14 + i * 6 + 1] = camera_rvecs[i][1]
x_0[14 + i * 6 + 2] = camera_rvecs[i][2]
x_0[14 + i * 6 + 3] = camera_tvecs[i][0]
x_0[14 + i * 6 + 4] = camera_tvecs[i][1]
x_0[14 + i * 6 + 5] = camera_tvecs[i][2]
res = minimize(vcf.stereo_2d_and_3d_error,
x_0,
args=(common_object_points, common_left_image_points, l_d,
common_object_points, common_right_image_points, r_d),
method='Powell',
tol=1e-4,
options={
'disp': False,
'maxiter': 100000
})
LOGGER.info("Stereo Re-Calibration: success=%s", str(res.success))
LOGGER.info("Stereo Re-Calibration: msg=%s", str(res.message))
# Now need to unpack the results, into the same set of vectors,
# as the stereo_video_calibration, so they are drop-in replacements.
x_1 = res.x
l2r_rvec[0][0] = x_1[0]
l2r_rvec[1][0] = x_1[1]
l2r_rvec[2][0] = x_1[2]
l2r_r = (cv2.Rodrigues(l2r_rvec))[0]
l2r_t[0][0] = x_1[3]
l2r_t[1][0] = x_1[4]
l2r_t[2][0] = x_1[5]
l_c[0][0] = x_1[6]
l_c[1][1] = x_1[7]
l_c[0][2] = x_1[8]
l_c[1][2] = x_1[9]
r_c[0][0] = x_1[10]
r_c[1][1] = x_1[11]
r_c[0][2] = x_1[12]
r_c[1][2] = x_1[13]
for i in range(0, number_of_frames):
camera_rvecs[i][0] = x_1[14 + i * 6 + 0]
camera_rvecs[i][1] = x_1[14 + i * 6 + 1]
camera_rvecs[i][2] = x_1[14 + i * 6 + 2]
camera_tvecs[i][0] = x_1[14 + i * 6 + 3]
camera_tvecs[i][1] = x_1[14 + i * 6 + 4]
camera_tvecs[i][2] = x_1[14 + i * 6 + 5]
# Still need to convert these parameters (which are relative to first
# camera), into consistent world-to-camera rvecs and tvecs for left/right.
first_world_to_camera = \
vu.extrinsic_vecs_to_matrix(camera_rvecs[0], camera_tvecs[0])
left_to_right = skcm.construct_rigid_transformation(l2r_r, l2r_t)
for i in range(0, number_of_frames):
if i == 0:
left_world_to_camera = first_world_to_camera
else:
extrinsic = \
vu.extrinsic_vecs_to_matrix(camera_rvecs[i], camera_tvecs[i])
left_world_to_camera = np.matmul(extrinsic, first_world_to_camera)
l_rvecs[i], l_tvecs[i] = \
vu.extrinsic_matrix_to_vecs(left_world_to_camera)
right_world_to_camera = \
np.matmul(left_to_right, left_world_to_camera)
r_rvecs[i], r_tvecs[i] = \
vu.extrinsic_matrix_to_vecs(right_world_to_camera)
# And recompute stereo RMS re-projection error.
s_reproj_2 = \
vm.compute_stereo_2d_err(l2r_r,
l2r_t,
common_object_points,
common_left_image_points,
l_c,
l_d,
common_object_points,
common_right_image_points,
r_c,
r_d,
l_rvecs,
l_tvecs
)
s_recon_2 = \
vm.compute_stereo_3d_error(l2r_r,
l2r_t,
common_object_points,
common_left_image_points,
l_c,
l_d,
common_right_image_points,
r_c,
r_d,
l_rvecs,
l_tvecs
)
LOGGER.info(
"Stereo Re-Calib: reproj_1=%s, recon_1=%s, reproj_2=%s, recon_2=%s",
str(s_reproj), str(s_recon), str(s_reproj_2), str(s_recon_2))
return s_reproj_2, s_recon_2, \
l_c, l_d, l_rvecs, l_tvecs, \
r_c, r_d, r_rvecs, r_tvecs, \
l2r_r, l2r_t, \
essential, fundamental
def stereo_video_calibration(left_ids,
left_object_points,
left_image_points,
right_ids,
right_object_points,
right_image_points,
image_size,
flags=cv2.CALIB_USE_INTRINSIC_GUESS,
override_left_intrinsics=None,
override_left_distortion=None,
override_right_intrinsics=None,
override_right_distortion=None,
override_l2r_rmat=None,
override_l2r_tvec=None):
"""
Default stereo calibration, using OpenCV methods.
We wrap it here, so we have a place to add extra validation code,
and a space for documentation. The aim is to check everything before
we pass it to OpenCV, and raise Exceptions consistently for any error
we can detect before we pass it to OpenCV.
:param left_ids: Vector of ndarrays containing integer point ids.
:param left_object_points: Vector of Vectors of 1x3 object points, float32
:param left_image_points: Vector of Vectors of 1x2 object points, float32
:param right_ids: Vector of ndarrays containing integer point ids.
:param right_object_points: Vector of Vectors of 1x3 object points, float32
:param right_image_points: Vector of Vectors of 1x2 object points, float32
:param image_size: (x, y) tuple, size in pixels, e.g. (1920, 1080)
:param flags: OpenCV flags to pass to calibrateCamera().
:return:
"""
# We only do override if all override params are specified.
# pylint:disable=too-many-boolean-expressions
do_override = False
if override_left_intrinsics is not None \
and override_left_distortion is not None \
and override_right_intrinsics is not None \
and override_right_distortion is not None \
and override_l2r_rmat is not None \
and override_l2r_tvec is not None:
do_override = True
l_c = override_left_intrinsics
l_d = override_left_distortion
r_c = override_right_intrinsics
r_d = override_right_distortion
number_of_frames = len(left_object_points)
l_rvecs = []
l_tvecs = []
r_rvecs = []
r_tvecs = []
if do_override:
for i in range(0, number_of_frames):
_, rvecs, tvecs = cv2.solvePnP(left_object_points[i],
left_image_points[i], l_c, l_d)
l_rvecs.append(rvecs)
l_tvecs.append(tvecs)
_, rvecs, tvecs = cv2.solvePnP(right_object_points[i],
right_image_points[i], r_c, r_d)
r_rvecs.append(rvecs)
r_tvecs.append(tvecs)
else:
_, l_c, l_d, l_rvecs, l_tvecs \
= cv2.calibrateCamera(left_object_points,
left_image_points,
image_size,
None, None)
_, r_c, r_d, r_rvecs, r_tvecs \
= cv2.calibrateCamera(right_object_points,
right_image_points,
image_size,
None, None)
# For stereo, OpenCV needs common points.
_, common_object_points, common_left_image_points, \
common_right_image_points \
= vu.filter_common_points_all_images(left_ids,
left_object_points,
left_image_points,
right_ids,
right_image_points, 10)
if do_override:
# Do OpenCV stereo calibration, using override intrinsics,
# just so we can get the essential and fundamental matrix out.
_, l_c, l_d, r_c, r_d, \
l2r_r, l2r_t, essential, fundamental = cv2.stereoCalibrate(
common_object_points,
common_left_image_points,
common_right_image_points,
l_c,
l_d,
r_c,
r_d,
image_size,
flags=cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_INTRINSIC)
l2r_r = override_l2r_rmat
l2r_t = override_l2r_tvec
assert np.allclose(l_c, override_left_intrinsics)
assert np.allclose(l_d, override_left_distortion)
assert np.allclose(r_c, override_right_intrinsics)
assert np.allclose(r_d, override_right_distortion)
else:
# Do OpenCV stereo calibration, using intrinsics from OpenCV mono.
_, l_c, l_d, r_c, r_d, \
l2r_r, l2r_t, essential, fundamental = cv2.stereoCalibrate(
common_object_points,
common_left_image_points,
common_right_image_points,
l_c,
l_d,
r_c,
r_d,
image_size,
flags=cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_FIX_INTRINSIC)
# Then do it again, using the passed in flags.
_, l_c, l_d, r_c, r_d, \
l2r_r, l2r_t, essential, fundamental = cv2.stereoCalibrate(
common_object_points,
common_left_image_points,
common_right_image_points,
l_c,
l_d,
r_c,
r_d,
image_size,
flags=flags)
if do_override:
# Stereo calibration is hard for a laparoscope.
# In clinical practice, the data may be way too variable.
# For stereo scopes, they are often fixed focus,
# i.e. fixed intrinsics, and fixed stereo.
# So, we may prefer to just do the best possible calibration
# in the lab, and then keep those values constant.
# But we then would still want to optimise the camera extrinsics
# as the camera poses directly affect the hand-eye calibration.
_, l_rvecs, l_tvecs, \
= stereo_calibration_extrinsics(
common_object_points,
common_left_image_points,
common_right_image_points,
l_rvecs,
l_tvecs,
l_c,
l_d,
r_c,
r_d,
l2r_r,
l2r_t
)
else:
# Normal OpenCV stereo calibration optimises intrinsics,
# distortion, and stereo parameters, but doesn't output pose.
# So here, we recompute the left camera pose.
for i in range(0, number_of_frames):
_, l_rvecs[i], l_tvecs[i] = cv2.solvePnP(
common_object_points[i], common_left_image_points[i], l_c, l_d)
# Here, we are computing the right hand side rvecs and tvecs
# given the new left hand side rvecs, tvecs and the l2r.
left_to_right = skcm.construct_rigid_transformation(l2r_r, l2r_t)
for i in range(0, number_of_frames):
left_chessboard_to_camera = \
vu.extrinsic_vecs_to_matrix(l_rvecs[i], l_tvecs[i])
right_chessboard_to_camera = \
np.matmul(left_to_right, left_chessboard_to_camera)
r_rvecs[i], r_tvecs[i] = \
vu.extrinsic_matrix_to_vecs(right_chessboard_to_camera)
# And recompute stereo projection error, given left camera and l2r.
# We also use all points, not just common points, for comparison
# with other methods outside of this function.
sse, num_samples = \
vm.compute_stereo_2d_err(l2r_r,
l2r_t,
common_object_points,
common_left_image_points,
l_c,
l_d,
common_object_points,
common_right_image_points,
r_c,
r_d,
l_rvecs,
l_tvecs
)
mse = sse / num_samples
s_reproj = np.sqrt(mse)
sse, num_samples = \
vm.compute_stereo_3d_error(l2r_r,
l2r_t,
common_object_points,
common_left_image_points,
l_c,
l_d,
common_right_image_points,
r_c,
r_d,
l_rvecs,
l_tvecs
)
mse = sse / num_samples
s_recon = np.sqrt(mse)
LOGGER.info("Stereo Calib: proj=%s, recon=%s", str(s_reproj), str(s_recon))
return s_reproj, s_recon, \
l_c, l_d, l_rvecs, l_tvecs, \
r_c, r_d, r_rvecs, r_tvecs, \
l2r_r, l2r_t, \
essential, fundamental
def stereo_handeye_calibration(
l2r_rmat: np.ndarray, l2r_tvec: np.ndarray, left_ids: List,
left_object_points: List, left_image_points: List, right_ids: List,
right_image_points: List, left_camera_matrix: np.ndarray,
left_camera_distortion: np.ndarray, right_camera_matrix: np.ndarray,
right_camera_distortion: np.ndarray, device_tracking_array: List,
calibration_tracking_array: List, left_rvecs: List[np.ndarray],
left_tvecs: List[np.ndarray], right_rvecs: List[np.ndarray],
right_tvecs: List[np.ndarray], quat_model2hand_array: List,
trans_model2hand_array: List):
"""Wrapper around handeye calibration functions and reprojection /
reconstruction error metrics.
:param l2r_rmat: [3x3] ndarray, rotation for l2r transform
:type l2r_rmat: np.ndarray
:param l2r_tvec: [3x1] ndarray, translation for l2r transform
:type l2r_tvec: np.ndarray
:param left_ids: Vector of ndarrays containing integer point ids.
:type left_ids: List
:param left_object_points: Vector of Vector of 1x3 of type float32
:type left_object_points: List
:param left_image_points: Vector of Vector of 1x2 of type float32
:type left_image_points: List
:param right_ids: Vector of ndarrays containing integer point ids.
:type right_ids: List
:param right_image_points: Vector of Vector of 1x3 of type float32
:type right_image_points: List
:param left_camera_matrix: Camera intrinsic matrix
:type left_camera_matrix: np.ndarray
:param left_camera_distortion: Camera distortion coefficients
:type left_camera_distortion: np.ndarray
:param right_camera_matrix: Camera intrinsic matrix
:type right_camera_matrix: np.ndarray
:param right_camera_distortion: Camera distortion coefficients
:type right_camera_distortion: np.ndarray
:param device_tracking_array: Tracking data for camera (hand)
:type device_tracking_array: List
:param calibration_tracking_array: Tracking data for calibration target
:type calibration_tracking_array: List
:param left_rvecs: Vector of 3x1 ndarray, Rodrigues rotations for each
camera
:type left_rvecs: List[np.ndarray]
:param left_tvecs: Vector of [3x1] ndarray, translations for each camera
:type left_tvecs: List[np.ndarray]
:param right_rvecs: Vector of 3x1 ndarray, Rodrigues rotations for each
camera
:type right_rvecs: List[np.ndarray]
:param right_tvecs: Vector of [3x1] ndarray, translations for each camera
:type right_tvecs: List[np.ndarray]
:param quat_model2hand_array: Array of model to hand quaternions
:type quat_model2hand_array: List
:param trans_model2hand_array: Array of model to hand translaions
:type trans_model2hand_array: List
:return: Reprojection error, reconstruction error, left handeye matrix,
left pattern to marker matrix, right handeye, right pattern to marker
:rtype: float, float, np.ndarray, np.ndarray, np.ndarray, np.ndarray
"""
# Do calibration, using Guofang's method on each left/right channel.
left_handeye_matrix, left_pattern2marker_matrix = \
he.handeye_calibration(left_rvecs, left_tvecs, quat_model2hand_array,
trans_model2hand_array)
right_handeye_matrix, right_pattern2marker_matrix = \
he.handeye_calibration(right_rvecs, right_tvecs, quat_model2hand_array,
trans_model2hand_array)
# Filter common image points
minimum_points = 10
_, common_object_pts, common_l_image_pts, common_r_image_pts = \
vu.filter_common_points_all_images(
left_ids, left_object_points, left_image_points,
right_ids, right_image_points,
minimum_points)
# Now try another optimisation.
# Have one version of the pattern2marker matrix and handeye matrix,
# and also optimise the tracking information to match.
number_of_frames = len(common_object_pts)
number_of_parameters = (number_of_frames * 6) + 12
x_0 = np.zeros(number_of_parameters)
for i in range(0, number_of_frames):
m2h = np.linalg.inv(device_tracking_array[i]) \
@ calibration_tracking_array[i]
m2h_rvec, m2h_tvec = vu.extrinsic_matrix_to_vecs(m2h)
x_0[i * 6 + 0] = m2h_rvec[0][0]
x_0[i * 6 + 1] = m2h_rvec[1][0]
x_0[i * 6 + 2] = m2h_rvec[2][0]
x_0[i * 6 + 3] = m2h_tvec[0][0]
x_0[i * 6 + 4] = m2h_tvec[1][0]
x_0[i * 6 + 5] = m2h_tvec[2][0]
p2m_rvec, p2m_tvec = vu.extrinsic_matrix_to_vecs(
left_pattern2marker_matrix)
x_0[number_of_frames * 6 + 0] = p2m_rvec[0][0]
x_0[number_of_frames * 6 + 1] = p2m_rvec[1][0]
x_0[number_of_frames * 6 + 2] = p2m_rvec[2][0]
x_0[number_of_frames * 6 + 3] = p2m_tvec[0][0]
x_0[number_of_frames * 6 + 4] = p2m_tvec[1][0]
x_0[number_of_frames * 6 + 5] = p2m_tvec[2][0]
h2e_rvec, h2e_tvec = vu.extrinsic_matrix_to_vecs(left_handeye_matrix)
x_0[(number_of_frames + 1) * 6 + 0] = h2e_rvec[0][0]
x_0[(number_of_frames + 1) * 6 + 1] = h2e_rvec[1][0]
x_0[(number_of_frames + 1) * 6 + 2] = h2e_rvec[2][0]
x_0[(number_of_frames + 1) * 6 + 3] = h2e_tvec[0][0]
x_0[(number_of_frames + 1) * 6 + 4] = h2e_tvec[1][0]
x_0[(number_of_frames + 1) * 6 + 5] = h2e_tvec[2][0]
res = least_squares(vcf.stereo_handeye_error,
x_0,
args=(common_object_pts, common_l_image_pts,
common_r_image_pts, left_camera_matrix,
left_camera_distortion, right_camera_matrix,
right_camera_distortion, l2r_rmat, l2r_tvec),
method='lm',
x_scale='jac',
verbose=0)
LOGGER.info("Stereo Handeye Re-Calibration: status=%s", str(res.status))
LOGGER.info("Stereo Handeye Re-Calibration: success=%s", str(res.success))
LOGGER.info("Stereo Handeye Re-Calibration: msg=%s", str(res.message))
# Extract data from result object.
x_1 = res.x
tmp_rvec = np.zeros((3, 1))
tmp_rvec[0][0] = x_1[number_of_frames * 6 + 0]
tmp_rvec[1][0] = x_1[number_of_frames * 6 + 1]
tmp_rvec[2][0] = x_1[number_of_frames * 6 + 2]
tmp_tvec = np.zeros((3, 1))
tmp_tvec[0][0] = x_1[number_of_frames * 6 + 3]
tmp_tvec[1][0] = x_1[number_of_frames * 6 + 4]
tmp_tvec[2][0] = x_1[number_of_frames * 6 + 5]
left_pattern2marker_matrix = vu.extrinsic_vecs_to_matrix(
tmp_rvec, tmp_tvec)
tmp_rvec[0][0] = x_1[(number_of_frames + 1) * 6 + 0]
tmp_rvec[1][0] = x_1[(number_of_frames + 1) * 6 + 1]
tmp_rvec[2][0] = x_1[(number_of_frames + 1) * 6 + 2]
tmp_tvec[0][0] = x_1[(number_of_frames + 1) * 6 + 3]
tmp_tvec[1][0] = x_1[(number_of_frames + 1) * 6 + 4]
tmp_tvec[2][0] = x_1[(number_of_frames + 1) * 6 + 5]
left_handeye_matrix = vu.extrinsic_vecs_to_matrix(tmp_rvec, tmp_tvec)
l2r_matrix = skcm.construct_rigid_transformation(l2r_rmat, l2r_tvec)
right_handeye_matrix = l2r_matrix @ left_handeye_matrix
right_pattern2marker_matrix = copy.deepcopy(left_pattern2marker_matrix)
# Remember that we have optimised tracking matrices.
# So computation of error statistics should include these new positions.
dummy_dt_array = []
dummy_ct_array = []
for i in range(0, number_of_frames):
tmp_rvec[0][0] = x_1[i * 6 + 0]
tmp_rvec[1][0] = x_1[i * 6 + 1]
tmp_rvec[2][0] = x_1[i * 6 + 2]
tmp_tvec[0][0] = x_1[i * 6 + 3]
tmp_tvec[1][0] = x_1[i * 6 + 4]
tmp_tvec[2][0] = x_1[i * 6 + 5]
calib_track_mat = vu.extrinsic_vecs_to_matrix(tmp_rvec, tmp_tvec)
device_track_mat = np.eye(4)
dummy_dt_array.append(device_track_mat)
dummy_ct_array.append(calib_track_mat)
# Now compute some output statistics.
sse, num_samples = vm.compute_stereo_2d_err_handeye(
common_object_pts, common_l_image_pts, left_camera_matrix,
left_camera_distortion, common_r_image_pts, right_camera_matrix,
right_camera_distortion, dummy_dt_array, dummy_ct_array,
left_handeye_matrix, left_pattern2marker_matrix, right_handeye_matrix,
right_pattern2marker_matrix)
mse = sse / num_samples
reproj_err = np.sqrt(mse)
sse, num_samples = vm.compute_stereo_3d_err_handeye(
l2r_rmat,
l2r_tvec,
common_object_pts,
common_l_image_pts,
left_camera_matrix,
left_camera_distortion,
common_r_image_pts,
right_camera_matrix,
right_camera_distortion,
dummy_dt_array,
dummy_ct_array,
left_handeye_matrix,
left_pattern2marker_matrix,
)
mse = sse / num_samples
recon_err = np.sqrt(mse)
return reproj_err, recon_err, \
left_handeye_matrix, left_pattern2marker_matrix, \
right_handeye_matrix, right_pattern2marker_matrix
def stereo_2d_and_3d_error(x_0, left_object_points, left_image_points,
left_distortion, right_object_points,
right_image_points, right_distortion):
"""
Private method to compute RMSE cost function, where x_0 contains
the l2r rvec, l2r tvec, left intrinsics, right intrinsics, and
left camera extrinsics.
"""
l2r_rvec = np.zeros((3, 1))
l2r_rvec[0][0] = x_0[0]
l2r_rvec[1][0] = x_0[1]
l2r_rvec[2][0] = x_0[2]
l2r_rmat = (cv2.Rodrigues(l2r_rvec))[0]
l2r_tvec = np.zeros((3, 1))
l2r_tvec[0][0] = x_0[3]
l2r_tvec[1][0] = x_0[4]
l2r_tvec[2][0] = x_0[5]
left_camera = np.eye(3)
left_camera[0][0] = x_0[6]
left_camera[1][1] = x_0[7]
left_camera[0][2] = x_0[8]
left_camera[1][2] = x_0[9]
right_camera = np.eye(3)
right_camera[0][0] = x_0[10]
right_camera[1][1] = x_0[11]
right_camera[0][2] = x_0[12]
right_camera[1][2] = x_0[13]
sse = 0
number_of_samples = 0
number_of_frames = len(left_object_points)
rvecs = []
tvecs = []
for i in range(0, number_of_frames):
rvec = np.zeros((3, 1))
rvec[0][0] = x_0[14 + 6 * i + 0]
rvec[1][0] = x_0[14 + 6 * i + 1]
rvec[2][0] = x_0[14 + 6 * i + 2]
tvec = np.zeros((3, 1))
tvec[0][0] = x_0[14 + 6 * i + 3]
tvec[1][0] = x_0[14 + 6 * i + 4]
tvec[2][0] = x_0[14 + 6 * i + 5]
rvecs.append(rvec)
tvecs.append(tvec)
for i in range(0, number_of_frames):
world_to_left_camera = vu.extrinsic_vecs_to_matrix(rvecs[0], tvecs[0])
if i > 0:
offset = vu.extrinsic_vecs_to_matrix(rvecs[i], tvecs[i])
world_to_left_camera = np.matmul(offset, world_to_left_camera)
rvec, tvec = vu.extrinsic_matrix_to_vecs(world_to_left_camera)
rvecs[i] = rvec
tvecs[i] = tvec
tmp_sse, tmp_num = vm.compute_stereo_2d_err(
l2r_rmat, l2r_tvec, left_object_points, left_image_points, left_camera,
left_distortion, right_object_points, right_image_points, right_camera,
right_distortion, rvecs, tvecs)
sse = sse + tmp_sse
number_of_samples = number_of_samples + tmp_num
tmp_sse, tmp_num = \
vm.compute_stereo_3d_error(l2r_rmat,
l2r_tvec,
left_object_points,
left_image_points,
left_camera,
left_distortion,
right_image_points,
right_camera,
right_distortion,
rvecs,
tvecs
)
sse = sse + tmp_sse
number_of_samples = number_of_samples + tmp_num
mse = sse / number_of_samples
rmse = np.sqrt(mse)
return rmse
def stereo_handeye_error(x_0, common_object_points, common_left_image_points,
common_right_image_points, left_intrinsics,
left_distortion, right_intrinsics, right_distortion,
l2r_rmat, l2r_tvec):
"""
Cost function to return residual error. x_0 should contain an array
of combined chessboard-marker-to-device-marker tracking 6DOF (rvec, tvec),
chessboard-pattern-to-marker 6DOF and the device-hand-to-eye matrix 6DOF.
:param x_0:
:param common_object_points:
:param common_left_image_points:
:param common_right_image_points:
:param left_intrinsics:
:param left_distortion:
:param right_intrinsics:
:param right_distortion:
:param l2r_rmat:
:param l2r_tvec:
:return: matrix of residuals for Levenberg-Marquardt optimisation.
"""
rvecs = []
tvecs = []
number_of_frames = len(common_object_points)
p2m_rvec = np.zeros((3, 1))
p2m_rvec[0][0] = x_0[6 * number_of_frames + 0]
p2m_rvec[1][0] = x_0[6 * number_of_frames + 1]
p2m_rvec[2][0] = x_0[6 * number_of_frames + 2]
p2m_tvec = np.zeros((3, 1))
p2m_tvec[0][0] = x_0[6 * number_of_frames + 3]
p2m_tvec[1][0] = x_0[6 * number_of_frames + 4]
p2m_tvec[2][0] = x_0[6 * number_of_frames + 5]
h2e_rvec = np.zeros((3, 1))
h2e_rvec[0][0] = x_0[6 * (number_of_frames + 1) + 0]
h2e_rvec[1][0] = x_0[6 * (number_of_frames + 1) + 1]
h2e_rvec[2][0] = x_0[6 * (number_of_frames + 1) + 2]
h2e_tvec = np.zeros((3, 1))
h2e_tvec[0][0] = x_0[6 * (number_of_frames + 1) + 3]
h2e_tvec[1][0] = x_0[6 * (number_of_frames + 1) + 4]
h2e_tvec[2][0] = x_0[6 * (number_of_frames + 1) + 5]
p2m = vu.extrinsic_vecs_to_matrix(p2m_rvec, p2m_tvec)
h2e = vu.extrinsic_vecs_to_matrix(h2e_rvec, h2e_tvec)
for i in range(0, number_of_frames):
m2h_rvec = np.zeros((3, 1))
m2h_rvec[0][0] = x_0[6 * i + 0]
m2h_rvec[1][0] = x_0[6 * i + 1]
m2h_rvec[2][0] = x_0[6 * i + 2]
m2h_tvec = np.zeros((3, 1))
m2h_tvec[0][0] = x_0[6 * i + 3]
m2h_tvec[1][0] = x_0[6 * i + 4]
m2h_tvec[2][0] = x_0[6 * i + 5]
m2h = vu.extrinsic_vecs_to_matrix(m2h_rvec, m2h_tvec)
extrinsics = h2e @ m2h @ p2m
rvec, tvec = vu.extrinsic_matrix_to_vecs(extrinsics)
rvecs.append(rvec)
tvecs.append(tvec)
residual = vm.compute_stereo_2d_err(l2r_rmat,
l2r_tvec,
common_object_points,
common_left_image_points,
left_intrinsics,
left_distortion,
common_object_points,
common_right_image_points,
right_intrinsics,
right_distortion,
rvecs,
tvecs,
return_residuals=True)
return residual