位姿变换
各坐标系变换总结
旋转矩阵的变换
坐标系运动方向正负判定
-
坐标系运动方向判断：右手使用右手螺旋定则，左手使用左手螺旋定则，大拇指方向为坐标轴指向，四指指向为正
-
坐标系运动方向的正负和坐标点运动的方向相反
变换顺序
在右手坐标系中，是左结合，即最后一个矩阵最先应用
静态坐标轴旋转和动态坐标轴旋转
如何从坐标系变换得到在初始坐标系下变换后的点的坐标？
坐标系绕动态轴运算：每一次是按变换后的新的坐标系的轴运算，得到的角度取负再带入R矩阵求变换矩阵
与坐标按初始坐标系固定轴旋转效果一致。
变换实例
下面以cityscapes涉及的右手相机坐标系O1转换到右手标准相机坐标系O2为例：
- 首先绕O1 x轴 旋转-90°，得到坐标系M1（forward:x, left:z, down:y)；（相当于某点绕O1x轴旋转+90°）
- 再绕M1的y轴旋转+90°，得到坐标系O2；（相当于某点再绕M1y轴旋转-90°）
综上变换矩阵为 T = Ry(-90)Rx(90)
# 向量绕固定轴旋转
R = euler_matrix(np.radians(90), np.radians(-90), 0, 'sxyz')
x = np.array([[1,0,0,1]])
y = np.array([[0,1,0,1]])
z = np.array([[0,0,1,1]])
z_new = R @ x.T
neg_x_new = R @ y.T
neg_y_new = R @ z.T
print(neg_x_new.T, neg_y_new.T, z_new.T) # x->z; y->neg_x, z->neg_y
"""
eg_x_new.T, neg_y_new.T, z_new.T) # x->z; y->neg_x, z->neg_y
8
[[-1.000000e+00 6.123234e-17 6.123234e-17 1.000000e+00]] [[-6.12323400e-17 -1.00000000e+00 3.74939946e-33 1.00000000e+00]] [[6.123234e-17 0.000000e+00 1.000000e+00 1.000000e+00]]
"""
#经过验证，O1中的坐标转换到O2中的坐标旋转矩阵定义
-
R旋转矩阵的定义是对坐标变换的定义，正向按上述方向确定，以下是右手坐标系R的定义
$$R_x(\theta) = \left[ \begin{matrix} 1 & 0 & 0 \newline 0 & cos \theta & -sin \theta \newline 0 & sin \theta & cos \theta \end{matrix} \right]$$
$$R_y(\theta) = \left[ \begin{matrix} cos \theta & 0 & sin \theta \newline 0 & 1 & 0 \newline-sin \theta & 0 & cos \theta \end{matrix} \right]$$
$$R_z(\theta) = \left[ \begin{matrix} cos \theta & -sin \theta & 0 \newline sin \theta & cos \theta & 0 \newline 0 & 0 & 1 \end{matrix} \right]$$ -
以下是左手坐标系定义
$$R_x(\theta) = \left[ \begin{matrix} 1 & 0 & 0 \newline 0 & cos \theta & sin \theta \newline 0 & -sin \theta & cos \theta \end{matrix} \right]$$
$$R_y(\theta) = \left[ \begin{matrix} cos \theta & 0 & -sin \theta \newline 0 & 1 & 0 \newline sin \theta & 0 & cos \theta \end{matrix} \right]$$
$$R_z (\theta) = \left[ \begin{matrix} cos \theta & sin \theta & 0 \newline -sin \theta & cos \theta & 0 \newline
0 & 0 & 1 \end{matrix} \right]$$
-
以下是绕任意轴n旋转
$$R( \hat n ,\theta) = \left[ \begin{matrix} n_x^2(1 - cos \theta) + cos \theta & n_x n_y(1 - cos \theta) + n_z sin \theta & n_x n_z (1 - cos \theta) - n_y sin \theta \newline
n_x n_y (1 - cos \theta) - n_z sin \theta & n_y ^ 2 ( 1- cos \theta) +cos \theta & n_y n_z (1 - cos \theta) + n_x sin \theta \newline
n_x n_z（1 - cos \ theta) + n_y sin \theta & n_y n_z (1 - cos\theta) - n_x sin \theta & n_x^2(1-cos \theta) + cos \theta
\end{matrix} \right]$$
左手坐标系变换矩阵 转换到 右手坐标系变换矩阵
已知某点在左手坐标系O1中的变换矩阵Q，相当于该点坐标不变，左手坐标系 O1 -> O2应用了变换矩阵Q'。下面对坐标系的变换，本质上还是求该点在原始坐标系中的变换。已知左手坐标系 O1 -> O2的变换，如何求相应的在右手坐标系M1->M2的变换矩阵？假设同一个物体在O1，O2中的点为$p_{o1} = [x, y, z], p_{o2} = [x', y', z']$, 则要求 $p_{m1} = [x, -y, z], p_{m2} =[x', -y', z']$的变换。
直接变换推导
已知$R_l = [r_{ij}]$ 为o1->o2的旋转矩阵，$T_l = [t_i]$为o1->o2的平移向量， $ 0 <= i <=2, 0<=j<=2$
则$p_{o1}->p_{o2}$ 可以表示为：
$$p_{o2} = R_l * p_{o1} + T_l$$
则$p_{m1}->p_{m2}$ 可以表示为：
$$p_{m2} = S_{y} * p_{o2} $$ (坐标系O2,M2只是y轴相反)
$$= S_y (R_l * p_{o1} + T_l) \ = S_y(R_l * p_{o1}) + S_y * T_l$$ (矩阵乘法的分配律)
$$ = S_y R_l * p_{o1} + S_y * T_l$$ ( 矩阵乘法的结合律)
$$ = S_y R_l *S_y p_{m1} + S_y * T_l -----(1)$$
综上，同一个物体在左手系中的转换变换到右手系中，平移矩阵需要沿某一个坐标轴取反（不一定是例子中的y轴），也就是$S_y$矩阵的作用，旋转矩阵需要变换为$S_y *R_l *S_y$, 平移矩阵变换为 $ S_y * T_l$.
def euler_matrix_left(roll, pitch, yaw ):
c_y = np.cos(np.radians(yaw))
s_y = np.sin(np.radians(yaw))
c_r = np.cos(np.radians(roll))
s_r = np.sin(np.radians(roll))
c_p = np.cos(np.radians(pitch))
s_p = np.sin(np.radians(pitch))
matrix = np.matrix(np.identity(3))
matrix[0, 0] = c_p * c_y
matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
matrix[1, 0] = s_y * c_p
matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
matrix[2, 0] = s_p
matrix[2, 1] = -c_p * s_r
matrix[2, 2] = c_p * c_r
return matrix
R_left = euler_matrix_left(np.radians(50), np.radians(-16) , np.radians(-78))
T_left = np.array([[-5.7, -8.9, -10]])
S_y = np.array([[1, 0, 0], [0, -1, 0], [0, 0, 1]])
R_right = S_y @ R_left @ S_y #见公式1
T_right = np.array([[-5.7, 8.9, -10]])
p_left = np.array([[-19, -20, 89]])
p_right = np.array([[-19, 20, 89]])
p_left_new = R_left @ p_left.T + T_left.T
p_right_new = R_right @ p_right.T + T_right.T
print(p_left_new.T)
print(p_right_new.T)
# 变换后在O1中为[[48.77249475 0.91216204 64.95644597]]，变换后在M1中为[[48.77249475 -0.91216204 64.95644597]]
# 实际是同一物体通过欧拉角推导
假设物体在坐标系o1绕X,Y,Z轴旋转了roll，pitch，yaw度，于o1变换到了o2， 则：
- 物体在o1中绕x轴旋转了roll，相当于物体在M1中绕x轴旋转了-roll
- 物体在o1中绕y轴旋转了pitch，相当于物体在M1中绕y轴旋转了pitch
- 物体在o1中绕z轴旋转了yaw，相当于物体在M1中绕z轴旋转了-yaw
已知物体在坐标系o1平移$[dx, dy, dz]$，则相当于在M1中平移了$[dx, -dy, dz]$
综上， M1变换到M2的旋转矩阵是$ R_z(-yaw) *R_y(pitch)*Rx(-roll)$。M1变换到M2的平移向量为T= [dx, -dy, dz]。
待验证
carla/UE4左手相机坐标系到标准相机坐标系验证
# 向量绕固定轴旋转
R1 = euler_matrix(np.radians(90), np.radians(-90), 0, 'sxyz')
# 坐标轴绕固定轴旋转
R2 = euler_matrix(0, np.radians(90), np.radians(-90), 'szyx')
R2 = np.linalg.inv(R2)
point = np.array([[0,0,1,1]])
point_t = R1 @ point.T
print(point_t) # z -> -y
print(R1)# R1 R2相等
print(R2)
"""
[[-6.12323400e-17]
[-1.00000000e+00]
[ 3.74939946e-33]
[ 1.00000000e+00]]
[[ 6.12323400e-17 -1.00000000e+00 -6.12323400e-17 0.00000000e+00]
[ 0.00000000e+00 6.12323400e-17 -1.00000000e+00 0.00000000e+00]
[ 1.00000000e+00 6.12323400e-17 3.74939946e-33 0.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
[[ 6.12323400e-17 -1.00000000e+00 -6.12323400e-17 -0.00000000e+00]
[-0.00000000e+00 6.12323400e-17 -1.00000000e+00 -0.00000000e+00]
[ 1.00000000e+00 6.12323400e-17 3.74939946e-33 0.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
"""S = np.array([[1, 0 ,0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
# 向量绕固定轴旋转
R = euler_matrix(np.radians(90), np.radians(-90), 0, 'sxyz')
T = R @ S
x = np.array([[1,0,0,1]])
y = np.array([[0,1,0,1]])
z = np.array([[0,0,1,1]])
z_new = T @ x.T
x_new = T @ y.T
neg_y_new = T @ z.T
print(x_new.T, neg_y_new.T, z_new.T) # x->z; y->x, z->neg_y
"""
[[ 1.000000e+00 -6.123234e-17 -6.123234e-17 1.000000e+00]] [[-6.12323400e-17 -1.00000000e+00 3.74939946e-33 1.00000000e+00]] [[6.123234e-17 0.000000e+00 1.000000e+00 1.000000e+00]]
"""blender右手坐标系到标准相机坐标系验证
R = euler_matrix(np.radians(180), 0 , 0, 'sxyz')
x = np.array([[1,0,0,1]])
y = np.array([[0,1,0,1]])
z = np.array([[0,0,1,1]])
x_new = R @ x.T
neg_y_new = R @ y.T
neg_z_new = R @ z.T
print(x_new.T, neg_y_new.T, neg_z_new.T) # x->x; y->-y, z->-z
"""
[[1. 0. 0. 1.]] [[ 0.0000000e+00 -1.0000000e+00 1.2246468e-16 1.0000000e+00]] [[ 0.0000000e+00 -1.2246468e-16 -1.0000000e+00 1.0000000e+00]]
"""cityscapes 右手相机坐标系到标准相机坐标系验证
# 向量绕固定轴旋转
R = euler_matrix(np.radians(90), np.radians(-90), 0, 'sxyz')
x = np.array([[1,0,0,1]])
y = np.array([[0,1,0,1]])
z = np.array([[0,0,1,1]])
z_new = R @ x.T
neg_x_new = R @ y.T
neg_y_new = R @ z.T
print(neg_x_new.T, neg_y_new.T, z_new.T) # x->z; y->neg_x, z->neg_y
"""
eg_x_new.T, neg_y_new.T, z_new.T) # x->z; y->neg_x, z->neg_y
8
[[-1.000000e+00 6.123234e-17 6.123234e-17 1.000000e+00]] [[-6.12323400e-17 -1.00000000e+00 3.74939946e-33 1.00000000e+00]] [[6.123234e-17 0.000000e+00 1.000000e+00 1.000000e+00]]
"""def euler_matrix_left(roll, pitch, yaw ):
c_y = np.cos(yaw)
s_y = np.sin(yaw)
c_r = np.cos(roll)
s_r = np.sin(roll)
c_p = np.cos(pitch)
s_p = np.sin(pitch)
matrix = np.matrix(np.identity(3))
matrix[0, 0] = c_p * c_y
matrix[0, 1] = c_y * s_p * s_r - s_y * c_r
matrix[0, 2] = -c_y * s_p * c_r - s_y * s_r
matrix[1, 0] = s_y * c_p
matrix[1, 1] = s_y * s_p * s_r + c_y * c_r
matrix[1, 2] = -s_y * s_p * c_r + c_y * s_r
matrix[2, 0] = s_p
matrix[2, 1] = -c_p * s_r
matrix[2, 2] = c_p * c_r
return matrix
R_left = euler_matrix_left(np.radians(50), np.radians(-16) , np.radians(-78))
T_left = np.array([[-5.7, -8.9, -10]])