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

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

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

根据该单应性，我应该能够计算出正确的相机姿势，即[R | t]，其中R是3x3旋转矩阵，t是3x1平移向量，因此虚拟立方体位于视频四元体上

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

以下是我尝试过的方法（大多数都基于相同的原则，只是翻译的计算略有不同）。设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
• 黑立方体：哈弗·洛克的技术
• 青色（和紫色）立方体：其他一些技术给出了完全相同的结果

正如你所看到的，黑色立方体或多或少是正常的，但是看起来并不均匀，尽管向量看起来是正交的

---

EDIT2:我在计算v3后对其进行了规范化（为了加强正交性），它似乎也解决了一些问题

如果你有单应性，你可以用如下方法计算相机姿势：

void cameraPoseFromHomography（常数Mat&H，Mat&pose）
{
pose=Mat：：眼睛（3,4，CV_32FC1）；//3x4矩阵，相机姿势
float norm1=（float）norm（H.col（0））；
float norm2=（float）norm（H.col（1））；
float tnorm=（norm1+norm2）/2.0f；//规范化值
Mat p1=H.col（0）；//指向H的第一列的指针
Mat p2=pose.col（0）；//指向pose的第一列的指针（空）
cv:：normalize（p1，p2）；//规范化旋转，并将柱复制到姿势
p1=H.col（1）；//指向H的第二列的指针
p2=pose.col（1）；//指向pose第二列的指针（空）
cv:：normalize（p1，p2）；//规范化旋转并将柱复制到姿势
p1=姿势col（0）；
p2=姿势列（1）；
Mat p3=p1.cross（p2）；//计算p1和p2的叉积
Mat c2=pose.col（2）；//指向pose第三列的指针
p3.copyTo（c2）；//第三列是第1列和第2列的叉积
pose.col（3）=H.col（2）/tnorm；//向量t[R | t]是pose的最后一列
}

---

这个方法对我有效。祝你好运。

以防任何人需要@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])

---

在我的任务中工作得很好。

这是一个python版本，基于Dmitry Voloshyn提交的版本，它规范化了旋转矩阵并将结果转换为3x4

def cameraPoseFromHomography(H):  
    norm1 = np.linalg.norm(H[:, 0])
    norm2 = np.linalg.norm(H[:, 1])
    tnorm = (norm1 + norm2) / 2.0;

    H1 = H[:, 0] / norm1
    H2 = H[:, 1] / norm2
    H3 = np.cross(H1, H2)
    T = H[:, 2] / tnorm

    return np.array([H1, H2, H3, T]).transpose()

---

从单应矩阵计算[R|T]比Jav|u Rock的答案稍微复杂一些

在OpenCV 3.0中，有一个名为cv:：decomposeHomographyMat的方法返回四个可能的解决方案，其中一个是正确的。然而，OpenCV没有提供一种方法来选择正确的方法

---

我现在正在做这件事，也许这个月晚些时候会在这里发布我的代码

Jav_Rock提出的答案并不能为三维空间中的相机姿势提供有效的解决方案

对于由单应引起的树维变换和旋转的估计，存在多种方法。提供用于分解单应的闭合公式，但它们非常复杂。而且，解决方案从来都不是唯一的

---

幸运的是，OpenCV 3已经实现了这个分解（）。给定单应性和正确缩放的内部矩阵，该函数提供一组四种可能的旋转和平移

图像上包含正方形的平面在相机上具有消失车道代理。 这条线的方程式是Ax+By+C=0

你们飞机的法线是（A，B，C）

设p00、p01、p10、p11为应用相机固有参数后的点坐标，且为同质形式，例如p00=（x00，y00,1）

消失线可计算为：

• 向下=p00交叉p01
• 左=p00交叉p10
• 右=p01交叉p11
• 向上=p10与p11交叉
• v1=左交叉右交叉
• v2=上交叉下交叉
• 消失线=v1交叉v2

---

标准向量叉积中的叉可以使用此函数。对我有用

def find_pose_from_homography(H, K):
    '''
    function for pose prediction of the camera from the homography matrix, given the intrinsics 
    
    :param H(np.array): size(3x3) homography matrix
    :param K(np.array): size(3x3) intrinsics of camera
    :Return t: size (3 x 1) vector of the translation of the transformation
    :Return R: size (3 x 3) matrix of the rotation of the transformation (orthogonal matrix)
    '''
    
    
    #to disambiguate two rotation marices corresponding to the translation matrices (t and -t), 
    #multiply H by the sign of the z-comp on the t-matrix to enforce the contraint that z-compoment of point
    #in-front must be positive and thus obtain a unique rotational matrix
    H=H*np.sign(H[2,2])

    h1,h2,h3 = H[:,0].reshape(-1,1), H[:,1].reshape(-1,1) , H[:,2].reshape(-1,1)
    
    R_ = np.hstack((h1,h2,np.cross(h1,h2,axis=0))).reshape(3,3)
    
    U, S, V = np.linalg.svd(R_)
    
    R = U@np.array([[1,0,0],
                   [0,1,0],
                    [0,0,np.linalg.det(U@V.T)]])@V.T
    
    t = (h3/np.linalg.norm(h1)).reshape(-1,1)
    
    return R,t

---

因此，opencv的solvepnp在执行时提供正确的结果