def get_calib_driver(calib_dir: str):
left_point_detector = get_point_detector(
np.loadtxt(
"tests/data/laparoscope_calibration/cbh-viking/calib.left.intrinsics.txt"
),
np.loadtxt(
"tests/data/laparoscope_calibration/cbh-viking/calib.left.distortion.txt"
))
right_point_detector = get_point_detector(
np.loadtxt(
"tests/data/laparoscope_calibration/cbh-viking/calib.right.intrinsics.txt"
),
np.loadtxt(
"tests/data/laparoscope_calibration/cbh-viking/calib.right.distortion.txt"
))
minimum_points = 50
calibration_driver = sc.StereoVideoCalibrationDriver(
left_point_detector, right_point_detector, minimum_points)
for i in range(3):
l_img, r_img, chessboard, scope = get_calib_data(calib_dir, i)
calibration_driver.grab_data(l_img, r_img, scope, chessboard)
return calibration_driver
def test_stereo_davinci():
left_images = []
files = glob.glob('tests/data/ChAruco_LR_frames_Steve_Axis_Tests/ExtractedFrames_L/*.jpg')
files.sort()
for file in files:
image = cv2.imread(file)
print("Loaded:" + str(file))
left_images.append(image)
assert(len(left_images) == 59)
right_images = []
files = glob.glob('tests/data/ChAruco_LR_frames_Steve_Axis_Tests/ExtractedFrames_R/*.jpg')
files.sort()
for file in files:
image = cv2.imread(file)
print("Loaded:" + str(file))
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(ref_img,
error_if_no_chessboard=False) # Try to accept as many as possible.
calibrator = sc.StereoVideoCalibrationDriver(detector, detector, minimum_number_of_points_per_image)
for i, _ in enumerate(left_images):
try:
number_left, number_right = calibrator.grab_data(left_images[i], right_images[i])
if number_left < minimum_number_of_points_per_image:
print("Image pair:" + str(i) + ", left image, SKIPPED, due to not enough points")
if number_right < minimum_number_of_points_per_image:
print("Image pair:" + str(i) + ", right image, SKIPPED, due to not enough points")
except ValueError as ve:
print("Image pair:" + str(i) + ", FAILED, due to:" + str(ve))
except TypeError as te:
print("Image pair:" + str(i) + ", FAILED, due to:" + str(te))
reproj_err, recon_err, params = calibrator.calibrate()
print("Reproj:" + str(reproj_err))
print("Recon:" + str(recon_err))
assert reproj_err < 1.1
assert recon_err < 6.4
# Now try iterative.
reference_image = ch.make_charuco_with_chessboard()
reference_ids, object_pts, reference_pts = detector.get_points(reference_image)
reproj_err, recon_err, params = calibrator.iterative_calibration(2,
reference_ids,
reference_pts,
(reference_image.shape[1], reference_image.shape[0]))
print("Reproj:" + str(reproj_err))
print("Recon:" + str(recon_err))
assert reproj_err < 0.9
assert recon_err < 2.78
calibrator.save_params('tests/output/ChAruco_LR_frames_Steve_Axis_Tests/params', '')
calibrator.save_data('tests/output/ChAruco_LR_frames_Steve_Axis_Tests/data', '')
def calibrate(left_pd: PointDetector,
right_pd: PointDetector,
calib_dir: str,
stereo_params=None):
""" Wrapper around calibration algorithm from scikit-surgerycalibration. """
LOGGER.info("Starting Calibration")
calibration_driver = sc.StereoVideoCalibrationDriver(
left_pd, right_pd, minimum_points)
LOGGER.info(f"Grabbing Data: {calib_dir}")
total_frame_grabbing_time = 0
num_frames = 10
for i in range(num_frames):
start = time.time()
l_img, r_img, chessboard, scope = get_calib_data(calib_dir, i)
calibration_driver.grab_data(l_img, r_img, scope, chessboard)
elapsed = time.time() - start
LOGGER.info(f"Took {elapsed} seconds to grab data")
total_frame_grabbing_time += elapsed
mean_frame_grabbing_time = total_frame_grabbing_time / num_frames
LOGGER.info("Calibrating")
start = time.time()
if not stereo_params:
stereo_reproj_err, stereo_recon_err, stereo_params = \
calibration_driver.calibrate()
else:
left_intrinsics = stereo_params.left_params.camera_matrix
left_distortion = stereo_params.left_params.dist_coeffs
right_intrinsics = stereo_params.right_params.camera_matrix
right_distortion = stereo_params.right_params.dist_coeffs
l2r_rmat = stereo_params.l2r_rmat
l2r_tvec = stereo_params.l2r_tvec
LOGGER.info("Using precalibration")
stereo_reproj_err, stereo_recon_err, stereo_params = \
calibration_driver.calibrate(
override_left_intrinsics=left_intrinsics,
override_left_distortion=left_distortion,
override_right_intrinsics=right_intrinsics,
override_right_distortion=right_distortion,
override_l2r_rmat=l2r_rmat,
override_l2r_tvec=l2r_tvec)
LOGGER.info(
f"Calibration (not including hand eye) took: {time.time() - start}")
tracked_reproj_err, tracked_recon_err, stereo_params = \
calibration_driver.handeye_calibration()
elapsed_time = time.time() - start
return stereo_reproj_err, stereo_recon_err, tracked_reproj_err, \
tracked_recon_err, elapsed_time, mean_frame_grabbing_time, stereo_params
def get_calib_driver(calib_dir: str):
""" Create left/right charuco point detectors and load calibration images from directory. """
reference_image = cv2.imread("tests/data/2020_01_20_storz/pattern_4x4_19x26_5_4_with_inset_9x14.png")
minimum_points = 50
number_of_squares = [19, 26]
square_tag_sizes = [5, 4]
filter_markers = True
number_of_chessboard_squares = [9, 14]
chessboard_square_size = 3
chessboard_id_offset = 500
left_pd = \
charuco_pd.CharucoPlusChessboardPointDetector(
reference_image,
minimum_number_of_points=minimum_points,
number_of_charuco_squares=number_of_squares,
size_of_charuco_squares=square_tag_sizes,
charuco_filtering=filter_markers,
number_of_chessboard_squares=number_of_chessboard_squares,
chessboard_square_size=chessboard_square_size,
chessboard_id_offset=chessboard_id_offset
)
right_pd = \
charuco_pd.CharucoPlusChessboardPointDetector(
reference_image,
minimum_number_of_points=minimum_points,
number_of_charuco_squares=number_of_squares,
size_of_charuco_squares=square_tag_sizes,
charuco_filtering=filter_markers,
number_of_chessboard_squares=number_of_chessboard_squares,
chessboard_square_size=chessboard_square_size,
chessboard_id_offset=chessboard_id_offset
)
calibration_driver = sc.StereoVideoCalibrationDriver(left_pd,
right_pd,
minimum_points)
for i in range(3):
l_img, r_img, chessboard, scope = get_calib_data(calib_dir, i)
calibration_driver.grab_data(l_img, r_img, scope, chessboard)
return calibration_driver
def iterative_calibrate(left_pd: PointDetector, right_pd: PointDetector,
calib_dir: str, iterative_image_file: str,
iterations: int, pattern: str):
""" Wrapper around iterative calibration algorithm from
scikit-surgerycalibration. """
LOGGER.info("Iterative Calibration")
iterative_image = cv2.imread(iterative_image_file)
iterative_ids, iterative_image_points, iterative_image_size = \
create_iterative_ref_data(iterative_image, left_pd, pattern)
calibration_driver = sc.StereoVideoCalibrationDriver(
left_pd, right_pd, minimum_points)
LOGGER.info(f"Grabbing Data: {calib_dir}")
total_frame_grabbing_time = 0
num_frames = 10
for i in range(num_frames):
start = time.time()
l_img, r_img, chessboard, scope = get_calib_data(calib_dir, i)
calibration_driver.grab_data(l_img, r_img, scope, chessboard)
elapsed = time.time() - start
LOGGER.info(f"Took {elapsed} seconds to grab data")
total_frame_grabbing_time += elapsed
mean_frame_grabbing_time = total_frame_grabbing_time / num_frames
LOGGER.info("Calibrating")
start = time.time()
stereo_reproj_err, stereo_recon_err, stereo_params = \
calibration_driver.iterative_calibration(iterations,
iterative_ids,
iterative_image_points,
iterative_image_size)
tracked_reproj_err, tracked_recon_err, stereo_params = \
calibration_driver.handeye_calibration()
elapsed_time = time.time() - start
return stereo_reproj_err, stereo_recon_err, tracked_reproj_err, \
tracked_recon_err, elapsed_time, mean_frame_grabbing_time
def test_chessboard_stereo_io():
left_images, right_images \
= vtu.load_left_right_pngs('tests/data/laparoscope_calibration/', 9)
chessboard_detector = \
pd.ChessboardPointDetector((14, 10),
3,
(1, 1)
)
calibrator = \
sc.StereoVideoCalibrationDriver(chessboard_detector, chessboard_detector, 140)
for i, _ in enumerate(left_images):
num_left, num_right = calibrator.grab_data(left_images[i],
right_images[i], np.eye(4),
np.eye(3))
assert num_left > 0
assert num_right > 0
# Then do calibration
reproj_err_1, recon_err_1, params_1 = calibrator.calibrate()
calibrator.save_data('tests/output/test_chessboard_stereo_io', '')
calibrator.save_params('tests/output/test_chessboard_stereo_io', '')
# Load data, re-do calibration, check for same result.
calibrator.load_data('tests/output/test_chessboard_stereo_io', '')
reproj_err_2, recon_err_2, params_2 = calibrator.calibrate()
assert (np.isclose(reproj_err_1, reproj_err_2))
assert (np.isclose(recon_err_1, recon_err_2))
# Now load parameters back in.
calibrator.load_params('tests/output/test_chessboard_stereo_io', '')
params_2 = calibrator.get_params()
assert np.allclose(params_1.l2r_rmat, params_2.l2r_rmat)
assert np.allclose(params_1.l2r_tvec, params_2.l2r_tvec)
assert np.allclose(params_1.essential, params_2.essential)
assert np.allclose(params_1.fundamental, params_2.fundamental)
def test_chessboard_stereo():
left_images, right_images \
= vtu.load_left_right_pngs('tests/data/laparoscope_calibration/', 9)
chessboard_detector = \
pd.ChessboardPointDetector((14, 10),
3,
(1, 1)
)
calibrator = \
sc.StereoVideoCalibrationDriver(chessboard_detector, chessboard_detector, 140)
# Repeatedly grab data, until you have enough.
for i, _ in enumerate(left_images):
number_left, number_right = calibrator.grab_data(
left_images[i], right_images[i])
assert number_left > 0
assert number_right > 0
assert not calibrator.is_device_tracked()
assert not calibrator.is_calibration_target_tracked()
# Then do calibration
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.7
# 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,
override_left_distortion=params.left_params.dist_coeffs,
override_right_intrinsics=params.right_params.camera_matrix,
override_right_distortion=params.right_params.dist_coeffs,
override_l2r_rmat=params.l2r_rmat,
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.7
# Test iterative calibration.
reference_ids, reference_points, reference_image_size = get_iterative_reference_data(
)
reproj_err, recon_err, params = calibrator.iterative_calibration(
3, 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
# 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
def test_triangulation():
"""
Testing because I need to understand the difference between triangulating
unrectified images, and rectified images.
"""
left_images = []
files = glob.glob('tests/data/chessboard/left/*.png')
files.sort()
for file in files:
image = cv2.imread(file)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
left_images.append(image)
assert (len(left_images) == 9)
right_images = []
files = glob.glob('tests/data/chessboard/right/*.png')
files.sort()
for file in files:
image = cv2.imread(file)
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
right_images.append(image)
assert (len(right_images) == 9)
chessboard_detector = cpd.ChessboardPointDetector((14, 10), 3, (1, 1))
calibrator = \
sc.StereoVideoCalibrationDriver(chessboard_detector,
chessboard_detector,
140)
# Repeatedly grab data, until you have enough.
for i, _ in enumerate(left_images):
success_l, success_r = \
calibrator.grab_data(left_images[i], right_images[i])
assert success_l > 0
# Then do calibration
reproj_err, recon_err, params = calibrator.calibrate()
left_image = cv2.imread('tests/data/chessboard/left-2520.png')
right_image = cv2.imread('tests/data/chessboard/right-2520.png')
left_intrinsics = params.left_params.camera_matrix
left_distortion = params.left_params.dist_coeffs
right_intrinsics = params.right_params.camera_matrix
right_distortion = params.right_params.dist_coeffs
l2r_rmat = params.l2r_rmat
l2r_tvec = params.l2r_tvec
left_undistorted = cv2.undistort(left_image, left_intrinsics,
left_distortion)
right_undistorted = cv2.undistort(right_image, right_intrinsics,
right_distortion)
pd = cpd.ChessboardPointDetector((14, 10), 3)
l_ud_ids, l_ud_obj, l_ud_im = pd.get_points(left_undistorted)
r_ud_ids, r_ud_obj, r_ud_im = pd.get_points(right_undistorted)
w = left_image.shape[1]
h = left_image.shape[0]
R1, R2, P1, P2, Q, vp1, vp2 = cv2.stereoRectify(left_intrinsics,
left_distortion,
right_intrinsics,
right_distortion, (w, h),
l2r_rmat, l2r_tvec)
undistort_rectify_map_l_x, undistort_rectify_map_l_y = \
cv2.initUndistortRectifyMap(left_intrinsics, left_distortion, R1, P1, (w, h), cv2.CV_32FC1)
undistort_rectify_map_r_x, undistort_rectify_map_r_y = \
cv2.initUndistortRectifyMap(right_intrinsics, right_distortion, R2, P2, (w, h), cv2.CV_32FC1)
left_rectified = cv2.remap(left_image, undistort_rectify_map_l_x,
undistort_rectify_map_l_y, cv2.INTER_LINEAR)
right_rectified = cv2.remap(right_image, undistort_rectify_map_r_x,
undistort_rectify_map_r_y, cv2.INTER_LINEAR)
l_rf_ids, l_rf_obj, l_rf_im = pd.get_points(left_rectified)
r_rf_ids, r_rf_obj, r_rf_im = pd.get_points(right_rectified)
points_4D = cv2.triangulatePoints(P1, P2, np.transpose(l_rf_im),
np.transpose(r_rf_im))
points_4D = np.transpose(points_4D)
points_4D = points_4D[:, 0:-1] / points_4D[:, -1].reshape((140, 1))
# Convert from first (left) camera rectified to left camera unrectified
points_3D = np.transpose(
np.matmul(np.linalg.inv(R1), np.transpose(points_4D)))
# Triangulate points in undistorted, but not rectified space.
image_points = np.zeros((l_ud_im.shape[0], 4))
image_points[:, 0:2] = l_ud_im
image_points[:, 2:4] = r_ud_im
triangulated = cvcpp.triangulate_points_using_hartley(
image_points, left_intrinsics, right_intrinsics, l2r_rmat, l2r_tvec)
# All being well, points_3D == triangulated
assert np.allclose(points_3D, triangulated, rtol=0.1, atol=0.1)