def test_construct_rigid_transformation():
tiny = 0.0001
rot_m = mat.construct_rx_matrix(np.pi / 2)
vm.validate_rotation_matrix(rot_m)
trans_v = np.array([[10], [10], [10]])
vm.validate_translation_column_vector(trans_v)
rigid_transformation = mat.construct_rigid_transformation(rot_m, trans_v)
vm.validate_rigid_matrix(rigid_transformation)
assert np.abs(rigid_transformation[0][0] - 1) < tiny
assert np.abs(rigid_transformation[0][1]) < tiny
assert np.abs(rigid_transformation[0][2]) < tiny
assert np.abs(rigid_transformation[0][3] - 10) < tiny
assert np.abs(rigid_transformation[1][0]) < tiny
assert np.abs(rigid_transformation[1][1]) < tiny
assert np.abs(rigid_transformation[1][2] + 1) < tiny
assert np.abs(rigid_transformation[1][3] - 10) < tiny
assert np.abs(rigid_transformation[2][0]) < tiny
assert np.abs(rigid_transformation[2][1] - 1) < tiny
assert np.abs(rigid_transformation[2][2]) < tiny
assert np.abs(rigid_transformation[2][3] - 10) < tiny
assert np.abs(rigid_transformation[3][0]) < tiny
assert np.abs(rigid_transformation[3][1]) < tiny
assert np.abs(rigid_transformation[3][2]) < tiny
assert np.abs(rigid_transformation[3][3] - 1) < tiny
def test_construct_rx_matrix():
tiny = 0.0001
new_point = check_construct_rx_matrix(90., 0, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1]) < tiny
assert np.abs(new_point[2] - 1) < tiny
new_point = check_construct_rx_matrix(np.pi / 2, 1, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1]) < tiny
assert np.abs(new_point[2] - 1) < tiny
new_point = check_construct_rx_matrix(-90., 0, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1]) < tiny
assert np.abs(new_point[2] + 1) < tiny
new_point = check_construct_rx_matrix(-np.pi / 2, 1, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1]) < tiny
assert np.abs(new_point[2] + 1) < tiny
new_point = check_construct_rx_matrix(180., 0, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1] + 1) < tiny
assert np.abs(new_point[2]) < tiny
new_point = check_construct_rx_matrix(np.pi, 1, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1] + 1) < tiny
assert np.abs(new_point[2]) < tiny
new_point = check_construct_rx_matrix(-180., 0, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1] + 1) < tiny
assert np.abs(new_point[2]) < tiny
new_point = check_construct_rx_matrix(-np.pi, 1, np.array([0, 1, 0]).T)
assert np.abs(new_point[0]) < tiny
assert np.abs(new_point[1] + 1) < tiny
assert np.abs(new_point[2]) < tiny
#check for bad types
with pytest.raises(TypeError):
mat.construct_rx_matrix(str('10.'), False)
def test_goicp_bad_rotation_limit():
fixed = np.loadtxt('tests/data/icp/rabbit_full.xyz')
moving = np.loadtxt('tests/data/icp/partial_rabbit_aligned.xyz')
tf = m.construct_rx_matrix(90, False)
moving = np.matmul(tf, np.transpose(moving))
moving = np.transpose(moving)
goicp_reg = goicp.RigidRegistration(rotation_limits=[-60, 60])
residual, moving_to_fixed = goicp_reg.register(moving, fixed)
assert residual > 2
def check_construct_rx_matrix(angle, is_in_radians, point):
""""
Check if the rotation matrix for rotating around the x axis is correct.
:param angle: the angle to rotate
:param is_in_radians: if angle is in radians or not
:param point: the point to be rotated
:returns: new_point -- the point after rotation
"""
rot_x = mat.construct_rx_matrix(angle, is_in_radians)
vm.validate_rotation_matrix(rot_x)
new_point = np.matmul(rot_x, point)
assert new_point[0] == point[0]
assert np.abs(np.linalg.norm(new_point) - np.linalg.norm(point)) <= 0.0001
return new_point
def set_pose_with_matrices(self, p2c, l2c, angle_of_handle):
"""
Method to apply 4x4 transformations to actors.
:param p2c: 4x4 matrix, either numpy or vtk matrix.
:param l2c: 4x4 matrix, either numpy or vtk matrix.
:param angle_of_handle: angle in deg.
:return: N/A
"""
# First we can compute the angle of the handle.
# This applies directly to the data, as it comes out
# of the vtkTransformPolyDataFilter, before the actor transformation.
probe_offset = np.eye(4)
probe_offset[0][3] = 0.007877540588378196
probe_offset[1][3] = 36.24640712738037
probe_offset[2][3] = -3.8626091003417997
r_x = \
cmu.construct_rx_matrix(angle_of_handle, is_in_radians=False)
rotation_about_x = \
cmu.construct_rigid_transformation(r_x, np.zeros((3, 1)))
self.cyl_trans.SetMatrix(
vmu.create_vtk_matrix_from_numpy(probe_offset @ rotation_about_x))
self.cyl_transform_filter.Update()
# Check p2c, l2c: if numpy, convert to vtk.
if isinstance(p2c, np.ndarray):
p2c = vmu.create_vtk_matrix_from_numpy(p2c)
if isinstance(l2c, np.ndarray):
l2c = vmu.create_vtk_matrix_from_numpy(l2c)
# This is where we apply transforms to each actor.
# Apply p2c to probe
self.cyl_actor.PokeMatrix(p2c)
probe_model = self.model_loader.get_surface_model('probe')
probe_model.actor.PokeMatrix(p2c)
# Apply l2c to organs in scene.
for model in self.model_loader.get_surface_models():
if model.get_name() != 'probe':
model.actor.PokeMatrix(l2c)
# Force re-render
self.overlay.Render()
self.repaint()
def test_tracker_baseclass():
"""
We should throw not implemented error when we make a tracker without
the required functions.
"""
with pytest.raises(TypeError):
_ = BadTracker() # pylint: disable=abstract-class-instantiated
good_tracker = GoodTracker()
assert len(good_tracker.rvec_rolling_means) == 0
assert len(good_tracker.tvec_rolling_means) == 0
with pytest.raises(NotImplementedError):
good_tracker.close()
with pytest.raises(NotImplementedError):
good_tracker.get_frame()
with pytest.raises(NotImplementedError):
good_tracker.get_tool_descriptions()
with pytest.raises(NotImplementedError):
good_tracker.start_tracking()
with pytest.raises(NotImplementedError):
good_tracker.stop_tracking()
config = {'smoothing buffer': 3}
rigid_bodies = []
rigid_bodies.append(RigidBody('test rb'))
rigid_bodies.append(RigidBody('test rb'))
another_good_tracker = GoodTracker(config, rigid_bodies)
port_handles, time_stamps, frame_numbers, tracking, tracking_quality = \
another_good_tracker.get_smooth_frame(["test rb"])
assert len(port_handles) == 1
assert len(time_stamps) == 1
assert len(frame_numbers) == 1
assert len(tracking) == 1
assert len(tracking_quality) == 1
assert np.isnan(tracking_quality[0])
with pytest.raises(ValueError):
port_handles, time_stamps, frame_numbers, tracking, tracking_quality = \
another_good_tracker.get_smooth_frame(["another body",
"test rb"])
port_handles = ["another body", "test rb"]
time_stamps = [1.2, 1.3]
frame_numbers = [0, 0]
tracking_rot = [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]]
tracking_trans = [[50.0, -100.0, 70.0], [0.0, 100.0, 200.0]]
quality = [1.0, 0.5]
rot_is_quaternion = False
another_good_tracker.add_frame_to_buffer(port_handles, time_stamps,
frame_numbers, tracking_rot,
tracking_trans, quality,
rot_is_quaternion)
port_handles_out, time_stamps, frame_numbers, tracking, tracking_quality = \
another_good_tracker.get_smooth_frame(port_handles)
test_index = port_handles_out.index("test rb")
assert time_stamps[test_index] == 1.3
assert frame_numbers[test_index] == 0
rotation = construct_rx_matrix(1.0, True)
transform = construct_rigid_transformation(rotation, [0.0, 100.0, 200.0])
assert np.allclose(tracking[test_index], transform)
assert tracking_quality[test_index] == 0.5
port_handles = ["test rb"]
time_stamps = [1.4]
frame_numbers = [1]
tracking_rot = [[0.2, 0.0, 0.0]]
tracking_trans = [[10.0, 50.0, 200.0]]
quality = [0.8]
another_good_tracker.add_frame_to_buffer(port_handles, time_stamps,
frame_numbers, tracking_rot,
tracking_trans, quality,
rot_is_quaternion)
port_handles_out, time_stamps, frame_numbers, tracking, tracking_quality = \
another_good_tracker.get_smooth_frame(port_handles)
assert len(port_handles_out) == 1
port_handles = ["another body", "test rb"]
port_handles_out, time_stamps, frame_numbers, tracking, tracking_quality = \
another_good_tracker.get_smooth_frame(port_handles)
assert len(port_handles_out) == 2
test_index = port_handles_out.index("test rb")
assert time_stamps[test_index] == 1.35
assert frame_numbers[test_index] == 1
rotation = construct_rx_matrix(0.6, True)
transform = construct_rigid_transformation(rotation, [5.0, 75.0, 200.0])
assert np.allclose(tracking[test_index], transform)
assert tracking_quality[test_index] == 0.65
test_index = port_handles_out.index("another body")
assert time_stamps[test_index] == 1.2
assert frame_numbers[test_index] == 0
rotation = construct_rx_matrix(0.0, True)
transform = construct_rigid_transformation(rotation, [50.0, -100.0, 70.0])
assert np.allclose(tracking[test_index], transform)
assert tracking_quality[test_index] == 1.0