diff --git a/requirements.txt b/requirements.txt index 8b3e7ad..8f736a5 100644 --- a/requirements.txt +++ b/requirements.txt @@ -8,5 +8,5 @@ nbsphinx scipy opencv-contrib-python scikit-surgerycore -scikit-surgeryimage +scikit-surgeryimage>=0.7.4 scikit-surgeryopencvcpp \ No newline at end of file diff --git a/setup.py b/setup.py index 4c1f0a7..2253901 100644 --- a/setup.py +++ b/setup.py @@ -57,7 +57,7 @@ 'scipy', 'opencv-contrib-python', 'scikit-surgerycore', - 'scikit-surgeryimage', + 'scikit-surgeryimage>=0.7.4', 'scikit-surgeryopencvcpp', ], diff --git a/sksurgerycalibration/video/video_calibration_cost_functions.py b/sksurgerycalibration/video/video_calibration_cost_functions.py index 3f667eb..55c238d 100644 --- a/sksurgerycalibration/video/video_calibration_cost_functions.py +++ b/sksurgerycalibration/video/video_calibration_cost_functions.py @@ -20,7 +20,7 @@ def stereo_2d_error(x_0, l2r_tvec ): """ - Private method to RMSE cost function, in stereo, where x_0 contains + Private method to return vector of residuals, in stereo, where x_0 contains the left camera extrinsics. """ rvecs = [] @@ -38,24 +38,22 @@ def stereo_2d_error(x_0, rvecs.append(rvec) tvecs.append(tvec) - tmp_sse, tmp_num = 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 - ) - - mse = tmp_sse / tmp_num - rmse = np.sqrt(mse) - - return rmse + 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 def stereo_2d_and_3d_error(x_0, diff --git a/sksurgerycalibration/video/video_calibration_driver_stereo.py b/sksurgerycalibration/video/video_calibration_driver_stereo.py index c3d3c13..4a34be3 100644 --- a/sksurgerycalibration/video/video_calibration_driver_stereo.py +++ b/sksurgerycalibration/video/video_calibration_driver_stereo.py @@ -166,13 +166,7 @@ def iterative_calibration(self, reference_ids, reference_image_points, reference_image_size, - flags: int = 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 + flags: int = cv2.CALIB_USE_INTRINSIC_GUESS ): """ Does iterative calibration, like Datta 2009. @@ -203,14 +197,7 @@ def iterative_calibration(self, reference_image_size) proj_err, recon_err, param_copy = \ - self.calibrate(flags, - override_left_intrinsics, - override_left_distortion, - override_right_intrinsics, - override_right_distortion, - override_l2r_rmat, - override_l2r_tvec - ) + self.calibrate(flags) LOGGER.info("Iterative calibration: %s: proj_err=%s, recon_err=%s.", str(i), str(proj_err), str(recon_err)) diff --git a/sksurgerycalibration/video/video_calibration_metrics.py b/sksurgerycalibration/video/video_calibration_metrics.py index 75b19ef..a3d7261 100644 --- a/sksurgerycalibration/video/video_calibration_metrics.py +++ b/sksurgerycalibration/video/video_calibration_metrics.py @@ -26,9 +26,10 @@ def compute_stereo_2d_err(l2r_rmat, right_camera_matrix, right_distortion, left_rvecs, - left_tvecs): + left_tvecs, + return_residuals=False): """ - Function to compute stereo SSE re-projection error, over multiple views. + 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 @@ -42,12 +43,15 @@ def compute_stereo_2d_err(l2r_rmat, :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 - :return: SSE re-reprojection error, number_samples + :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) @@ -72,18 +76,23 @@ def compute_stereo_2d_err(l2r_rmat, right_camera_matrix, right_distortion) - diff_left = left_image_points[i] - projected_left - - lse = lse + np.sum(np.square(diff_left)) - - diff_right = right_image_points[i] - projected_right - rse = rse + np.sum(np.square(diff_right)) number_of_samples = number_of_samples \ + len(left_image_points[i]) \ + len(right_image_points[i]) - LOGGER.debug("Stereo RMS reprojection: left sse=%s, right sse=%s, num=%s", - str(lse), str(rse), str(number_of_samples)) + 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 diff --git a/sksurgerycalibration/video/video_calibration_wrapper.py b/sksurgerycalibration/video/video_calibration_wrapper.py index 72c5a95..8c31e9e 100644 --- a/sksurgerycalibration/video/video_calibration_wrapper.py +++ b/sksurgerycalibration/video/video_calibration_wrapper.py @@ -6,7 +6,7 @@ from typing import List import numpy as np import cv2 -from scipy.optimize import minimize +from scipy.optimize import least_squares import sksurgerycore.transforms.matrix as skcm import sksurgerycalibration.video.video_calibration_utils as vu import sksurgerycalibration.video.video_calibration_metrics as vm @@ -111,21 +111,66 @@ def stereo_video_calibration(left_ids, :param flags: OpenCV flags to pass to calibrateCamera(). :return: """ - # Calibrate left, using all available points - l_rms, l_c, l_d, l_rvecs, l_tvecs \ - = cv2.calibrateCamera(left_object_points, - left_image_points, - image_size, - None, None) - # Calibrate right using all available points. - r_rms, r_c, r_d, r_rvecs, r_tvecs \ - = cv2.calibrateCamera(right_object_points, - right_image_points, - image_size, - None, None) + # 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 - # But for stereo, OpenCV needs common points. + 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, @@ -134,72 +179,100 @@ def stereo_video_calibration(left_ids, right_ids, right_image_points, 10) - # First do stereo calibration, using fixed intrinsics. - s_rms, 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. - s_rms, 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: - # pylint:disable=too-many-boolean-expressions - 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 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 fix it. + # 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. - s_reproj, l_rvecs, l_tvecs, \ + + _, l_rvecs, l_tvecs, \ = stereo_calibration_extrinsics( common_object_points, common_left_image_points, common_right_image_points, l_rvecs, l_tvecs, - override_left_intrinsics, - override_left_distortion, - override_right_intrinsics, - override_right_distortion, - override_l2r_rmat, - override_l2r_tvec + l_c, + l_d, + r_c, + r_d, + l2r_r, + l2r_t ) - # And recompute rvecs and tvecs, consistently, given new l2r params. - number_of_frames = len(left_object_points) + 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): - _, l_rvecs[i], l_tvecs[i] = cv2.solvePnP( - common_object_points[i], - common_left_image_points[i], - l_c, - l_d) left_chessboard_to_camera = \ vu.extrinsic_vecs_to_matrix(l_rvecs[i], l_tvecs[i]) right_chessboard_to_camera = \ @@ -244,8 +317,8 @@ def stereo_video_calibration(left_ids, mse = sse / num_samples s_recon = np.sqrt(mse) - LOGGER.info("Stereo Calib: l=%s, r=%s, opencv=%s, proj=%s, recon=%s", - str(l_rms), str(r_rms), str(s_rms), str(s_reproj), str(s_recon)) + 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, \ @@ -470,21 +543,21 @@ def stereo_calibration_extrinsics(common_object_points, x_0[i * 6 + 4] = l_tvecs[i][1] x_0[i * 6 + 5] = l_tvecs[i][2] - res = minimize(vcf.stereo_2d_error, x_0, - args=(common_object_points, - common_left_image_points, - common_right_image_points, - override_left_intrinsics, - override_left_distortion, - override_right_intrinsics, - override_right_distortion, - override_l2r_rmat, - override_l2r_tvec - ), - method='Powell', - tol=1e-2, - options={'disp': False, 'maxiter': 10000}) - + res = least_squares(vcf.stereo_2d_error, x_0, + args=(common_object_points, + common_left_image_points, + common_right_image_points, + override_left_intrinsics, + override_left_distortion, + override_right_intrinsics, + override_right_distortion, + override_l2r_rmat, + override_l2r_tvec), + method='lm', + x_scale='jac', + verbose=0) + + LOGGER.info("Stereo Re-Calibration: status=%s", str(res.status)) LOGGER.info("Stereo Re-Calibration: success=%s", str(res.success)) LOGGER.info("Stereo Re-Calibration: msg=%s", str(res.message)) diff --git a/tests/data/2020_01_20_storz/pattern_4x4_19x26_5_4_with_inset_9x14.png b/tests/data/2020_01_20_storz/pattern_4x4_19x26_5_4_with_inset_9x14.png new file mode 100644 index 0000000..720b2f9 Binary files /dev/null and b/tests/data/2020_01_20_storz/pattern_4x4_19x26_5_4_with_inset_9x14.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/calib.left.distortion.txt b/tests/data/laparoscope_calibration/cbh-viking/calib.left.distortion.txt new file mode 100755 index 0000000..5d33916 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/calib.left.distortion.txt @@ -0,0 +1 @@ +-0.291690 -0.001882 0.007161 -0.000171 0.374519 \ No newline at end of file diff --git a/tests/data/laparoscope_calibration/cbh-viking/calib.left.intrinsics.txt b/tests/data/laparoscope_calibration/cbh-viking/calib.left.intrinsics.txt new file mode 100755 index 0000000..f47de1e --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/calib.left.intrinsics.txt @@ -0,0 +1,3 @@ +1766.276290 0.000000 915.665775 +0.000000 1769.623383 458.985368 +0.000000 0.000000 1.000000 \ No newline at end of file diff --git a/tests/data/laparoscope_calibration/cbh-viking/calib.right.distortion.txt b/tests/data/laparoscope_calibration/cbh-viking/calib.right.distortion.txt new file mode 100755 index 0000000..2b6657f --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/calib.right.distortion.txt @@ -0,0 +1 @@ +-0.291115 0.226945 -0.000367 0.013965 -0.121846 \ No newline at end of file diff --git a/tests/data/laparoscope_calibration/cbh-viking/calib.right.intrinsics.txt b/tests/data/laparoscope_calibration/cbh-viking/calib.right.intrinsics.txt new file mode 100755 index 0000000..e47abdb --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/calib.right.intrinsics.txt @@ -0,0 +1,3 @@ +1792.669020 0.000000 1113.645820 +0.000000 1792.169495 478.497416 +0.000000 0.000000 1.000000 \ No newline at end of file diff --git a/tests/data/laparoscope_calibration/cbh-viking/config-cbh-dots.json b/tests/data/laparoscope_calibration/cbh-viking/config-cbh-dots.json new file mode 100644 index 0000000..2987bf4 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/config-cbh-dots.json @@ -0,0 +1,52 @@ +{ + "tracker": { + "tracker type": "unit test", + "use quaternions": false, + "tracked objects": { + "marker": "config/8700339.rom", + "laparoscope": "config/8700449.rom", + "pointer": "config/8700340.rom" + } + }, + "calibration": { + "general": { + "load previous calibration file": true, + "left intrinsics": "", + "left distortion": "", + "right intrinsics": "", + "right distortion": "", + "l2r": "", + "features": "dots", + "frames required": 5, + "smooth images": false, + "iterative": false, + "maximum reprojection error": 20, + "maximum reconstruction error": 5, + "display good or bad": true, + "required to pass": false + }, + "charuco": { + "minimum points per frame": 50, + "squares": [19, 26], + "square sizes": [5, 4], + "chessboard squares": [9, 14], + "chessboard square size": 3, + "filter markers": false, + "error if no chessboard": true, + "error if no charuco": false + }, + "dots": { + "intrinsic params": "tests/data/calibration/cbh-viking/calib.left.intrinsics.txt", + "distortion coeffs": "tests/data/calibration/cbh-viking/calib.left.distortion.txt", + "minimum points per frame": 36, + "dots": [14, 16], + "separation in mm": 4, + "fiducial indexes": [69, 74, 149, 154], + "reference image size in pixels": [1200, 1360], + "pixels per mm": 80 + } + }, + "recording": { + "output dir": "output/" + } +} \ No newline at end of file diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/left_image.png new file mode 100755 index 0000000..928ada6 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/right_image.png new file mode 100755 index 0000000..fc62e8e Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/tracking_data.txt new file mode 100755 index 0000000..7253347 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_08_32/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982398 -0.164113 -0.089220 72.810715 +-0.008690 0.517266 -0.855780 -263.958832 +0.186595 -0.839942 -0.509588 -1396.689209 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/left_image.png new file mode 100755 index 0000000..595821a Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/right_image.png new file mode 100755 index 0000000..90dc420 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/tracking_data.txt new file mode 100755 index 0000000..e0a6823 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_09_35/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982717 -0.162246 -0.089131 72.787117 +-0.007737 0.517065 -0.855911 -263.931549 +0.184955 -0.840428 -0.509384 -1396.719971 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/left_image.png new file mode 100755 index 0000000..19283d6 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/right_image.png new file mode 100755 index 0000000..684b189 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/tracking_data.txt new file mode 100755 index 0000000..ec75519 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_22/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982798 -0.161738 -0.089161 72.783470 +-0.007532 0.517470 -0.855668 -263.936035 +0.184533 -0.840277 -0.509786 -1396.716064 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/left_image.png new file mode 100755 index 0000000..f1607ef Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/right_image.png new file mode 100755 index 0000000..490886b Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/tracking_data.txt new file mode 100755 index 0000000..77ce890 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_10_45/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982774 -0.161856 -0.089202 72.792496 +-0.007553 0.517448 -0.855681 -263.932983 +0.184655 -0.840268 -0.509757 -1396.728027 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/left_image.png new file mode 100755 index 0000000..3e3bf45 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/right_image.png new file mode 100755 index 0000000..1f35410 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/tracking_data.txt new file mode 100755 index 0000000..0f5014c --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_22/tracking_data.txt @@ -0,0 +1,4 @@ +nan nan nan nan +nan nan nan nan +nan nan nan nan +nan nan nan nan diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/left_image.png new file mode 100755 index 0000000..91fbb5e Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/right_image.png new file mode 100755 index 0000000..c877310 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/tracking_data.txt new file mode 100755 index 0000000..17f8e8a --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_11_56/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982902 -0.161179 -0.089024 72.783546 +-0.007396 0.517651 -0.855560 -263.924164 +0.183982 -0.840273 -0.509992 -1396.731201 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/left_image.png new file mode 100755 index 0000000..aa77fff Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/right_image.png new file mode 100755 index 0000000..c113175 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/tracking_data.txt new file mode 100755 index 0000000..b9b4797 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_04/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982862 -0.161351 -0.089152 72.793602 +-0.007386 0.517705 -0.855527 -263.921356 +0.184194 -0.840207 -0.510024 -1396.717651 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/left_image.png new file mode 100755 index 0000000..633feee Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/right_image.png new file mode 100755 index 0000000..ef8adbd Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/tracking_data.txt new file mode 100755 index 0000000..29daf82 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_24/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982902 -0.161175 -0.089025 72.777924 +-0.007394 0.517654 -0.855558 -263.921570 +0.183979 -0.840272 -0.509995 -1396.695679 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/left_image.png new file mode 100755 index 0000000..f6352aa Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/right_image.png new file mode 100755 index 0000000..be6f952 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/tracking_data.txt new file mode 100755 index 0000000..3787ccb --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_12_44/tracking_data.txt @@ -0,0 +1,4 @@ +-0.982908 -0.161018 -0.089251 72.785416 +-0.007027 0.517258 -0.855801 -263.899078 +0.183965 -0.840546 -0.509548 -1396.661499 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/left_image.png new file mode 100755 index 0000000..1c5a70e Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/right_image.png new file mode 100755 index 0000000..489e920 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/tracking_data.txt new file mode 100755 index 0000000..2663706 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_18/tracking_data.txt @@ -0,0 +1,4 @@ +-0.974732 -0.205281 -0.088079 72.982742 +-0.031747 0.517605 -0.855030 -264.551239 +0.221111 -0.830629 -0.511043 -1395.835815 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/left_image.png new file mode 100755 index 0000000..0fe5968 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/right_image.png new file mode 100755 index 0000000..888c18a Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/tracking_data.txt new file mode 100755 index 0000000..4349e97 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_13_46/tracking_data.txt @@ -0,0 +1,4 @@ +-0.976352 -0.197388 -0.088175 72.941139 +-0.027534 0.518080 -0.854889 -264.480377 +0.214426 -0.832245 -0.511263 -1395.980469 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/left_image.png new file mode 100755 index 0000000..6f3b1db Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/right_image.png new file mode 100755 index 0000000..5235735 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/tracking_data.txt new file mode 100755 index 0000000..0a449cc --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_26/tracking_data.txt @@ -0,0 +1,4 @@ +-0.976963 -0.194236 -0.088409 72.939697 +-0.025577 0.517852 -0.855088 -264.430023 +0.211872 -0.833127 -0.510891 -1396.021362 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/left_image.png new file mode 100755 index 0000000..46c9dc4 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/right_image.png new file mode 100755 index 0000000..2900a2b Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/tracking_data.txt new file mode 100755 index 0000000..e3884f9 --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_14_58/tracking_data.txt @@ -0,0 +1,4 @@ +-0.977317 -0.192506 -0.088278 72.926239 +-0.024757 0.517827 -0.855127 -264.402130 +0.210330 -0.833545 -0.510847 -1396.077759 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/left_image.png new file mode 100755 index 0000000..54184c1 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/right_image.png new file mode 100755 index 0000000..638e31c Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/tracking_data.txt new file mode 100755 index 0000000..3e355fc --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_16_59/tracking_data.txt @@ -0,0 +1,4 @@ +-0.978301 -0.188249 -0.086541 74.033569 +-0.024157 0.518483 -0.854746 -265.486938 +0.205775 -0.834109 -0.511780 -1396.832520 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/left_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/left_image.png new file mode 100755 index 0000000..c132d64 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/left_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/right_image.png b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/right_image.png new file mode 100755 index 0000000..4644713 Binary files /dev/null and b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/right_image.png differ diff --git a/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/tracking_data.txt b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/tracking_data.txt new file mode 100755 index 0000000..c35112c --- /dev/null +++ b/tests/data/laparoscope_calibration/cbh-viking/snapshots-metal-1/14_17_36/tracking_data.txt @@ -0,0 +1,4 @@ +-0.979023 -0.184886 -0.085623 74.031738 +-0.023645 0.520491 -0.853539 -265.775879 +0.202373 -0.833610 -0.513945 -1396.724243 +0.000000 0.000000 0.000000 1.000000 diff --git a/tests/video/test_charuco_plus_chessboard.py b/tests/video/test_charuco_plus_chessboard.py index 94bbcdf..86b0c72 100644 --- a/tests/video/test_charuco_plus_chessboard.py +++ b/tests/video/test_charuco_plus_chessboard.py @@ -31,8 +31,11 @@ def test_stereo_davinci(): right_images.append(image) assert (len(right_images) == 59) + ref_img = cv2.imread('tests/data/2020_01_20_storz/pattern_4x4_19x26_5_4_with_inset_9x14.png') + minimum_number_of_points_per_image = 50 - detector = pd.CharucoPlusChessboardPointDetector(error_if_no_chessboard=False) # Try to accept as many as possible. + detector = pd.CharucoPlusChessboardPointDetector(ref_img, + error_if_no_chessboard=False) # Try to accept as many as possible. calibrator = sc.StereoVideoCalibrationDriver(detector, minimum_number_of_points_per_image) for i, _ in enumerate(left_images): try: diff --git a/tests/video/test_chessboard_calibration.py b/tests/video/test_chessboard_calibration.py index e42cfc4..2fb239e 100644 --- a/tests/video/test_chessboard_calibration.py +++ b/tests/video/test_chessboard_calibration.py @@ -142,11 +142,12 @@ def test_chessboard_stereo(): reproj_err, recon_err, params = calibrator.calibrate() # Just for a regression test, checking reprojection error, and recon error. + print("\nStereo, default=" + str(reproj_err) + ", " + str(recon_err)) assert reproj_err < 0.7 - assert recon_err < 1.8 - print("Stereo, default=" + str(reproj_err) + ", " + str(recon_err)) + assert recon_err < 1.7 - # Now test rerunning where we optimize the extrinsics with other params fixed. + # Test running with fixed intrinsics and fixed stereo, using existing + # calibration parameters, thereby re-optimising the camera poses. reproj_err, recon_err, params = \ calibrator.calibrate( override_left_intrinsics=params.left_params.camera_matrix, @@ -157,9 +158,10 @@ def test_chessboard_stereo(): override_l2r_tvec=params.l2r_tvec ) + # The above re-optimisation shouldn't make things worse, as its using same intrinsics and stereo. + print("Stereo, re-optimise=" + str(reproj_err) + ", " + str(recon_err)) assert reproj_err < 0.7 - assert recon_err < 1.8 - print("Stereo, extrinsics=" + str(reproj_err) + ", " + str(recon_err)) + assert recon_err < 1.7 # Test iterative calibration. reference_ids, reference_points, reference_image_size = get_iterative_reference_data() @@ -168,6 +170,38 @@ def test_chessboard_stereo(): reference_ids, reference_points, reference_image_size) + print("Stereo, iterative=" + str(reproj_err) + ", " + str(recon_err)) assert reproj_err < 0.7 assert recon_err < 1.6 - print("Stereo, iterative=" + str(reproj_err) + ", " + str(recon_err)) + + # Now test re-optimising extrinsics, using a completely different set of calibration params. + ov_l_c = np.loadtxt('tests/data/laparoscope_calibration/cbh-viking/calib.left.intrinsics.txt') + ov_l_d = np.loadtxt('tests/data/laparoscope_calibration/cbh-viking/calib.left.distortion.txt') + ov_r_c = np.loadtxt('tests/data/laparoscope_calibration/cbh-viking/calib.right.intrinsics.txt') + ov_r_d = np.loadtxt('tests/data/laparoscope_calibration/cbh-viking/calib.right.distortion.txt') + + ov_l2r_t = np.zeros((3, 1)) + ov_l2r_t[0][0] = -4.5 + + reproj_err, recon_err, params = \ + calibrator.calibrate( + override_left_intrinsics=ov_l_c, + override_left_distortion=ov_l_d, + override_right_intrinsics=ov_r_c, + override_right_distortion=ov_r_d, + override_l2r_rmat=np.eye(3), + override_l2r_tvec=ov_l2r_t + ) + + # Must check that the overrides have actually been set on the output. + assert np.allclose(params.left_params.camera_matrix, ov_l_c) + assert np.allclose(params.left_params.dist_coeffs, ov_l_d) + assert np.allclose(params.right_params.camera_matrix, ov_r_c) + assert np.allclose(params.right_params.dist_coeffs, ov_r_d) + assert np.allclose(params.l2r_rmat, np.eye(3)) + assert np.allclose(params.l2r_tvec, ov_l2r_t) + + # Not expecting good results, as the camera parameters are completely wrong. + print("Stereo, override=" + str(reproj_err) + ", " + str(recon_err)) + assert reproj_err < 33 + assert recon_err < 109 diff --git a/tests/video/test_hand_eye.py b/tests/video/test_hand_eye.py index cad989b..9e04438 100644 --- a/tests/video/test_hand_eye.py +++ b/tests/video/test_hand_eye.py @@ -67,6 +67,9 @@ def test_handeye_calibration_mono(): obj_tracking = load_tracking_from_glob( 'tests/data/2020_01_20_storz/12_50_30/calib.calib_obj_tracking.*.txt') + ref_img = cv2.imread( + 'tests/data/2020_01_20_storz/pattern_4x4_19x26_5_4_with_inset_9x14.png') + assert len(images) == 10 assert len(device_tracking) == 10 @@ -74,7 +77,8 @@ def test_handeye_calibration_mono(): min_number_of_points_per_image = 50 detector = \ - chpd.CharucoPlusChessboardPointDetector(error_if_no_chessboard=False) + chpd.CharucoPlusChessboardPointDetector(ref_img, + error_if_no_chessboard=False) calibrator = \ vidcal.video_calibration_driver_mono.MonoVideoCalibrationDriver( @@ -122,6 +126,9 @@ def test_handeye_calibration_stereo(): obj_tracking = load_tracking_from_glob( 'tests/data/2020_01_20_storz/12_50_30/calib.calib_obj_tracking.*.txt') + ref_img = cv2.imread( + 'tests/data/2020_01_20_storz/pattern_4x4_19x26_5_4_with_inset_9x14.png') + assert len(left_images) == 10 assert len(right_images) == 10 assert len(device_tracking) == 10 @@ -129,7 +136,8 @@ def test_handeye_calibration_stereo(): min_number_of_points_per_image = 50 detector = \ - chpd.CharucoPlusChessboardPointDetector(charuco_filtering=True, + chpd.CharucoPlusChessboardPointDetector(ref_img, + charuco_filtering=True, error_if_no_chessboard=False) calibrator = \