def test_rotate():
n_frames = 100
n_markers = 10
angles = Angles.from_random_data(size=(3, 1, n_frames))
rt = Rototrans.from_euler_angles(angles, "xyz")
markers = Markers.from_random_data(size=(3, n_markers, n_frames))
rotated_markers = Markers.from_rototrans(markers, rt)
expected_rotated_marker = np.ndarray((4, n_markers, n_frames))
for marker in range(n_markers):
for frame in range(n_frames):
expected_rotated_marker[:, marker, frame] = np.dot(
rt.isel(time=frame),
markers.isel(channel=marker, time=frame),
)
np.testing.assert_array_almost_equal(rotated_markers,
expected_rotated_marker,
decimal=10)
rotated_markers = Markers.from_rototrans(markers.isel(time=0),
rt.isel(time=0))
expected_rotated_marker = np.ndarray(rotated_markers.shape)
for marker in range(n_markers):
expected_rotated_marker[:, marker] = np.dot(
rt.isel(time=0), markers.isel(channel=marker, time=0))
np.testing.assert_array_almost_equal(rotated_markers,
expected_rotated_marker,
decimal=10)
rotated_markers = Markers.from_rototrans(markers, rt.isel(time=0))
expected_rotated_marker = np.ndarray(rotated_markers.shape)
for marker in range(n_markers):
expected_rotated_marker[:,
marker] = np.dot(rt.isel(time=0),
markers.isel(channel=marker))
np.testing.assert_array_almost_equal(rotated_markers,
expected_rotated_marker,
decimal=10)
with pytest.raises(ValueError):
Markers.from_rototrans(markers.isel(time=0), rt)
'ipopt.hessian_approximation': "exact",
# 'ipopt.tol':1e-10,'ipopt.dual_inf_tol':1e-15
}
solver = nlpsol('solver', 'ipopt', prob, options)
w0 = np.zeros((6, 1))
# w0[0]= marker_model[0,0,0] - marker_treat[0,0,0]
# w0[1]= marker_model[1,0,0] - marker_treat[1,0,0]
# w0[2]= marker_model[2,0,0] - marker_treat[2,0,0]
solve = solver(x0=w0, lbx=-1000, ubx=10000)
w_opt = solve['x']
print(w_opt)
rt = Rototrans((RT(w_opt[0], w_opt[1], w_opt[2], w_opt[3], w_opt[4],
w_opt[5])))
# rt = Rototrans((RT(0.841114, 0.103637, -0.596597, -0.458244, 0.145129, 3.21253)))
marker_rotate = Markers.from_rototrans(marker_treat, rt)
# Data for three-dimensional scattered points
ax = plt.axes(projection='3d')
zdata = marker_rotate[2, :, 0]
xdata = marker_rotate[0, :, 0]
ydata = marker_rotate[1, :, 0]
ax.scatter3D(xdata, ydata, zdata, cmap='Greens')
zdata = marker_model[2, :, 0]
xdata = marker_model[0, :, 0]
ydata = marker_model[1, :, 0]
ax.scatter3D(xdata, ydata, zdata, edgecolors="red")
ax.view_init(60, 35)
plt.show()
# nb_frame = range(1,marker_rotate.shape[2]+1)
# anb_frame = np.ndarray((1,marker_rotate.shape[2]))