Computing camera pose with homography matrix based on 4 coplanar points

Question

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.

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.

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.

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:

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.

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

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.

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.

Any insights on what I am doing wrong?

Edit after trying @Jav_Rock's method

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

Here is an example:

• Yellow cube: Solve PNP
• Black Cube: Jav_Rock's technique
• Cyan (and Purple) cube(s): some other techniques given the exact same results

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.

Answer 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.

Answer 2

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

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.

Answer 3

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

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.

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