基于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
• Black Cube:Jav_Rock的技巧
• 青色(和紫色)立方体:一些其他技术给出完全相同的结果

正如你所看到的,黑色立方体或多或少都可以,但看起来并不均匀,尽管矢量看似正交.

EDIT2:我在计算它之后对v3进行了规范化(为了强制执行正交性),它似乎也解决了一些问题.

Answer 1

如果你有Homography,你可以用这样的方法计算相机姿势:

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
}

这种方法适用于我.祝好运.

Answer 2

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

为了估计由单应性引起的树维变换和旋转,存在多种方法.其中一个提供了用于分解单应性的封闭公式,但它们非常复杂.此外,解决方案永远不是唯一的.

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

Answer 3

以防任何人需要python移植@Jav_Rock编写的函数:

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])

在我的任务中工作正常.

Answer 4

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

在OpenCV 3.0中,有一个名为cv :: decomposeHomographyMat的方法,它返回四个可能的解决方案,其中一个是正确的.但是,OpenCV没有提供一种方法来挑选出正确的方法.

我现在正在研究这个问题,也许会在本月晚些时候在这里发布我的代码.