def get_stereo_transform():
endoscope_chesspts = list(read_camera.load_all('../calibration/endoscope_chesspts.p'))
camera_info = list(read_camera.load_all('../camera_data/camera_info.p'))
left_chesspts = np.matrix(list(read_camera.load_all('../camera_data/left_chesspts'))[0])
right_chesspts = np.matrix(list(read_camera.load_all('../camera_data/right_chesspts'))[0])
z = np.zeros((25, 1))
left_chesspts = np.hstack((left_chesspts, z))
right_chesspts = np.hstack((right_chesspts, z))
TL_R = transform.get_transform("Left Camera", "Right Camera", left_chesspts, right_chesspts, verbose=False)
return TL_R
# For phantom background:
# z_lower = -0.126
""" POSE PSM2 WAS CALIBRATED IN """
pos = PyKDL.Vector(-0.118749, 0.0203151, -0.111688)
rot = PyKDL.Rotation(-0.988883, -0.00205771, -0.148682, -0.00509171,
0.999786, 0.0200282, 0.148609, 0.0205626, -0.988682)
""" Move to arbitrary start position (near upper left corner) & release anything gripper is
holding. """
home(psm2, pos, rot)
""" Get PSM and endoscope calibration data (25 corresponding chess points) """
psm2_calibration_data = list(
transform.load_all('world/psm2_recordings.txt'))
psm2_calibration_matrix = transform.psm_data_to_matrix(
psm2_calibration_data)
endoscope_calibration_matrix = np.matrix(
list(read_camera.load_all('world/endoscope_chesspts.p'))[0])
""" Get the coordinates of most recently found needle centers (in endoscope frame) """
needle_points = np.matrix(
list(read_needle.load_all('needle_data/needle_points.p'))[0])
if USE_WORLD_TRANSFORM:
world = transform.generate_world()
TE_W = rigid_transform.solve_for_rigid_transformation(
endoscope_calibration_matrix, world)
needle_to_world = transform.transform_data("Endoscope", "World",
needle_points, TE_W)
needle_to_world[:, 2] = 0.
pprint.pprint(needle_to_world)
pos2 = PyKDL.Vector(-0.0972128, -0.0170138, -0.106974)
sideways = PyKDL.Rotation(-0.453413, 0.428549, -0.781513, -0.17203,
0.818259, 0.548505, 0.874541, 0.383143,
-0.297286)
""" Move to arbitrary start position (near upper left corner) & release anything gripper is
holding. """
home(psm2, pos2, sideways)
""" Get PSM and endoscope calibration data (25 corresponding chess points) """
psm2_calibration_data = list(
transform.load_all('utils/psm2_recordings.txt'))
psm2_calibration_matrix = transform.fit_to_plane(
transform.psm_data_to_matrix(psm2_calibration_data))
endoscope_calibration_matrix = transform.fit_to_plane(
np.matrix(
list(read_camera.load_all('camera_data/endoscope_chesspts.p'))[0]))
world = transform.generate_world()
TE_2 = transform.get_transform("Endoscope", "PSM2",
endoscope_calibration_matrix,
psm2_calibration_matrix)
psme_2 = transform.transform_data("Endoscope", "PSM2",
endoscope_calibration_matrix, TE_2,
psm2_calibration_matrix)
pprint.pprint(psme_2)
""" Move to chessboard corner, descend, come up,and go to next. """
move_to(psm2, psme_2.tolist(), z_upper)
home(psm2, pos, rot)