如何从四元数计算单应矩阵

Question

我最近正在搜索ar。无人机2.0姿态估计， 我想通过使用ROS平台结合 imu数据（四元数）来找到两帧之间的单应矩阵， 但是我发现的单应性似乎很奇怪...

我得到的数据就像：

-frame1.png，Orientation_x，Orientation_y，Orientation_z，Orientation_w：

-frame2.png，Orientation_x，Orientation_y，Orientation_z，Orientation_w：

以下是我的代码：

//camer matrix
tf::Matrix3x3 K

//qua 0->1
tf::Quaternion p

//qua 0->2

tf::Quaternion q


//Rotation from frame0 to frame 1
tf::Matrix3x3 r01(p);


//Rotation from frame0 to frame 2
tf::Matrix3x3 r02(q);

////Rotation from frame1 to frame 2
tf::Matrix3x3 r12 = r02*(r01.inverse());

//homography: K*R*K^-1
tf::Matrix3x3 homography = K*r12*(K.inverse());

我已经尝试了很多次，但是无法弄清楚... 有人知道问题出在哪里吗？ 在此先感谢！