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

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

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

从这个单应性我应该能够计算一个正确的相机姿态，即[R | t]其中R是一个3×3的旋转matrix，t是一个3×1的平移vector，以便虚拟立方体位于video四边形上。

我已经阅读了很多解决scheme（其中一些是关于SO的），并试图实现它们，但是它们似乎只能在一些“简单”的情况下工作（比如当video四边形是正方形时），但在大多数情况下不起作用。

这里是我尝试的方法（大多数是基于相同的原则，只有翻译的计算略有不同）。 设K是相机的固有matrix，H是单应matrix。 我们计算： 

A = K-1 * H

令a1，a2，a3为A的列向量，r1，r2，r3为旋转matrixR的列向量。 

 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，非常感谢您的回答，我尝试了您的方法（以及其他许多方面），这似乎是或多或less的好。 不过，我在计算基于4个共面点的姿势时仍然碰到一些问题。 为了检查结果，我将它与solvePnP的结果进行了比较（由于迭代重投影误差最小化方法，结果会好得多）。

这里是一个例子：

立方体

  • 黄色立方体：解决PNP
  • 黑魔方：Jav_Rock的技术
  • 青色（和紫色）立方体（s）：一些其他技术给出完全相同的结果

正如你所看到的，黑色的立方体或多或less好，但似乎不是很好的比例，虽然载体似乎正交。

编辑2：我计算后（为了执行正交性）v3归一化，它似乎也解决了一些问题。

如果你有你的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 }

这种方法工作forms我。 祝你好运。

从单应性matrix计算[R | T]比Jav_Rock的答案稍微复杂一些。

在OpenCV 3.0中，有一个名为cv :: decomposeHomographyMat的方法，返回四个潜在的解决scheme，其中一个是正确的。 但是，OpenCV没有提供一个方法来挑选出正确的。

我现在正在研究这个，也许会在本月晚些时候在这里发布我的代码。

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

为了估计由单应性引起的树维变换和旋转，存在多种方法。 其中一个提供了分解单应性的封闭公式，但是它们非常复杂。 而且，解决scheme从来不是唯一的。

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

以防万一需要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])

在我的任务中工作正常。

在图像上包含您的广场的平面上有消失的车道代理您的相机。 这条线的方程是A x + B y + C = 0。

（A，B，C）正常的飞机！

假设p00，p01，p10，p11是应用摄像机固有参数后的点坐标，并且是均匀的，例如p00 =（x00，y00,1）

消失线可以计算为：

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

在标准vector中交叉的产品

这里是一个python版本，基于Dmitriy Voloshyn提交的版本，规范化了旋转matrix并将结果转换为3×4。 

 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()
Interesting Posts