如何将运动学轨迹矩阵转换为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.00332 0.00734 0.023578-0.054199 0.998245

所以，

时间戳：1421307756.191874tx：0.004530

ty：0.050319tz:0003332

qx:0003734qy：0.023578

qz：-0.054199qw:0.998245

并且，fx=525.00fy=525.0；//默认焦距

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

我们将不胜感激。

谢谢。