def test_chessboard_mono_io():
images = []
files = glob.glob('tests/data/laparoscope_calibration/left/*.png')
for file in files:
image = cv2.imread(file)
images.append(image)
chessboard_detector = pd.ChessboardPointDetector((14, 10), 3, (1, 1))
calibrator = mc.MonoVideoCalibrationDriver(chessboard_detector, 140)
for image in images:
successful = calibrator.grab_data(image, np.eye(4), np.eye(3))
assert successful > 0
reproj_err_1, recon_err_1, params_1 = calibrator.calibrate()
calibrator.save_data('tests/output/test_chessboard_mono_io', '')
calibrator.save_params('tests/output/test_chessboard_mono_io', '')
# Now, load data back in, recalibrate, should get same results.
# Technically, you aren't running the 'grab' bit again.
# The calibration works off the already extracted points.
calibrator.load_data('tests/output/test_chessboard_mono_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))
calibrator.load_params('tests/output/test_chessboard_mono_io', '')
params_3 = calibrator.get_params()
assert np.allclose(params_2.camera_matrix, params_3.camera_matrix)
assert np.allclose(params_2.dist_coeffs, params_3.dist_coeffs)
for i, _ in enumerate(params_3.rvecs):
assert np.allclose(params_2.rvecs[i], params_3.rvecs[i])
assert np.allclose(params_2.tvecs[i], params_3.tvecs[i])
def test_set_model2hand_arrays():
"""Test that model2hand arrays are set without raising an error"""
chessboard_detector = pd.ChessboardPointDetector((14, 10), 3, (1, 1))
stereo_calib = \
vidcal.video_calibration_driver_stereo.StereoVideoCalibrationDriver(
chessboard_detector, chessboard_detector, 140)
tracking_data_dir = 'tests/data/2020_01_20_storz/12_50_30'
file_prefix = 'calib'
stereo_calib.load_data(tracking_data_dir, file_prefix)
stereo_calib.tracking_data.set_model2hand_arrays()
def test_load_data_stereo_calib():
""" Load tracking and image data from test directory. """
chessboard_detector = pd.ChessboardPointDetector((14, 10), 3, (1, 1))
stereo_calib = \
vidcal.video_calibration_driver_stereo.StereoVideoCalibrationDriver(
chessboard_detector, chessboard_detector, 140)
tracking_data_dir = 'tests/data/2020_01_20_storz/12_50_30'
file_prefix = 'calib'
stereo_calib.load_data(tracking_data_dir, file_prefix)
assert len(stereo_calib.tracking_data.device_tracking_array) == 10
assert len(stereo_calib.tracking_data.calibration_tracking_array) == 10
assert len(stereo_calib.video_data.left_data.images_array) == 10
assert len(stereo_calib.video_data.right_data.images_array) == 10
def __init__(self,
configuration,
source):
"""
Constructor must throw if anything at all wrong.
:raises ValueError, RuntimeError.
"""
if configuration is None:
raise ValueError(
"You must provide a configuration file. "
"(see config/video_chessboard_conf.json for example).")
# Throws RuntimeError if anything wrong.
source = vcu.validate_camera_source(source)
# For now just doing chessboards.
# The underlying framework works for several point detectors,
# but each would have their own parameters etc.
method = configuration.get("method", "chessboard")
if method != "chessboard":
raise ValueError("Only chessboard calibration"
" is currently supported")
# The video source either defaults to whatever size OpenCV
# gives, or you can specify the size in the config file.
window_size = configuration.get("window size", None)
self.cap = vcu.open_video_source(source, window_size)
# These are the key parameters for the chessboard.
size = configuration.get("square size in mm", 3)
corners = configuration.get("corners", [14, 10])
self.corners = (corners[0], corners[1])
# .. and hence we can now create a chessboard point detector.
self.detector = cpd.ChessboardPointDetector(corners, size)
self.frame_ok = False
self.frame = None
self.key_pressed = None
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 run_video_calibration(configuration=None, save_dir=None, prefix=None):
"""
Performs Video Calibration using OpenCV
source and scikit-surgerycalibration.
Currently only chessboards are supported
:param config_file: location of a configuration file.
:param save_dir: optional directory name to dump calibrations to.
:param prefix: file name prefix when saving
:raises ValueError: if method is not supported
"""
if prefix is not None and save_dir is None:
save_dir = "./"
if configuration is None:
configuration = {}
# For now just doing chessboards.
# The underlying framework works for several point detectors,
# but each would have their own parameters etc.
method = configuration.get("method", "chessboard")
if method != "chessboard":
raise ValueError("Only chessboard calibration is currently supported")
source = configuration.get("source", 0)
corners = configuration.get("corners", [14, 10])
corners = (corners[0], corners[1])
size = configuration.get("square size in mm", 3)
window_size = configuration.get("window size", None)
min_num_views = configuration.get("minimum number of views", 5)
keypress_delay = configuration.get("keypress delay", 10)
interactive = configuration.get("interactive", True)
sample_frequency = configuration.get("sample frequency", 1)
cap = cv2.VideoCapture(source)
if not cap.isOpened():
raise RuntimeError("Failed to open camera.")
if window_size is not None:
cap.set(cv2.CAP_PROP_FRAME_WIDTH, window_size[0])
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, window_size[1])
print("Video feed set to (" + str(window_size[0]) + " x " +
str(window_size[1]) + ")")
else:
width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
print("Video feed defaults to (" + str(width) + " x " + str(height) +
")")
detector = cpd.ChessboardPointDetector(corners, size)
calibrator = mc.MonoVideoCalibrationDriver(detector,
corners[0] * corners[1])
print("Press 'q' to quit and 'c' to capture an image.")
print("Minimum number of views to calibrate:" + str(min_num_views))
frames_sampled = 0
while True:
frame_ok, frame = cap.read()
key = None
frames_sampled += 1
if not frame_ok:
print("Reached end of video source or read failure.")
key = ord('q')
else:
if interactive:
cv2.imshow("live image", frame)
key = cv2.waitKey(keypress_delay)
else:
if frames_sampled % sample_frequency == 0:
key = ord('c')
if key == ord('q'):
break
if key == ord('c'):
number_points = calibrator.grab_data(frame)
if number_points > 0:
img_pts = calibrator.video_data.image_points_arrays[-1]
img = cv2.drawChessboardCorners(frame, corners, img_pts,
number_points)
if interactive:
cv2.imshow("detected points", img)
number_of_views = calibrator.get_number_of_views()
print("Number of frames = " + str(number_of_views))
if number_of_views >= min_num_views:
proj_err, recon_err, params = calibrator.calibrate()
print("Reprojection (2D) error is:" + str(proj_err))
print("Reconstruction (3D) error is:" + str(recon_err))
print("Intrinsics are:")
print(params.camera_matrix)
print("Distortion matrix is:")
print(params.dist_coeffs)
if save_dir is not None:
if not os.path.isdir(save_dir):
os.makedirs(save_dir)
calibrator.save_data(save_dir, prefix)
calibrator.save_params(save_dir, prefix)
else:
print("Failed to detect points")
cap.release()
cv2.destroyAllWindows()
def test_chessboard_mono():
images = []
files = glob.glob('tests/data/laparoscope_calibration/left/*.png')
for file in files:
image = cv2.imread(file)
images.append(image)
# This illustrates that the PointDetector sub-class holds the knowledge of the model.
chessboard_detector = \
pd.ChessboardPointDetector((14, 10), # (Width, height), number of internal corners
3, # Actual square size in mm
(1, 1) # Scale factors. Here, images are 1920x1080 so no scaling needed.
) # If images were 1920x540, you'd pass in (1920, 540),
# and PointDetector base class would scale it up.
# Pass a PointDetector sub-class to the calibration driver.
calibrator = \
mc.MonoVideoCalibrationDriver(chessboard_detector, 140)
# Repeatedly grab data, until you have enough.
for image in images:
successful = calibrator.grab_data(image, np.eye(4))
assert successful > 0
assert calibrator.is_device_tracked()
assert not calibrator.is_calibration_target_tracked()
# Extra checking, as its a unit test
assert calibrator.get_number_of_views() == 9
calibrator.pop()
assert calibrator.get_number_of_views() == 8
successful = calibrator.grab_data(images[-1])
assert successful
assert calibrator.get_number_of_views() == 9
# Then do calibration
reproj_err, recon_err, params = calibrator.calibrate()
# Just for a regression test, checking reprojection error, and recon error.
# We do appear to get different performance on Linux/Mac
assert reproj_err < 0.6
assert recon_err < 0.3
# Test components of iterative calibration.
original_image = calibrator.video_data.images_array[0]
_, _, original_pts = chessboard_detector.get_points(original_image)
# First ensure undistorting / redistorting points works.
undistorted_points = cv2.undistortPoints(
original_pts, calibrator.calibration_params.camera_matrix,
calibrator.calibration_params.dist_coeffs, None,
calibrator.calibration_params.camera_matrix)
distorted_pts = vu.distort_points(
undistorted_points.reshape(
(-1, 2)), calibrator.calibration_params.camera_matrix,
calibrator.calibration_params.dist_coeffs)
assert np.allclose(original_pts, distorted_pts, rtol=1e-4, atol=1e-4)
rectify_map_1, rectify_map_2 = cv2.initUndistortRectifyMap(
calibrator.calibration_params.camera_matrix,
calibrator.calibration_params.dist_coeffs, None,
calibrator.calibration_params.camera_matrix,
(original_image.shape[1], original_image.shape[0]), cv2.CV_32FC1)
#undistorted_image = cv2.undistort(original_image, calibrator.calibration_params.camera_matrix, calibrator.calibration_params.dist_coeffs, calibrator.calibration_params.camera_matrix)
undistorted_image = cv2.remap(original_image, rectify_map_1, rectify_map_2,
cv2.INTER_LANCZOS4)
_, _, undistorted_pts = chessboard_detector.get_points(undistorted_image)
distorted_pts = vu.distort_points(
undistorted_pts, calibrator.calibration_params.camera_matrix,
calibrator.calibration_params.dist_coeffs)
assert np.allclose(original_pts, distorted_pts, rtol=1e-1, atol=1e-1)
# 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)
assert reproj_err < 0.7
assert recon_err < 0.4
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 run_video_calibration_checker(configuration=None,
calib_dir='./',
prefix=None):
"""
Application that detects a calibration pattern, runs
solvePnP, and prints information out to enable you to
check how accurate a calibration actually is.
:param config_file: location of configuration file.
:param calib_dir: the location of the calibration directory you want to
check
:param prefix: the file prefix for the calibration data you want to check
:raises ValueError: if no configuration provided.
:raises RuntimeError: if can't open source.
"""
if configuration is None:
raise ValueError("Calibration Checker requires a config file")
source = configuration.get("source", 0)
corners = configuration.get("corners", [14, 10])
corners = (corners[0], corners[1])
size = configuration.get("square size in mm", 3)
window_size = configuration.get("window size", None)
keypress_delay = configuration.get("keypress delay", 10)
interactive = configuration.get("interactive", True)
sample_frequency = configuration.get("sample frequency", 1)
existing_calibration = MonoCalibrationParams()
existing_calibration.load_data(calib_dir, prefix, halt_on_ioerror=False)
intrinsics = existing_calibration.camera_matrix
distortion = existing_calibration.dist_coeffs
cap = cv2.VideoCapture(source)
if not cap.isOpened():
raise RuntimeError("Failed to open camera:" + str(source))
if window_size is not None:
cap.set(cv2.CAP_PROP_FRAME_WIDTH, window_size[0])
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, window_size[1])
print("Video feed set to (" + str(window_size[0]) + " x " +
str(window_size[1]) + ")")
else:
width = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
height = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
print("Video feed defaults to (" + str(width) + " x " + str(height) +
")")
# For detecting the chessboard points
detector = cpd.ChessboardPointDetector(corners, size)
num_pts = corners[0] * corners[1]
captured_positions = np.zeros((0, 3))
frames_sampled = 0
while True:
frame_ok, frame = cap.read()
key = None
frames_sampled += 1
if not frame_ok:
print("Reached end of video source or read failure.")
key = ord('q')
else:
undistorted = cv2.undistort(frame, intrinsics, distortion)
if interactive:
cv2.imshow("live image", undistorted)
key = cv2.waitKey(keypress_delay)
else:
if frames_sampled % sample_frequency == 0:
key = ord('a')
if key == ord('q'):
break
image_points = np.array([])
if key in [ord('c'), ord('m'), ord('t'), ord('a')]:
_, object_points, image_points = \
detector.get_points(undistorted)
pnp_ok = False
img = None
tvec = None
if image_points.shape[0] > 0:
img = cv2.drawChessboardCorners(undistorted, corners, image_points,
num_pts)
if interactive:
cv2.imshow("detected points", img)
pnp_ok, _, tvec = cv2.solvePnP(object_points, image_points,
intrinsics, None)
if pnp_ok:
captured_positions = np.append(captured_positions,
np.transpose(tvec),
axis=0)
if key in [ord('t'), ord('a')] and captured_positions.shape[0] > 1:
print(
str(captured_positions[-1][0] - captured_positions[-2][0]) +
" " +
str(captured_positions[-1][1] - captured_positions[-2][1]) +
" " +
str(captured_positions[-1][2] - captured_positions[-2][2]) +
" ")
if key in [ord('m'), ord('a')] and \
captured_positions.shape[0] > 1:
print("Mean:" + str(np.mean(captured_positions, axis=0)))
print("StdDev:" + str(np.std(captured_positions, axis=0)))
if key in [ord('c'), ord('a')] and pnp_ok:
print("Pose" + str(tvec[0][0]) + " " + str(tvec[1][0]) + " " +
str(tvec[2][0]))
if not pnp_ok and image_points.shape[0] > 0:
print("Failed to solve PnP")
if image_points.shape[0] == 0:
print("Failed to detect points")
cap.release()
cv2.destroyAllWindows()
def __init__(self, configuration, calib_dir, overlay_offset):
source = configuration.get("source", 1)
corners = configuration.get("corners")
self.size = configuration.get("square size in mm")
window_size = configuration.get("window size")
self.overlay_offset = overlay_offset
_, self.intrinsics, self.distortion, _ = \
configure_camera(configuration, calib_dir)
if corners is None:
raise ValueError("You must specify the number of internal corners")
if self.size is None:
raise ValueError("You must specify the size of each square in mm.")
if window_size is None:
raise ValueError("You must specify the window size.")
if self.intrinsics is None:
raise ValueError("Couldn't load intrinsic parameters")
if self.distortion is None:
raise ValueError("Couldn't load self.distortion parameters")
self.corners = (corners[0], corners[1])
self.detector = cpd.ChessboardPointDetector(self.corners, self.size)
self.num_pts = corners[0] * corners[1]
self.cap = cv2.VideoCapture(int(source))
if not self.cap.isOpened():
raise RuntimeError("Failed to open camera:" + str(source))
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, window_size[0])
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, window_size[1])
print("Video feed set to (" + str(window_size[0]) + " x " +
str(window_size[1]) + ")")
self.vtk_overlay_window = VTKOverlayWindow()
self.timer = None
self.update_rate = 15
self.ultrasound_actor = vtk.vtkImageActor()
self.ultrasound_actor.SetInputData(
self.vtk_overlay_window.image_importer.GetOutput())
self.vtk_overlay_window.foreground_renderer.AddActor(
self.ultrasound_actor)
f_x = self.intrinsics[0, 0]
c_x = self.intrinsics[0, 2]
f_y = self.intrinsics[1, 1]
c_y = self.intrinsics[1, 2]
width, height = window_size[0], window_size[1]
#pylint:disable=line-too-long
cm.set_camera_intrinsics(
self.vtk_overlay_window.get_foreground_renderer(),
self.vtk_overlay_window.get_foreground_camera(), width, height,
f_x, f_y, c_x, c_y, 1, 1000)
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)
