基于4个共面点的单应矩阵计算相机姿态

Question

我在视频（或图像）中有4个共面点，代表一个四边形（不一定是正方形或矩形），我希望能够在它们的上方显示一个虚拟立方体，立方体的角恰好位于这些角上视频四分之一。

由于这些点是共面的，因此我可以计算单位正方形的角（即[0,0] [0,1] [1,0] [1,1]）与四边形的视频坐标之间的单应性。

通过这种单应性，我应该能够计算出正确的摄像机姿势，即[R | t]，其中R是3x3旋转矩阵，t是3x1平移矢量，以便虚拟立方体位于视频四边形上。

我已经阅读了许多解决方案（其中一些在SO上）并尝试实现它们，但是它们似乎仅在某些“简单”情况下才有效（例如，当视频四边形为正方形时），但在大多数情况下不起作用。

这是我尝试过的方法（大多数方法基于相同的原理，仅翻译的计算略有不同）。令K为相机的本征矩阵，H为单应性。我们计算：

A = K-1 * H

假设a1，a2，a3是A的列向量，r1，r2，r3是旋转矩阵R的列向量。

r1 = a1 / ||a1||

r2 = a2 / ||a2||

r3 = r1 x r2

t = a3 / sqrt(||a1||*||a2||)

问题是这在大多数情况下不起作用。为了检查我的结果，我将R和t与通过OpenCV的solvePnP方法获得的结果进行了比较（使用以下3D点[0,0,0] [0,1,0] [1,0,0] [1,1 ，0]）。

由于我以相同的方式显示多维数据集，因此我注意到在每种情况下，solvePnP均提供正确的结果，而从单应性图中获得的姿势大多是错误的。

从理论上讲，由于我的点是共面的，因此可以从单应性计算姿势，但是我找不到从H计算姿势的正确方法。

关于我在做什么错的任何见解？

尝试@Jav_Rock的方法后进行编辑

您好Jav_Rock，非常感谢您的回答，我尝试了您的方法（以及许多其他方法），这似乎或多或少都可以。但是，在基于4个共面点计算姿势时，我仍然碰巧遇到一些问题。为了检查结果，我将其与solvePnP的结果进行比较（由于采用了迭代重投影误差最小化方法，因此效果会更好）。

这是一个例子：

• 黄色立方体：解决PNP

• 黑色立方体：Jav_Rock的技术

• 青色（和紫色）多维数据集：其他一些技术得出的结果完全相同

如您所见，尽管矢量看起来是正交的，但黑色立方体或多或少是可以的，但似乎比例不高。

EDIT2：我对v3进行了归一化（为了增强正交性），它似乎也解决了一些问题。

Answer 1

万一有人需要@Jav_Rock编写的函数的python移植：def cameraPoseFromHomography(H):    H1 = H[:, 0]    H2 = H[:, 1]    H3 = np.cross(H1, H2)    norm1 = np.linalg.norm(H1)    norm2 = np.linalg.norm(H2)    tnorm = (norm1 + norm2) / 2.0;    T = H[:, 2] / tnorm    return np.mat([H1, H2, H3, T])在我的任务中工作正常。