齐次矩阵与位姿的转换

姿态（旋转矩阵，空间坐标）——齐次矩阵

def Rm_t2T(Rm, t):
    return np.array([list(Rm[0]) + [t[0]],
                     list(Rm[1]) + [t[1]],
                     list(Rm[2]) + [t[2]],
                     [0, 0, 0, 1]])

t_imu2left = [-0.037781689316,-0.042011231184, 0.35873106122]
Rm_imu2left = np.array([[-0.8508201,-0.27519914,0.44762773],
 [-0.34178448,-0.35720245,-0.86924667]
 ,[ 0.39910966,-0.89256475,0.20985623]])
T_imu2left = Rm_t2T(Rm_imu2left, t_imu2left)

齐次矩阵——姿态

array1 = np.array([T_imu2right[0,0:3], T_imu2right[1,0:3], T_imu2right[2,0:3]])
Rc_xyz = [T_imu2right[0,3:4][0],T_imu2right[1,3:4][0],T_imu2right[2,3:4][0]]