基于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方法得到的结果进行比较（使用以下三维点[0,0,0] [0,1,0] [1,0,0] [1,1,0]）。

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

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

您对我做错的事情有任何见解吗？

尝试@Jav_Rock的方法后编辑

嗨Jav_Rock，非常感谢您的答案，我尝试了您的方法（以及其他许多方法），似乎更或多或少地可以接受。然而，当基于4个共面点计算姿态时，我仍然遇到一些问题。为了检查结果，我将其与solvePnP的结果进行比较（由于迭代重投影误差最小化方法，后者将更好）。

以下是一个例子：

• 黄色立方体：解决PNP
• 黑色立方体：Jav_Rock的技术
• 青色（和紫色）立方体：一些其他技术可以得到完全相同的结果

正如您所看到的，黑色立方体或多或少还好，但似乎比例不正确，尽管向量似乎是正交的。

编辑2：在计算v3后，我对其进行了归一化（以强制正交性），它似乎也解决了一些问题。

- JimN

---

OpenCV的solvePnP提供了正确的结果，而你的实现是错误的吗？ - navneeth

是的，solvePnP可以给出正确的结果，而我的实现仅使用单应性无法给出正确的旋转/平移向量。 - JimN

如果您分享您的代码，我们可以一起查看并了解如何修复它。您可能忘记了一个重要的事情，那就是强制旋转矩阵正交性。 - fireant

我相信你已经掌握了所有必要的步骤： 1.获取相机内参 2.定义4个点的对应关系并使用DLT算法计算H 3.使用K.inv()左乘H 4.按@Jav_Rock所述分解结果 - marcos.nieto

我尝试了两种方法，但每次都得到错误的结果。使用solvePnP至少我的投影的某些部分是有意义的。您能否请看一下我的问题并提供答案？https://dev59.com/nIjca4cB1Zd3GeqP0reC#29078048 - Jakob Alexander Eichler

嘿，有人能帮我解决我的最新问题吗？它与这个问题类似，但我不确定我该如何使用下面提供的解决方案。如何调用cameraPoseFromHomography？H参数和姿态参数是什么？如何像问题中的图像一样绘制一个立方体？请帮帮我，因为我不知道该怎么做！问候- Jonas（您可以在此处找到问题：https://stackoverflow.com/questions/51009968/how-to-draw-cube-c） - user3596335

Answer 1

如果您拥有单应矩阵，您可以使用以下类似的方法计算相机的位姿：

void cameraPoseFromHomography(const Mat& H, Mat& pose)
{
    pose = Mat::eye(3, 4, CV_32FC1);      // 3x4 matrix, the camera pose
    float norm1 = (float)norm(H.col(0));  
    float norm2 = (float)norm(H.col(1));  
    float tnorm = (norm1 + norm2) / 2.0f; // Normalization value

    Mat p1 = H.col(0);       // Pointer to first column of H
    Mat p2 = pose.col(0);    // Pointer to first column of pose (empty)

    cv::normalize(p1, p2);   // Normalize the rotation, and copies the column to pose

    p1 = H.col(1);           // Pointer to second column of H
    p2 = pose.col(1);        // Pointer to second column of pose (empty)

    cv::normalize(p1, p2);   // Normalize the rotation and copies the column to pose

    p1 = pose.col(0);
    p2 = pose.col(1);

    Mat p3 = p1.cross(p2);   // Computes the cross-product of p1 and p2
    Mat c2 = pose.col(2);    // Pointer to third column of pose
    p3.copyTo(c2);       // Third column is the crossproduct of columns one and two

    pose.col(3) = H.col(2) / tnorm;  //vector t [R|t] is the last column of pose
}

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

- Jav_Rock

---

你好 Jav_Rock，非常感谢你的回答。我尝试了你的方法并编辑了帖子，这样你就可以看到获得的结果了。再次感谢。 - JimN

我认为这张图片不可见。无论如何，如果你想深入了解理论，可以阅读来自dsp.stackexchange的这个问题： http://dsp.stackexchange.com/q/2736/1473 - Jav_Rock

我可能没有理解正确（代码与你的完全相同），或者自从您发布这个答案以来，OpenCV在处理Mat对象方面发生了变化。使用像p1、p2等赋值语句不会改变pose参数，并导致结果姿态与其初始化时完全相同——一个3x4的单位矩阵。使用copyTo()方法可以解决这个问题。似乎需要进行深层复制。请参考@Jacob在https://dev59.com/eljUa4cB1Zd3GeqPRW0q的回答。 - rbaleksandar

我尝试将代码翻译成Java，但我的返回结果很糟糕。 - Jakob Alexander Eichler

@Jav_Rock，你的方法在不使用相机内部函数的情况下工作得如何？ - alexburtnik

Answer 2

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

对于通过单应性引起的三维变换和旋转的估计，存在多种方法。 其中之一 提供了分解单应性的闭合公式，但它们非常复杂。而且，解决方案从未是唯一的。

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

- Emiswelt

---

从最后两个可能的解中选择正确解的计算非常复杂。您是否知道任何可以从最终两个解中返回一个解的论文实现？ - Sanjeev Kumar

@YonatanSimson 一个单应性矩阵描述了由四个共面点给出的透视变换。您下面的答案利用了一个单应性矩阵。问题出在哪里？ - Emiswelt

Answer 3

如果有人需要@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])

在我的任务中表现良好。

- Dmytriy Voloshyn

---

这个没有内在的相机参数，它是如何工作的？ - Mehdi

@Mehdi 我认为假设单应性在规范化坐标上工作：p'=K^(-1)[p;1]。 - Felix Goldberg

对我来说没有起作用。它没有给出正确的结果。我用自己的图像进行了检查，其中一张几乎与平面物体平行，另一张则是以透视角度拍摄的。对于这两种情况，它都返回了近似的R作为单位矩阵。 - undefined

Answer 4

从单应矩阵中计算[R|T]比Jav_Rock的回答稍微复杂一些。

在OpenCV 3.0中，有一个名为cv::decomposeHomographyMat的方法，它返回四个潜在解决方案之一是正确的。然而，OpenCV没有提供一种方法来挑选出正确的解决方案。

我正在研究这个问题，可能会在本月晚些时候在此发布我的代码。

- Yang Kui

---

你已经想出如何选择正确的解决方案了吗？ - Sanjeev Kumar

Answer 5

包含图像中你方块的平面相对于相机有消失线代理。

该行的公式为Ax+By+C=0。

你的平面的法向量是（A，B，C）！

假设p00、p01、p10和p11是应用相机内参后的点坐标，以齐次形式给出，例如p00 =（x00，y00，1）

可以计算消失线如下：

• down = p00叉乘p01;
• left = p00叉乘p10;
• right = p01叉乘p11;
• up = p10叉乘p11;
• v1 = left叉乘right;
• v2 = up叉乘down;
• vanish_line = v1叉乘v2;

其中cross表示标准向量叉积

- DejanM

Answer 6

你可以使用这个函数。对我来说很有效。

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 = [e-mail]([[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

- saksham jindal

Answer 7

这是一个基于 Dmitriy Voloshyn 提交的版本的 Python 版本，它规范化旋转矩阵并转置结果为 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()

- Clay