2

I am trying to estimate the pose of a camera by scanning two images taken from it, detecting features in the images, matching them, creating the fundamental matrix, using the camera intrinsics to calculate the essential matrix and then decompose it to find the Rotation and Translation.

Here is the matlab code: 

I1 = rgb2gray(imread('1.png'));
I2 = rgb2gray(imread('2.png'));

points1 = detectSURFFeatures(I1);
points2 = detectSURFFeatures(I2);

points1 = points1.selectStrongest(40);
points2 = points2.selectStrongest(40);

[features1, valid_points1] = extractFeatures(I1, points1);
[features2, valid_points2] = extractFeatures(I2, points2);

indexPairs = matchFeatures(features1, features2);

matchedPoints1 = valid_points1(indexPairs(:, 1), :);
matchedPoints2 = valid_points2(indexPairs(:, 2), :);

F = estimateFundamentalMatrix(matchedPoints1,matchedPoints2);

K = [2755.30930612600,0,0;0,2757.82356074384,0;1652.43432833339,1234.09417974414,1];

%figure; showMatchedFeatures(I1, I2, matchedPoints1, matchedPoints2);

E = transpose(K)*F*K;
W = [0,-1,0;1,0,0;0,0,1];
Z = [0,1,0;-1,0,0;0,0,0];
[U,S,V] = svd(E);

R = U*inv(W)*transpose(V);

T = U(:,3);

thetaX = radtodeg(atan2(R(3,2),R(3,3)));
thetaY = radtodeg(atan2(-R(3,1),sqrt(R(3,2)^2 +R(3,3)^2)));
thetaZ = radtodeg(atan2(R(2,1),R(1,1)));

The problem I am facing is that R and T are always incorrect. ThetaZ is most of the times equal to ~90, If I repeat the calculation a lot of times I sometimes get the expected angles. (Only in some cases though)

I dont seem to understand why. It might be because the Fundamental Matrix I calculated is wrong. Or is there a different spot where I am going wrong?

Also what scale/units is T in? (Translation Vector) Or is it inferred differently.

P.S. New to computer vision...

Dima
  • 38,860
  • 14
  • 75
  • 115
Rohit H.S.
  • 65
  • 1
  • 6

3 Answers3

2

Please note that from decomposing E, 4 solutions are possible (2 possible rotations X 2 possible translations). Specifically regarding R, it can also be: R = UWtranspose(V); Similarly, T can also be: T = -U(:,3);

To check if this is your bug, please post here all the 4 possible solutions for a given case where you get ThetaZ~90.

Another thing I would check (since you have K), is estimating the essential matrix directly (without going through fundamental matrix): http://www.mathworks.com/matlabcentral/fileexchange/47032-camera-geometry-algorithms/content//CV/CameraGeometry/EssentialMatrixFrom2DPoints.m

ezfn
  • 173
  • 5
  • Got it here is the output when ThetaZ ~ 90 degrees: `thetaX = 0.0095 thetaY = 0.0382 thetaZ = -87.9229 x = 0.0004 0.0003 -1.0000 thetaX = 0.0095 thetaY = 0.0382 thetaZ = -87.9229 x = -0.0004 -0.0003 1.0000 thetaX = -0.0384 thetaY = 0.0096 thetaZ = 92.0771 x = 0.0004 0.0003 -1.0000 thetaX = -0.0384 thetaY = 0.0096 thetaZ = 92.0771 x = -0.0004 -0.0003 1.0000` – Rohit H.S. Dec 01 '14 at 18:37
  • Ok and here are the values when ThetaZ is accurate: `x = 0.0039 -0.0061 1.0000 thetaX = -0.2571 thetaY = -0.4948 thetaZ = -16.0276 x = 0.0032 -0.0035 -1.0000 thetaX = -0.2571 thetaY = -0.4948 thetaZ = -16.0276 x = -0.0032 0.0035 1.0000 ` – Rohit H.S. Dec 01 '14 at 18:48
  • OK, now we can be pretty sure that something is wrong with your essential matrix. Let's start by checking F: can you verify that for the corresponding points you get transpose(x1)*F*x2 = 0 approximately. Even better: check it on corresponding points that weren't used to calculate F. This would be a good validation. – ezfn Dec 01 '14 at 22:11
0

Try transposing K. The K that you get from estimateCameraParameters assumes row-vectors post-multiplied by a matrix, while the K in most textbooks assumes column-vectors pre-multipied by a matrix.

Edit: In the R2015b release of the Computer Vision System Toolbox there is a cameraPose function, which computes relative orientation and location from the fundamental matrix.

Dima
  • 38,860
  • 14
  • 75
  • 115
  • I tried doing that, I caliberated my camera again and the camera intrinsic matrix changed drastically: [661.758831380405,0,614.992428800078;0,655.032599632343,367.806203964231;0,0,1] (Matlab returns ^ its inverse as the camera calib matrix) I am using the camera on board a iphone 5S, do you think the auto focus might cause an error in the camera calib. I am thinking the error is either in the intrinsic matrix or the fundamental matrix. – Rohit H.S. Dec 01 '14 at 19:51
  • Wait, I am confused. K is the intrinsic matrix, right? I am saying that you should transpose K, before you calculate E. And yes, auto-focus is a bad thing, but it may be ok, depending on the kind of accuracy that you need. – Dima Dec 01 '14 at 19:56
  • Yes, I transposed K, but the angles are still all over the place. The three things that can be wrong are Fundamental Matrix, Essential and Intrinsic. Seeing that I derive the Essential from the other two, and considering the intrinsic matrix is correct since I am using the in built matlab toolbox, this leaves the fundamental matrix which could be incorrect. I also tried this check, E = R[t]x , which does not hold true (not even close). – Rohit H.S. Dec 01 '14 at 20:14
  • Keep in mind, that since you only have the intrinsics, but not the extrinsics, your translation is only up to scale, meaning that the `t` you get is in some unknown units. Try putting the checkerboard into the scene and follow this example: http://www.mathworks.com/help/vision/examples/sparse-3-d-reconstruction-from-two-views.html This would be a good sanity check. – Dima Dec 01 '14 at 20:21
  • @RohitH.S., there is a new function in matlab, which will do what you need. See the edit. – Dima Sep 04 '15 at 20:59
  • Did you actually find the error? I have (more or less) the same problem and can't find a solution. If I use the inverse of K I get the rotations right, if i use the "non-invers" of K I get the translations right. I did include all 4 possible solutions, and this is not the problem. Further I get the exact same behavior if I use `cameraPose` instead of my own "code" – Zwähnia Aug 09 '16 at 13:49
0

U and V need to be enforcedto be SO(3). http://mathworld.wolfram.com/SpecialOrthogonalMatrix.html In other words, if U and/or V has a negative determinat, the last column in U and/or V need to be negated. (det(U) < 0) => U(:,3) = -U(:,3)

Best Regards.

Jens Edhammer
  • 1