r_cur = R.from_quat(pose_cur[4:8])
Rm_cur = r_cur.as_matrix().reshape(3, 3)
print("Rm_cur: ", Rm_cur)
print("pose_cur: ", pose_cur[1:4][:,None])
Rt_cur = np.concatenate((Rm_cur, pose_cur[1:4][:,None]), 1)
print("Rt_cur: ", Rt_cur)
Rt_cur = np.concatenate((Rt_cur, np.array([[0, 0, 0, 1]])), 0)
print("Rt_cur: ", Rt_cur)
实现结果:
Rm_cur: [[-0.26450476 0.00700511 -0.96435894]
[-0.02100228 -0.9997783 -0.00150188]
[-0.96415566 0.01985648 0.26459325]]
pose_cur: [[-1.076914]
[ 0.492415]
[ 1.329825]]
Rt_cur: [[-0.26450476 0.00700511 -0.96435894 -1.076914 ]
[-0.02100228 -0.9997783 -0.00150188 0.492415 ]
[-0.96415566 0.01985648 0.26459325 1.329825 ]]
Rt_cur: [[-0.26450476 0.00700511 -0.96435894 -1.076914 ]
[-0.02100228 -0.9997783 -0.00150188 0.492415 ]
[-0.96415566 0.01985648 0.26459325 1.329825 ]
[ 0. 0. 0. 1. ]]