Commit b87652f6 authored by Guofang Xiao's avatar Guofang Xiao

Issue #12 - Finished constructing rotation matrices from Euler angles and testing.

parent cf4da1e2
Pipeline #1490 canceled with stages
in 12 minutes and 11 seconds
"""
Construct 3x3 rotation matrices for rotating around the x, y, z axes.
Construct 3x3 rotation matrices for rotating around the x, y and z axes
individually, as well as 3x3 rotation matrices from sequences of three
Euler angles.
"""
import numpy as np
......@@ -62,18 +64,26 @@ def construct_rz_matrix(angle, is_in_radians=True):
return rot_z
def construct_rotation_matrix_from_euler_angles(angle_a, angle_b, angle_c, sequence, is_in_radians=True):
def construct_rotm_from_euler(
angle_a, angle_b, angle_c,
sequence, is_in_radians=True):
"""
Construct a rotation matrix from a sequence of three Euler angles, to pre-multiply column vectors.
The three elemental rotations represented by the Euler angles are about the intrinsic axes.
They can also be interpreted to be about the extrinsic axes, in the reverse sequence.
:param angle_A, angle_B, angle_C: the three Euler angles
:param sequence: the sequence of axes the three elemental rotations are about, with respect to the intrinsic axes
Construct a rotation matrix from a sequence of three Euler angles, to
pre-multiply column vectors. In the case of tracking, the column vector is
under the local coordinate system and the resulting vector is under the
world (reference) coordinate system.
The three elemental rotations represented by the Euler angles are about the
INTRINSIC axes. They can also be interpreted to be about the EXTRINSIC axes,
in the reverse order.
:param angle_a: first Euler angle
:param angle_b: second Euler angle
:param angle_c: third Euler angle
:param sequence: the sequence of axes the three elemental rotations are
about, with respect to the intrinsic axes
:param is_in_radians: if angle is in radians or not, default being True
:returns: rot_m -- the 3x3 rotation matrix
"""
while True:
if sequence == 'zxz':
rot_a = construct_rz_matrix(angle_a, is_in_radians)
......@@ -93,12 +103,39 @@ def construct_rotation_matrix_from_euler_angles(angle_a, angle_b, angle_c, seque
rot_c = construct_ry_matrix(angle_c, is_in_radians)
break
raise ValueError(sequence + " is not in repository.")
if sequence == 'zyz':
rot_a = construct_rz_matrix(angle_a, is_in_radians)
rot_b = construct_ry_matrix(angle_b, is_in_radians)
rot_c = construct_rz_matrix(angle_c, is_in_radians)
break
rot_m = rot_a * rot_b * rot_c
if sequence == 'xzx':
rot_a = construct_rx_matrix(angle_a, is_in_radians)
rot_b = construct_rz_matrix(angle_b, is_in_radians)
rot_c = construct_rx_matrix(angle_c, is_in_radians)
break
return rot_m
if sequence == 'yxy':
rot_a = construct_ry_matrix(angle_a, is_in_radians)
rot_b = construct_rx_matrix(angle_b, is_in_radians)
rot_c = construct_ry_matrix(angle_c, is_in_radians)
break
if sequence == 'xyz':
rot_a = construct_rx_matrix(angle_a, is_in_radians)
rot_b = construct_ry_matrix(angle_b, is_in_radians)
rot_c = construct_rz_matrix(angle_c, is_in_radians)
break
construct_rotation_matrix_from_euler_angles(0, 0, -90, 'zxz', 0)
if sequence == 'zyx':
rot_a = construct_rz_matrix(angle_a, is_in_radians)
rot_b = construct_ry_matrix(angle_b, is_in_radians)
rot_c = construct_rx_matrix(angle_c, is_in_radians)
break
raise ValueError(sequence + " is not a valid sequence.")
rot_tmp = np.matmul(rot_a, rot_b)
rot_m = np.matmul(rot_tmp, rot_c)
return rot_m
......@@ -11,19 +11,17 @@ import sksurgerycore.utilities.validate_matrix as vm
def check_construct_rx_matrix(angle, is_in_radians, point):
""""
Test if the rotation matrix for rotating around the x axis is correct.
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
......@@ -32,19 +30,17 @@ def check_construct_rx_matrix(angle, is_in_radians, point):
def check_construct_ry_matrix(angle, is_in_radians, point):
""""
Test if the rotation matrix for rotating around the y axis is correct.
Check if the rotation matrix for rotating around the y 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_y = mat.construct_ry_matrix(angle, is_in_radians)
vm.validate_rotation_matrix(rot_y)
new_point = np.matmul(rot_y, point)
assert new_point[1] == point[1]
assert np.abs(np.linalg.norm(new_point) - np.linalg.norm(point)) <= 0.0001
......@@ -53,19 +49,17 @@ def check_construct_ry_matrix(angle, is_in_radians, point):
def check_construct_rz_matrix(angle, is_in_radians, point):
""""
Test if the rotation matrix for rotating around the z axis is correct.
Check if the rotation matrix for rotating around the z 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_z = mat.construct_rz_matrix(angle, is_in_radians)
vm.validate_rotation_matrix(rot_z)
new_point = np.matmul(rot_z, point)
assert new_point[2] == point[2]
assert np.abs(np.linalg.norm(new_point) - np.linalg.norm(point)) <= 0.0001
......@@ -124,7 +118,7 @@ def test_construct_ry_matrix():
assert np.abs(new_point[1]) <= tiny
assert np.abs(new_point[2] + 1) <= tiny
new_point = check_construct_ry_matrix(np.pi / 2, 1, np.array([1, 0, 0]).T)
new_point = check_construct_ry_matrix(np.pi/2, 1, np.array([1, 0, 0]).T)
assert np.abs(new_point[0]) <= tiny
assert np.abs(new_point[1]) <= tiny
assert np.abs(new_point[2] + 1) <= tiny
......@@ -134,7 +128,7 @@ def test_construct_ry_matrix():
assert np.abs(new_point[1]) <= tiny
assert np.abs(new_point[2] - 1) <= tiny
new_point = check_construct_ry_matrix(-np.pi / 2, 1, np.array([1, 0, 0]).T)
new_point = check_construct_ry_matrix(-np.pi/2, 1, np.array([1, 0, 0]).T)
assert np.abs(new_point[0]) <= tiny
assert np.abs(new_point[1]) <= tiny
assert np.abs(new_point[2] - 1) <= tiny
......@@ -168,7 +162,7 @@ def test_construct_rz_matrix():
assert np.abs(new_point[1] - 1) <= tiny
assert np.abs(new_point[2]) <= tiny
new_point = check_construct_rz_matrix(np.pi / 2, 1, np.array([1, 0, 0]).T)
new_point = check_construct_rz_matrix(np.pi/2, 1, np.array([1, 0, 0]).T)
assert np.abs(new_point[0]) <= tiny
assert np.abs(new_point[1] - 1) <= tiny
assert np.abs(new_point[2]) <= tiny
......@@ -178,7 +172,7 @@ def test_construct_rz_matrix():
assert np.abs(new_point[1] + 1) <= tiny
assert np.abs(new_point[2]) <= tiny
new_point = check_construct_rz_matrix(-np.pi / 2, 1, np.array([1, 0, 0]).T)
new_point = check_construct_rz_matrix(-np.pi/2, 1, np.array([1, 0, 0]).T)
assert np.abs(new_point[0]) <= tiny
assert np.abs(new_point[1] + 1) <= tiny
assert np.abs(new_point[2]) <= tiny
......@@ -202,3 +196,75 @@ def test_construct_rz_matrix():
assert np.abs(new_point[0] + 1) <= tiny
assert np.abs(new_point[1]) <= tiny
assert np.abs(new_point[2]) <= tiny
def check_construct_rotm_from_euler(
angle_a, angle_b, angle_c,
sequence, is_in_radians,
point):
""""
Check if the rotation matrix for rotating around the x axis is correct.
:param angle_a: first Euler angle
:param angle_b: second Euler angle
:param angle_c: third Euler angle
: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_m = mat.construct_rotm_from_euler(
angle_a, angle_b, angle_c,
sequence, is_in_radians)
vm.validate_rotation_matrix(rot_m)
new_point = np.matmul(rot_m, point)
assert np.abs(np.linalg.norm(new_point) - np.linalg.norm(point)) <= 0.0001
return new_point
def test_construct_rotm_from_euler():
tiny = 0.0001
new_point = check_construct_rotm_from_euler(
90, -90, 0,
'zyz', 0,
np.array([0, -1, -1]).T)
assert np.abs(new_point[0] - 1) <= tiny
assert np.abs(new_point[1] - 1) <= tiny
assert np.abs(new_point[2]) <= tiny
new_point = check_construct_rotm_from_euler(
np.pi/2, -np.pi/2, 0,
'zyz', 1,
np.array([0, -1, -1]).T)
assert np.abs(new_point[0] - 1) <= tiny
assert np.abs(new_point[1] - 1) <= tiny
assert np.abs(new_point[2]) <= tiny
new_point = check_construct_rotm_from_euler(
90, -90, 0,
'zxz', 0,
np.array([1, 0, -1]).T)
assert np.abs(new_point[0] - 1) <= tiny
assert np.abs(new_point[1] - 1) <= tiny
assert np.abs(new_point[2]) <= tiny
new_point = check_construct_rotm_from_euler(
0, 90, -90,
'zyx', 0,
np.array([0, -1, 1]).T)
assert np.abs(new_point[0] - 1) <= tiny
assert np.abs(new_point[1] - 1) <= tiny
assert np.abs(new_point[2]) <= tiny
new_point = check_construct_rotm_from_euler(
0, 90, -90,
'xyz', 0,
np.array([-1, 0, 1]).T)
assert np.abs(new_point[0] - 1) <= tiny
assert np.abs(new_point[1] - 1) <= tiny
assert np.abs(new_point[2]) <= tiny
test_construct_rotm_from_euler()
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment