Expansion of the main matrix: verification of four possible solutions for R and T

Question

I want to make some structure with movement using OpenCV. So far I have a fundamental matrix and a basic matrix. Having an essential matrix, I am doing SVD to get R and T.

My problem is that I have 2 possible solutions for R and 2 possible solutions for T, which leads to 4 solutions for a common pose, where only one of the four solutions is correct. How to find the right solution?

Here is my code:

private void calculateRT(Mat E, Mat R, Mat T){ Mat w = new Mat(); Mat u = new Mat(); Mat vt = new Mat(); Mat diag = new Mat(3,3,CvType.CV_64FC1); double[] diagVal = {1,0,0,0,1,0,0,0,1}; diag.put(0, 0, diagVal); Mat newE = new Mat(3,3,CvType.CV_64FC1); Core.SVDecomp(E, w, u, vt, Core.DECOMP_SVD); Core.gemm(u, diag, 1, vt, 1, newE); Core.SVDecomp(newE, w, u, vt, Core.DECOMP_SVD); publishProgress("U: " + u.dump()); publishProgress("W: " + w.dump()); publishProgress("vt:" + vt.dump()); double[] W_Values = {0,-1,0,1,0,0,0,0,1}; Mat W = new Mat(new Size(3,3), CvType.CV_64FC1); W.put(0, 0, W_Values); double[] Wt_values = {0,1,0-1,0,0,0,0,1}; Mat Wt = new Mat(new Size(3,3), CvType.CV_64FC1); Wt.put(0,0,Wt_values); Mat R1 = new Mat(); Mat R2 = new Mat(); // u * W * vt = R Core.gemm(u, Wt, 1, vt, 1, R2); Core.gemm(u, W, 1, vt, 1, R1); publishProgress("R: " + R.dump()); // +- T (2 possible solutions for T) Mat T1 = new Mat(); Mat T2 = new Mat(); // T = ut u.col(2).copyTo(T1); publishProgress("T : " + T.dump()); Core.multiply(T, new Scalar(-1.0, -1.0, -1.0), T2); // TODO Here I have to find the correct combination for R1 R2 and T1 T2 } 

Answer 1

There is theoretical ambiguity in reconstructing the relative Euclidean poses of two chambers from their fundamental matrix. This ambiguity is due to the fact that, given the two-dimensional point of the image, the classic pinhole camera model cannot determine whether the corresponding 3D point is in front of the camera or behind the camera. To eliminate this ambiguity, you need to know the one-point correspondence in the images: since these two two-dimensional points are considered projections of one three-dimensional point located in front of both cameras (since it is visible on both images) this will allow you to choose the correct R and T.

For this purpose, one of the methods is explained in § 6.1.4 (p47) of the following doctoral dissertation: "Geometry, Constraints, and the Calculation of the Trifocal Tensor", C.Ressl ( PDF ). The following is a diagram of this method. I will denote two corresponding two-dimensional points by x1 and x2, two camera matrices on K1 and K2, and an essential matrix on E12.

I am. Calculate the SVD of the base matrix E12 = U * S * V' . If det(U) < 0 set U = -U . If det(V) < 0 set V = -V .

II. Define W = [0,-1,0; 1,0,0; 0,0,1] W = [0,-1,0; 1,0,0; 0,0,1] W = [0,-1,0; 1,0,0; 0,0,1] , R2 = U * W * V' and T2 = third column of U

III. Define M = [ R2'*T2 ]x , X1 = M * inv(K1) * x1 and X2 = M * R2' * inv(K2) * x2

IV. If X1(3) * X2(3) < 0 , set R2 = U * W' * V' and recalculate M and X1

v. If X1(3) < 0 set T2 = -T2

VI. Define P1_E = K1 * [ I | 0 ] P1_E = K1 * [ I | 0 ] and P2_E = K2 * [ R2 | T2 ] P2_E = K2 * [ R2 | T2 ]

The designation ' denotes the transposition and the designation [.]x used in step iii. corresponds to a skew-symmetric operator. Application of the skew-symmetric operator on a 3x1 vector e = [e_1; e_2; e_3] e = [e_1; e_2; e_3] e = [e_1; e_2; e_3] leads to the following (see Wikipedia article on cross-product ):

 [e]x = [0,-e_3,e_2; e_3,0,-e_1; -e_2,e_1,0] 

Finally, note that the norm T2 will always be 1, since it is one of the columns of the orthogonal matrix. This means that you cannot restore the true distance between the two cameras. To do this, you need to know the true distance between two points in the scene and take this into account when calculating the true distance between cameras.