smoothly-vslam学习笔记（3.三维空间刚体运动）

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)