def test_construct_rt():
# Test the ways to create an eye matrix
eye = RotoTrans()
np.testing.assert_equal(eye.get_num_frames(), 1)
np.testing.assert_equal(eye[:, :, 0], np.eye(4))
eye = RotoTrans(RotoTrans.rt_from_euler_angles())
np.testing.assert_equal(eye.get_num_frames(), 1)
np.testing.assert_equal(eye[:, :, 0], np.eye(4))
# Test the way to create a rt, but not when providing bot angles and sequence
# this is done in test_euler2rot_rot2euler
nb_frames = 10
random_vector = FrameDependentNpArray(np.random.rand(3, 1, nb_frames))
random_from_angles = RotoTrans(
RotoTrans.rt_from_euler_angles(angles=random_vector,
angle_sequence="xyz"))
np.testing.assert_equal(random_from_angles.get_num_frames(), nb_frames)
np.testing.assert_equal(random_from_angles[0:3, 3, :],
np.zeros((3, 1, nb_frames))) # Translation is 0
random_from_translations = RotoTrans(
RotoTrans.rt_from_euler_angles(translations=random_vector))
np.testing.assert_equal(random_from_translations.get_num_frames(),
nb_frames)
np.testing.assert_equal(
random_from_translations[0:3, 0:3, :],
np.repeat(np.eye(3)[:, :, np.newaxis], nb_frames, axis=2),
) # rotation is eye3
def mean(self):
"""
Returns
-------
Performs an optimization to compute the mean over the frames
"""
# Chose an arbitrary angle sequence to convert into angle during the optimization
seq = "xyz"
# Compute the element-wise mean for the optimization to target
rt_mean = super(RotoTrans, self).mean()
# Define the objective function
x_tp = FrameDependentNpArray(np.ndarray((3, 1, 1)))
def obj(x):
x_tp[0:3, 0, 0] = x.reshape(-1, 1)
rt = RotoTrans(angles=x_tp, angle_sequence=seq)
return (rt[0:3, 0:3] - rt_mean[0:3, 0:3]).reshape(9)
# Initial guess of the optimization
x0 = np.squeeze(rt_mean.get_euler_angles(seq))
# Call the optimizer
x_tp[0:3, 0, 0] = least_squares(obj, x0).x.reshape(-1, 1)
return RotoTrans(angles=x_tp,
angle_sequence=seq,
translations=rt_mean[0:3, 3, :])
def __new__(cls,
rt=np.eye(4),
angles=FrameDependentNpArray(),
angle_sequence="",
translations=FrameDependentNpArray(),
*args,
**kwargs):
"""
Parameters
----------
rt : FrameDependentNpArray (4x4xF)
Rototranslation matrix sorted in 4x4xF, default is the matrix that don't rotate nor translate the system, is
ineffective if angles is provided
angles : FrameDependentNpArray
Euler angles of the rototranslation, angles parameter is ineffective if angles_sequence if not defined, but
will override rt
angle_sequence : str
Euler sequence of angles; valid values are all permutation of 3 axes (e.g. "xyz", "yzx", ...)
translations : FrameDependentNpArray
First 3 rows of 4th row, translation is ineffective if angles is not provided
"""
# Determine if we construct RotoTrans from rt or angles/translations
if angle_sequence:
rt = cls.rt_from_euler_angles(angles=angles,
angle_sequence=angle_sequence,
translations=translations)
else:
s = rt.shape
if s[0] != 4 or s[1] != 4:
raise IndexError("RotoTrans must by a 4x4xF matrix")
# Make sure last line reads [0, 0, 0, 1]
if len(s) == 2:
rt[3, :] = np.array([0, 0, 0, 1])
else:
rt[3, 0:3, :] = 0
rt[3, 3, :] = 1
# Finally, we must return the newly created object:
return super(RotoTrans, cls).__new__(cls, array=rt, *args, **kwargs)
"yx",
"yz",
"zx",
"zy",
"xyz",
"xzy",
"yxz",
"yzx",
"zxy",
"zyx",
"zyzz",
]
# If the difference between the initial and the final angles are less than epsilon, tests is success
EPSILON = 1e-12
# Define some random data to tests
ANGLES = FrameDependentNpArray(np.random.rand(40, 1, 100))
def test_construct_rt():
# Test the ways to create an eye matrix
eye = RotoTrans()
np.testing.assert_equal(eye.get_num_frames(), 1)
np.testing.assert_equal(eye[:, :, 0], np.eye(4))
eye = RotoTrans(RotoTrans.rt_from_euler_angles())
np.testing.assert_equal(eye.get_num_frames(), 1)
np.testing.assert_equal(eye[:, :, 0], np.eye(4))
# Test the way to create a rt, but not when providing bot angles and sequence
# this is done in test_euler2rot_rot2euler
nb_frames = 10
markers_color=(0, 0, 1),
markers_size=10.0,
markers_opacity=.5)
vtkModelByNames = VtkModel(vtkWindow,
markers_color=(0, 1, 1),
markers_size=10.0,
markers_opacity=.5)
vtkModelFromC3d = VtkModel(vtkWindow,
markers_color=(0, 1, 0),
markers_size=10.0,
markers_opacity=.5)
# Create some RotoTrans attached to the first model
all_rt_real = RotoTransCollection()
all_rt_real.append(
RotoTrans(angles=FrameDependentNpArray(np.zeros((3, 1, 1))),
angle_sequence="yxz",
translations=d[:, 0, 0:1]))
all_rt_real.append(
RotoTrans(angles=FrameDependentNpArray(np.zeros((3, 1, 1))),
angle_sequence="yxz",
translations=d[:, 0, 0:1]))
# Create some RotoTrans attached to the second model
one_rt = RotoTrans.define_axes(d, [3, 5], [[4, 3], [4, 5]], "zx", "z",
[3, 4, 5])
# Create some mesh (could be from any mesh source)
meshes = MeshCollection()
meshes.append(Mesh(vertex=d, triangles=[[0, 1], [5, 0], [1, 6]]))
def get_euler_angles(self, angle_sequence):
"""
Parameters
----------
angle_sequence : str
Euler sequence of angles; valid values are all permutation of axes (e.g. "xyz", "yzx", ...)
Returns
-------
angles : Markers3d
Euler angles associated with RotoTrans
"""
if angle_sequence != "zyzz":
angles = FrameDependentNpArray(
np.ndarray((len(angle_sequence), 1, self.get_num_frames())))
else:
angles = FrameDependentNpArray(
np.ndarray((3, 1, self.get_num_frames())))
if angle_sequence == "x":
angles[0, :, :] = np.arcsin(self[2, 1, :])
elif angle_sequence == "y":
angles[0, :, :] = np.arcsin(self[0, 2, :])
elif angle_sequence == "z":
angles[0, :, :] = np.arcsin(self[1, 0, :])
elif angle_sequence == "xy":
angles[0, :, :] = np.arcsin(self[2, 1, :])
angles[1, :, :] = np.arcsin(self[0, 2, :])
elif angle_sequence == "xz":
angles[0, :, :] = -np.arcsin(self[1, 2, :])
angles[1, :, :] = -np.arcsin(self[0, 1, :])
elif angle_sequence == "yx":
angles[0, :, :] = -np.arcsin(self[2, 0, :])
angles[1, :, :] = -np.arcsin(self[1, 2, :])
elif angle_sequence == "yz":
angles[0, :, :] = np.arcsin(self[0, 2, :])
angles[1, :, :] = np.arcsin(self[1, 0, :])
elif angle_sequence == "zx":
angles[0, :, :] = np.arcsin(self[1, 0, :])
angles[1, :, :] = np.arcsin(self[2, 1, :])
elif angle_sequence == "zy":
angles[0, :, :] = -np.arcsin(self[0, 1, :])
angles[1, :, :] = -np.arcsin(self[2, 0, :])
elif angle_sequence == "xyz":
angles[0, :, :] = np.arctan2(self[1, 2, :], self[2, 2, :])
angles[1, :, :] = np.arcsin(self[0, 1, :])
angles[2, :, :] = np.arctan2(-self[0, 1, :], self[0, 0, :])
elif angle_sequence == "xzy":
angles[0, :, :] = np.arctan2(self[2, 1, :], self[1, 1, :])
angles[2, :, :] = np.arctan2(self[0, 2, :], self[0, 0, :])
angles[1, :, :] = -np.arcsin(self[0, 1, :])
elif angle_sequence == "xzy":
angles[1, :, :] = -np.arcsin(self[1, 2, :])
angles[0, :, :] = np.arctan2(self[0, 2, :], self[2, 2, :])
angles[2, :, :] = np.arctan2(self[1, 0, :], self[1, 1, :])
elif angle_sequence == "yzx":
angles[2, :, :] = np.arctan2(-self[1, 2, :], self[1, 1, :])
angles[0, :, :] = np.arctan2(-self[2, 0, :], self[0, 0, :])
angles[1, :, :] = np.arcsin(self[1, 2, :])
elif angle_sequence == "zxy":
angles[1, :, :] = np.arcsin(self[2, 1, :])
angles[2, :, :] = np.arctan2(-self[2, 0, :], self[2, 2, :])
angles[0, :, :] = np.arctan2(-self[0, 1, :], self[1, 1, :])
elif angle_sequence == "zyz":
angles[0, :, :] = np.arctan2(self[1, 2, :], self[0, 2, :])
angles[1, :, :] = np.arccos(self[2, 2, :])
angles[2, :, :] = np.arctan2(self[2, 1, :], -self[2, 0, :])
elif angle_sequence == "zxz":
angles[0, :, :] = np.arctan2(self[0, 2, :], -self[1, 2, :])
angles[1, :, :] = np.arccos(self[2, 2, :])
angles[2, :, :] = np.arctan2(self[2, 0, :], self[2, 1, :])
elif angle_sequence == "zyzz":
angles[0, :, :] = np.arctan2(self[1, 2, :], self[0, 2, :])
angles[1, :, :] = np.arccos(self[2, 2, :])
angles[2, :, :] = np.arctan2(self[2, 1, :], -self[2, 0, :])
return angles
def rt_from_euler_angles(
angles=FrameDependentNpArray(),
angle_sequence="",
translations=FrameDependentNpArray(),
):
"""
Parameters
----------
angles : FrameDependentNpArray
Euler angles of the rototranslation
angle_sequence : str
Euler sequence of angles; valid values are all permutation of axes (e.g. "xyz", "yzx", ...)
translations : FrameDependentNpArray
Translation part of the Rototrans matrix
Returns
-------
rt : RotoTrans
The rototranslation associated to the input parameters
"""
# Convert special zyzz angle sequence to zyz
if angle_sequence == "zyzz":
angles[2, :, :] -= angles[0, :, :]
angle_sequence = "zyz"
# If the user asked for a pure rotation
if angles.get_num_frames() != 0 and translations.get_num_frames() == 0:
translations = FrameDependentNpArray(
np.zeros((3, 1, angles.get_num_frames())))
# If the user asked for a pure translation
if angles.get_num_frames() == 0 and translations.get_num_frames() != 0:
angles = FrameDependentNpArray(
np.zeros((0, 1, translations.get_num_frames())))
# Sanity checks
if angles.get_num_frames() != translations.get_num_frames():
raise IndexError(
"angles and translations must have the same number of frames")
if angles.shape[0] is not len(angle_sequence):
raise IndexError(
"angles and angles_sequence must be the same size")
if angles.get_num_frames() == 0:
return RotoTrans()
rt_out = np.repeat(np.eye(4)[:, :, np.newaxis],
angles.get_num_frames(),
axis=2)
try:
for i in range(len(angles)):
a = angles[i, :, :]
matrix_to_prod = np.repeat(np.eye(4)[:, :, np.newaxis],
angles.get_num_frames(),
axis=2)
if angle_sequence[i] == "x":
# [[1, 0 , 0 ],
# [0, cos(a), -sin(a)],
# [0, sin(a), cos(a)]]
matrix_to_prod[1, 1, :] = np.cos(a)
matrix_to_prod[1, 2, :] = -np.sin(a)
matrix_to_prod[2, 1, :] = np.sin(a)
matrix_to_prod[2, 2, :] = np.cos(a)
elif angle_sequence[i] == "y":
# [[ cos(a), 0, sin(a)],
# [ 0 , 1, 0 ],
# [-sin(a), 0, cos(a)]]
matrix_to_prod[0, 0, :] = np.cos(a)
matrix_to_prod[0, 2, :] = np.sin(a)
matrix_to_prod[2, 0, :] = -np.sin(a)
matrix_to_prod[2, 2, :] = np.cos(a)
elif angle_sequence[i] == "z":
# [[cos(a), -sin(a), 0],
# [sin(a), cos(a), 0],
# [0 , 0 , 1]]
matrix_to_prod[0, 0, :] = np.cos(a)
matrix_to_prod[0, 1, :] = -np.sin(a)
matrix_to_prod[1, 0, :] = np.sin(a)
matrix_to_prod[1, 1, :] = np.cos(a)
else:
raise ValueError(
"angle_sequence must be a permutation of axes (e.g. "
"xyz"
", "
"yzx"
", ...)")
rt_out = np.einsum("ijk,jlk->ilk", rt_out, matrix_to_prod)
except IndexError:
raise ValueError(
"angle_sequence must be a permutation of axes (e.g. "
"xyz"
", "
"yzx"
", ...)")
# Put the translations
rt_out[0:3, 3:4, :] = translations[0:3, :, :]
return RotoTrans(rt_out)
d5 = Markers3d.from_c3d(MARKERS_ANALOGS_C3D, idx=[[0], [1], [2]], prefix=":")
# Create a windows with a nice gray background
vtkWindow = VtkWindow(background_color=(0.5, 0.5, 0.5))
# Add marker holders to the window
vtkModelReal = VtkModel(vtkWindow, markers_color=(1, 0, 0), markers_size=10.0, markers_opacity=1)
vtkModelPred = VtkModel(vtkWindow, markers_color=(0, 0, 0), markers_size=10.0, markers_opacity=0.5)
vtkModelMid = VtkModel(vtkWindow, markers_color=(0, 0, 1), markers_size=10.0, markers_opacity=0.5)
vtkModelByNames = VtkModel(vtkWindow, markers_color=(0, 1, 1), markers_size=10.0, markers_opacity=0.5)
vtkModelFromC3d = VtkModel(vtkWindow, markers_color=(0, 1, 0), markers_size=10.0, markers_opacity=0.5)
# Create some RotoTrans attached to the first model
all_rt_real = RotoTransCollection()
all_rt_real.append(
RotoTrans(angles=FrameDependentNpArray(np.zeros((3, 1, 1))), angle_sequence="yxz", translations=d[:, 0, 0:1])
)
all_rt_real.append(
RotoTrans(angles=FrameDependentNpArray(np.zeros((3, 1, 1))), angle_sequence="yxz", translations=d[:, 0, 0:1])
)
# Create some RotoTrans attached to the second model
one_rt = RotoTrans.define_axes(d, [3, 5], [[4, 3], [4, 5]], "zx", "z", [3, 4, 5])
# Create some mesh (could be from any mesh source)
meshes = MeshCollection()
meshes.append(Mesh(vertex=d, triangles=[[0, 1], [5, 0], [1, 6]]))
# Animate all this
i = 0
while vtkWindow.is_active: