def test_quaternion_slerp_batch():
random_state = np.random.RandomState(229)
q_start = pr.random_quaternion(random_state)
q_end = pr.random_quaternion(random_state)
t = np.linspace(0, 1, 101)
Q = pbr.quaternion_slerp_batch(q_start, q_end, t)
for i in range(len(t)):
qi = pr.quaternion_slerp(q_start, q_end, t[i])
pr.assert_quaternion_equal(Q[i], qi)
canvas_y, canvas_x, _ = canvas.shape
# canvas = np.zeros((canvas_y, canvas_x, 3), dtype=np.uint8)
# 读取纸张
paper = cv2.imread(r'input\paper.png')
jmax, imax, _ = paper.shape
# 纸张放在画布的初始位置，这里定位中心，并建立起纸张坐标->画布坐标的转换关系
dx, dy = (canvas_x - imax)//2, (canvas_y-jmax)//2
def paper2canvas(x, y):
return x+dx, y+dy
# 通过欧拉角获得旋转的四元数
z_rot, y_rot, x_rot = 30, 30, 15
rot_matrix = active_matrix_from_extrinsic_euler_zyx(((z_rot/180*np.pi), (y_rot/180*np.pi), (x_rot/180*np.pi)))
qs = quaternion_slerp_batch(q_id, quaternion_from_matrix(rot_matrix), np.linspace(0, 1, n_steps))
def do_rot(v, t):
'''对t时刻时的向量（点）v做旋转变换'''
return q_prod_vector(qs[t], v)
# 除了旋转之外，还会有平移
trans_start, trans_max = (0, 0, 0), (0, 0, 1600)
trans = np.linspace(trans_start, trans_max, n_steps)
def do_trans(v, t):
return (v[0]+trans[t][0]), (v[1]+trans[t][1]), (v[2]+trans[t][2])
# 透视的话需要知道眼睛的位置
eye = (imax//2, jmax//2, -1600)
def proj(v):
'''将空间坐标映射到纸张初始平面'''
def test_quaternion_slerp_batch_zero_angle():
random_state = np.random.RandomState(228)
q = pr.random_quaternion(random_state)
Q = pbr.quaternion_slerp_batch(q, q, [0.5])
pr.assert_quaternion_equal(q, Q[0])
"""
===============
Pose Trajectory
===============
Plotting pose trajectories with pytransform3d is easy.
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform3d.batch_rotations import quaternion_slerp_batch
from pytransform3d.rotations import q_id
from pytransform3d.trajectories import plot_trajectory
n_steps = 100000
P = np.empty((n_steps, 7))
P[:, 0] = np.cos(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 1] = np.sin(np.linspace(-2 * np.pi, 2 * np.pi, n_steps))
P[:, 2] = np.linspace(-1, 1, n_steps)
q_end = np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)])
P[:, 3:] = quaternion_slerp_batch(q_id, q_end, np.linspace(0, 1, n_steps))
ax = plot_trajectory(P=P,
s=0.3,
n_frames=100,
normalize_quaternions=False,
lw=2,
c="k")
plt.show()
