c++ - 如何将kinect轨迹矩阵转换为4x4齐次矩阵（示例）

最近我使用RGBD-Slam来获取kinect相机轨迹。每个框架都有：

时间戳tx ty tz qx qy qz qw

tx ty tz 根据动作捕捉系统的定义，给出彩色摄像机光学中心相对于世界原点的位置。

qx qy qz qw 以运动捕捉系统定义的相对于世界原点的单位四元数的形式给出彩色相机的光学中心的方向。

我需要将其转换为4x4齐次变换矩阵。任何解决这个问题的提示？我们举个例子如下：

1421307756.191874 0.004530 0.050319 0.003332 0.003734 0.023578 -0.054199 0.998245

所以，

时间戳：1421307756.191874; tx ：0.004530

ty ：0.050319; tz ：0.003332

qx ：0.003734; qy ：0.023578

qz ： - 0.054199; qw ：0.998245

和， fx = 525.0; fy = 525.0; //默认焦距

cx = 319.5; cy = 239.5; //默认光学中心

非常感谢善意的协助。

感谢。