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.

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

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

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

Solution 4

Just in case anybody needs python porting of the function written by @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.

View more solutions

Comments

[navneeth]

So opencv's solvepnp provides correct results while your implementation is wrong ?

[JimN]

Yes solvePnP gives correct results while my implementation using homographies only does not gives correct rotation/translation vectors.

[fireant]

If you share your code we can go through it and see how it can be fixed. One thing you might have forgotten is to enforce orthonormality of the rotation matrix.

[marcos.nieto]

I believe you have all the steps you need: 1.-Obtain camera intrinsics 2.-Define 4-point correspondences and compute H with DLT 3.-Left-multiply H with K.inv() 4.-Decompose the result as explained by @Jav_Rock

[Jakob Alexander Eichler]

I tryed both methods but I all the time get wrong results. With solvePnP at least some parts of my projection manke sense. Can you please have a look at my question for providing an answer? stackoverflow.com/a/29078048/663551

[Admin]

Hey. Would somebody help me solving my latest question? It is something similar to this question but I'm not really sure how I have to use the solution provided below. How to call cameraPoseFromHomography? What parameter is H and what parameter is pose? How to draw a cube like in the questions image? Please help me because I'm clueless how to go on! Greetings- Jonas (You can find the question here: stackoverflow.com/questions/51009968/how-to-draw-cube-c)

[JimN]

Hi Jav_Rock, thanks very much for your answer, I tried your method and edited the post so that you can see the obtained results. Thanks again.

[Jav_Rock]

I think the image is not visible. Anyway, if you want to go deeper into theory you can read this question from the dsp.stackexchange dsp.stackexchange.com/q/2736/1473

[rbaleksandar]

Either I'm not getting it right (code is 100% the same as yours) or OpenCV has changed in the way it handles the Mat-object since you've posted this answer. Using assignments such as yours (p1,p2...) does NOT change the pose-argument and leads to a resulting pose identical to its initialization - a 3x4 identity matrix. Using copyTo() resolves the issue. It seems that deep copy is necessary. Check @Jacob's reply at stackoverflow.com/questions/6411476/…

[Jakob Alexander Eichler]

I tryed to translate the code to Java but my returned results are bad.

[Jakob Alexander Eichler]

Is it recommended to estimate the camera pose with solvePnP or homography?

[Gaurav Fotedar]

Why is normalization needed before copying 1st two columns?

[Mehdi]

how does this work without intrinsic camera parameters?

[Felix Goldberg]

@Mehdi I suppose the homography is assumed to work on normalized coordinates: p'=K^(-1)[p;1].

[Sanjeev Kumar]

Have you figured out how to pick the correct solution?

[Sanjeev Kumar]

The calculation of picking the correct solution out of last two possible solutions is very complicated. Do you know any implementation of the paper which can return one solution out of final two solutions?

[alexburtnik]

@Jav_Rock, How is your approach working without using intrinsics of the camera?

[Admin]

Hey guys. I see @Jav_Rock answer is marked as correct, so I'm pretty sure it is a working solution to the question but tbh I can not really get how I have to use @Jav_Rock approach to end up with a 3d cube like it is mentioned in the question. What are the parameters for cameraPoseFromHomography? Can I calculate the cube without knowing anything more than the 4 corner points of the rectangle? Any help would be very appreciated. Greetings

[Emiswelt]

@YonatanSimson A homography describes the perspective transform given by four coplanar points. Your own answer below utilizes a homography matrix. What's the issue?