1.欧式变换的累加
我们知道,三维空间的刚体运动可分解为旋转和平移两部分,设旋转为R,平移为t,则坐标变换可表示为:b = R * a + t
假设c1点经过c2点变换至w点,则坐标转换过程可由下式表示:
c2 = R1 * c1 + t1
w = R2 * c2 + t2
w = R2 * (R1 * c1 + t1) + t2
表示为矩阵,则如下图所示:
2.平移与旋转的表示
2.1.平移的表示
简单的三维坐标表示如: t =(∆x, ∆y, ∆z)
2.2旋转的表示
2.2.1旋转矩阵
向量在不同坐标系中可由基向量乘以坐标表示:
因向量不变,故在不同坐标系下可建立等式:
等式两边同乘[e1, e2, e3]T,可得,等式左边[e1, e2, e3] * [e1, e2, e3]T 为单位向量,最终得出[a1, a2, a3]T,右边通过计算可得:
则我们得到了旋转矩阵R,即为右式左侧参数,如上图所示。矩阵变换表示为b = a * R
2.2.2欧拉角
欧拉角表示法通过将旋转进行三次分解进行简化,先假设绕X轴旋转度, 绕Y轴旋转
度,绕Z轴旋转
度。由task1中世界坐标系转换为相机坐标系过程易知每次旋转的坐标转移矩阵,将其顺次相乘即为最终的欧拉角表示,计算及图解见下图:
但是欧拉角有两个问题:
1.欧拉角不能进行一一对应,三维空间中的任意一个方向都可以通过至少两种不同欧拉角表示。
2.欧拉角存在万向锁问题:若第一次旋转角为90度,会丢失一个自由度,导致在丢失自由度上被锁死。
2.2.3旋转向量
可以用旋转向量来刻画旋转,对应于一个三维向量,其方向与旋转轴一致,而长度等于旋转角,这种向量称为旋转向量,可以用来描述旋转。
其与旋转矩阵存在着对应的转换关系:罗德里斯公式
其中u为Z轴的单位向量符号,^是向量到反对称矩阵的转换符,为旋转角在XOY平面上的投影角,罗德里斯公式的推导过程如下:
反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角0,取两边的迹
2.2.4四元数
证明过程如下:
其中u,v,
等皆同旋转向量推导。
3.代码实现
# 取四个相机的内参数
# 'id': '54138969',
# 'center': [512.54150390625, 515.4514770507812],
# 'focal_length': [1145.0494384765625, 1143.7811279296875],
# 'radial_distortion': [-0.20709891617298126, 0.24777518212795258, -0.0030751503072679043],
# 'tangential_distortion': [-0.0009756988729350269, -0.00142447161488235],
# 'res_w': 1000,
# 'res_h': 1002,
# 'azimuth': 70,
#
q0, qx, qy, qz = [0.5834008455276489, -0.7853162288665771, 0.14548823237419128, -0.14749594032764435]
R_input = Rotation.from_quat([qx, qy, qz, q0]).as_matrix()
R_new = np.array([[0, -1, 0],
[1, 0, 0],
[0, 0, 1]])
R_output = np.dot(R_new, R_input)
q = Rotation.from_matrix(R_output).as_quat()
# 平移向量的修改
translation = [2044.45849609375, 4935.1171875, 1481.2275390625]
translation_new = np.array([0, 0, 1])
translation += translation_new
translation = translation.reshape((-1, 1))
R = np.append(R_output, translation, axis=1)
# 从相机参数字典中获取内部参数
Kd_list = []
T_list = []
M_list = []
for i in range(4):
focal_length_x, focal_length_y = h36m_cameras_intrinsic_params[i]['focal_length']
center_x, center_y = h36m_cameras_intrinsic_params[i]['center']
theta = np.radians(h36m_cameras_intrinsic_params[i]['azimuth'])
# 从相机参数字典中获取畸变参数
k1, k2, k3 = h36m_cameras_intrinsic_params[i]['radial_distortion']
Kd = [k1, k2, k3]
t1, t2 = h36m_cameras_intrinsic_params[i]['tangential_distortion']
T = [t1, t2]
K = np.array([
[focal_length_x, -focal_length_x * (1 / np.tan(theta)), center_x],
[0, focal_length_y / np.sin(theta), center_y],
[0, 0, 1]
])
M = np.dot(K, R)
M_list.append(M)
Kd_list.append(Kd)
T_list.append(T)
p = np.array([2, 3, 4])
p_new = np.dot(p, M_list[0])
r2 = p_new[0] ** 2 + p_new[1] ** 2
xc = p_new[0] * (1 + Kd_list[0][0] * r2 + Kd_list[0][1] * (r2 ** 2) + Kd_list[0][2] * (r2 ** 3)) + (
2 * T_list[0][0] * p_new[0] * p_new[1] + T_list[0][1] * (r2 + 2 * (p_new[0] ** 2)))
yc = p_new[1] * (1 + Kd_list[0][0] * r2 + Kd_list[0][1] * (r2 ** 2) + Kd_list[0][2] * (r2 ** 3)) + (
2 * T_list[0][1] * p_new[0] * p_new[1] + T_list[0][0] * (r2 + 2 * (p_new[1] ** 2)))
p_new = [xc, yc]
print(p_new)