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

I have 4 coplanar points in a video (or image) representing a quad (not necessarily a square or rectangle) and I would like to be able to display a virtual cube on top of them where the corners of the cube stand exactly on the corners of the video quad.

我在一个视频(或图像)中有4个共面点，它们代表一个四轴飞行器(不一定是正方形或矩形)，我希望能够在它们上面显示一个虚拟立方体，立方体的角正好位于视频四轴飞行器的角上。

Since the points are coplanar I can compute the homography between the corners of a unit square (i.e. [0,0] [0,1] [1,0] [1,1]) and the video coordinates of the quad.

因为这些点是共面的，所以我可以计算单位正方形的角(即[0,0][0,1][1,0][1,0][1,1,1])和四边形的视频坐标之间的同形性。

From this homography I should be able to compute a correct camera pose, i.e. [R|t] where R is a 3x3 rotation matrix and t is a 3x1 translation vector so that the virtual cube lies on the video quad.

从这个homography中，我应该能够计算出一个正确的摄像机姿态，也就是[R|t]，其中R是一个3x3旋转矩阵，t是一个3x1平移矢量，这样虚拟立方体就位于视频四轴上。

I have read many solutions (some of them on SO) and tried implementing them but they seem to work only in some "simple" cases (like when the video quad is a square) but do not work in most cases.

我读过许多解决方案(其中一些是这样)，并尝试实现它们，但它们似乎只适用于某些“简单”的情况(比如视频四轴是正方形)，但在大多数情况下并不适用。

Here are the methods I tried (most of them are based on the same principles, only the computation of the translation are slightly different). Let K be the intrinsics matrix from the camera and H be the homography. We compute:

这里是我尝试过的方法(它们大多数基于相同的原则，只是翻译的计算略有不同)。让K是来自摄像机的intrinsic矩阵，H是homography。我们计算:

A = K-1 * H

Let a1,a2,a3 be the column vectors of A and r1,r2,r3 the column vectors of the rotation matrix R.

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

r1 = a1 / ||a1||
r2 = a2 / ||a2||
r3 = r1 x r2
t = a3 / sqrt(||a1||*||a2||)

The issue is that this does not work in most cases. In order to check my results, I compared R and t with those obtained by OpenCV's solvePnP method (using the following 3D points [0,0,0] [0,1,0] [1,0,0] [1,1,0]).

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

Since I display the cube in the same way, I noticed that in every case solvePnP provides correct results, while the pose obtained from the homography is mostly wrong.

由于我以同样的方式显示立方体，我注意到在每一种情况下solvePnP都提供了正确的结果，而从同源性得到的姿态大多是错误的。

In theory since my points are coplanar, it is possible to compute the pose from a homography but I couldn't find the correct way to compute the pose from H.

在理论上，由于我的点是共面的，所以可以用单形来计算这个位姿但是我找不到从H来计算这个位姿的正确方法。

Any insights on what I am doing wrong?

我做错了什么?

Edit after trying @Jav_Rock's method

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

Hi Jav_Rock, thanks very much for your answer, I tried your approach (and many others as well) which seems to be more or less OK. Nevertheless I still happen to have some issues when computing the pose based on 4 coplanar point. In order to check the results I compare with results from solvePnP (which will be much better due to the iterative reprojection error minimization approach).

嗨，Jav_Rock，非常感谢您的回答，我尝试了您的方法(以及其他许多方法)，这似乎或多或少还可以。尽管如此，我在计算基于4个共面点的姿态时还是遇到了一些问题。为了检验结果，我比较了solvePnP的结果(由于迭代再投影误差最小化方法，这将会更好)。

Here is an example:

这是一个例子:

• Yellow cube: Solve PNP
• 黄色方块:解决PNP型
• Black Cube: Jav_Rock's technique
• 黑色立方体:Jav_Rock的技术
• Cyan (and Purple) cube(s): some other techniques given the exact same results
• Cyan(和紫色)立方体:一些其他的技术给出了完全相同的结果。

As you can see, the black cube is more or less OK but doesn't seem well proportioned, although the vectors seem orthonormal.

正如你所看到的，黑色方块或多或少是可以的，但看起来不那么比例，尽管这些向量看起来是标准正交的。

EDIT2: I normalized v3 after computing it (in order to enforce orthonormality) and it seems to solve some problems as well.

EDIT2:在计算了v3之后(为了执行标准正交性)，它似乎也解决了一些问题。

6 个解决方案

#1

If you have your Homography, you can calculate the camera pose with something like this:

如果你有你的同源性，你可以用这样的方法来计算相机的姿势:

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
}

This method works form me. Good luck.

这种方法对我起作用。祝你好运。

#2

The answer proposed by Jav_Rock does not provide a valid solution for camera poses in three-dimensional space.

Jav_Rock提出的答案并没有为三维空间中的摄像机姿态提供有效的解决方案。

For estimating a tree-dimensional transform and rotation induced by a homography, there exist multiple approaches. One of them provides closed formulas for decomposing the homography, but they are very complex. Also, the solutions are never unique.

对于估计由同构引起的树维变换和旋转，有多种方法。其中一个给出了分解同构关系的封闭公式，但是它们非常复杂。而且，解决方案从来都不是唯一的。

Luckily, OpenCV 3 already implements this decomposition (decomposeHomographyMat). Given an homography and a correctly scaled intrinsics matrix, the function provides a set of four possible rotations and translations.

幸运的是，OpenCV 3已经实现了这一分解(分解同源语)。给定一个homography和一个正确伸缩的intrinsic矩阵，该函数提供一组可能的旋转和转换。

#3

Computing [R|T] from the homography matrix is a little more complicated than Jav_Rock's answer.

从同构矩阵中计算[R|T]比Jav_Rock的答案要复杂一些。

In OpenCV 3.0, there is a method called cv::decomposeHomographyMat that returns four potential solutions, one of them is correct. However, OpenCV didn't provide a method to pick out the correct one.

在OpenCV 3.0中，有一种方法叫做cv:分解匀像符，它返回四个可能的解，其中一个是正确的。然而，OpenCV并没有提供一种方法来选择正确的方法。

I'm now working on this and maybe will post my codes here later this month.

我现在正在做这件事，可能这个月晚些时候会把我的代码贴在这里。

#4

Just in case anybody needs python porting of the function written by @Jav_Rock:

以防有人需要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])

Works fine in my tasks.

我的工作做得很好。

#5

Plane that contain your Square on image has vanishing lane agents your camera. Equation of this line is Ax+By+C=0.

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

Normal of your plane is (A,B,C)!

你的飞机正常(A,B,C)!

Let p00,p01,p10,p11 are coordinates of point after applying camera's intrinsic parameters and in homogenous form e.g, p00=(x00,y00,1)

设p00,p01,p10,p11为点坐标，应用相机的固有参数，均质形式e。g,p00 =(x00 y00 1)

Vanishing line can be calculated as:

消失线可计算为:

• down = p00 cross p01;
• 向下= p00 cross p01;
• left = p00 cross p10;
• 左= p00 cross p10;
• right = p01 cross p11;
• 右= p01交叉p11;
• up = p10 cross p11;
• 向上= p10 cross p11;
• v1=left cross right;
• v1 =左叉右;
• v2=up cross down;
• v2 =上十字架下;
• vanish_line = v1 cross v2;
• vanish_line = v1交叉v2;

Where cross in standard vector cross product

标准向量叉乘在哪里

#6

Here's a python version, based on the one submitted by Dmitriy Voloshyn that normalizes the rotation matrix and transposes the result to be 3x4.

这是一个python版本，它是由Dmitriy 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()