[37] | 1 | % By Philip Torr 2002
|
---|
| 2 | % copyright Microsoft Corp.
|
---|
| 3 | %this is a script to test the self calibration stuff
|
---|
| 4 | %torr_test_SFMsc.m.m
|
---|
| 5 | %main()
|
---|
| 6 | %profile on
|
---|
| 7 | clear all;
|
---|
| 8 | m3 = 256;
|
---|
| 9 | method = 'mapsac';
|
---|
| 10 | method = 'linear';
|
---|
| 11 |
|
---|
| 12 |
|
---|
| 13 | % % do we want the same result each time
|
---|
| 14 | % randn('state',0)
|
---|
| 15 | % rand('state',0)
|
---|
| 16 | no_test = 1;
|
---|
| 17 | % [true_F,x1,y1,x2,y2,nx1,ny1,nx2,ny2] = ...
|
---|
| 18 | % torr_gen_2view_matches(foc, no_matches, noise_sigma, translation_mult, translation_adder, ...
|
---|
| 19 | % rotation_multplier, min_Z,Z_RAN,m3);
|
---|
| 20 | [true_F,x1,y1,x2,y2,nx1,ny1,nx2,ny2,true_C,true_R,true_TX, true_E, true_X, true_t] = torr_gen_2view_matches;
|
---|
| 21 |
|
---|
| 22 |
|
---|
| 23 | [true_P1, true_P2] = torr_RCT2P(true_C,true_R, true_t);
|
---|
| 24 | torr_display_structure(true_X, true_P1, true_P2,1);
|
---|
| 25 |
|
---|
| 26 | no_matches = length(nx1);
|
---|
| 27 |
|
---|
| 28 | %if we set this to one then the result should be the same as the groundtruth...
|
---|
| 29 | no_noise = 0;
|
---|
| 30 | if (no_noise)
|
---|
| 31 | nx1 = x1;
|
---|
| 32 | nx2 = x2;
|
---|
| 33 | ny1 = y1;
|
---|
| 34 | ny2 = y2;
|
---|
| 35 | end
|
---|
| 36 |
|
---|
| 37 |
|
---|
| 38 |
|
---|
| 39 | matches = [nx1,ny1,nx2,ny2];
|
---|
| 40 | perfect_matches = [x1,y1,x2,y2];
|
---|
| 41 | set_rank2 = 1;
|
---|
| 42 |
|
---|
| 43 |
|
---|
| 44 | %first estimate F
|
---|
| 45 | [f, e1, n_inliers,inlier_index,nF] = torr_estimateF( matches, m3, [], method, set_rank2);
|
---|
| 46 |
|
---|
| 47 | %now guess the camera calibration matrix
|
---|
| 48 | CC = diag(ones(3,1),0);
|
---|
| 49 | CC(3,3) = 1;
|
---|
| 50 |
|
---|
| 51 | %next self calibrate for focal length
|
---|
| 52 | [focal_length, nE,CCC] = torr_self_calib_f(nF,CC);
|
---|
| 53 |
|
---|
| 54 | %next correct the points so that they lie on the fundamental matrix
|
---|
| 55 | [corrected_matches error2] = torr_correctx4F(f, nx1,ny1,nx2,ny2, no_matches, m3);
|
---|
| 56 |
|
---|
| 57 | %corrected matches should have zero error:
|
---|
| 58 | e2 = torr_errf2(f, corrected_matches(:,1), corrected_matches(:,2), corrected_matches(:,3),corrected_matches(:,4), length(nx1), m3);
|
---|
| 59 |
|
---|
| 60 | %now we have an Essential matrix we can establish the camera frame...
|
---|
| 61 | [P1,P2,R,t,srot_axis,rot_angle,g] = torr_linear_EtoPX(nE,corrected_matches,CCC,m3);
|
---|
| 62 |
|
---|
| 63 | %non linear estimation of g (and hence f)
|
---|
| 64 | [g,f] = torr_nonlinG(g ,nx1,ny1,nx2,ny2, no_matches, m3, CCC);
|
---|
| 65 |
|
---|
| 66 | %next convert the 6 parameters of g to a fundamental matrix
|
---|
| 67 | f2 = torr_g2F(g,CCC);
|
---|
| 68 |
|
---|
| 69 | %next correct the points so that they lie on the fundamental matrix
|
---|
| 70 | [corrected_matches error2] = torr_correctx4F(f2, nx1,ny1,nx2,ny2, no_matches, m3);
|
---|
| 71 |
|
---|
| 72 | %next we need to obtain P1 & P2
|
---|
| 73 | [P1, P2] = torr_g2FP(g,CCC);
|
---|
| 74 |
|
---|
| 75 | %now use P matrices and corrected matches to get structure:
|
---|
| 76 | X = torr_triangulate(corrected_matches, m3, P1, P2);
|
---|
| 77 |
|
---|
| 78 | torr_display_structure(X, P1, P2, 1);
|
---|
| 79 |
|
---|
| 80 | %inlier_index = torr_robust_chieral(X,P1,P2);
|
---|
| 81 | %this can be used to remove outliers, see torr_tool
|
---|
| 82 |
|
---|
| 83 |
|
---|
| 84 | %test
|
---|
| 85 | XX = [X(1,:) ./ X(4,:) ; X(2,:) ./ X(4,:) ; X(3,:) ./ X(4,:) ];
|
---|
| 86 |
|
---|
| 87 | disp('ratio of estimated and true X');
|
---|
| 88 | XX ./true_X
|
---|
| 89 |
|
---|
| 90 |
|
---|
| 91 |
|
---|
| 92 | show_result = 0;
|
---|
| 93 | if show_result
|
---|
| 94 | disp('look at reprojection error to groundtruth points')
|
---|
| 95 | x1_rp = P1 * X;
|
---|
| 96 | x1_rp(1,:) = x1_rp(1,:) ./ x1_rp(3,:) * m3;
|
---|
| 97 | x1_rp(2,:) = x1_rp(2,:) ./ x1_rp(3,:) * m3;
|
---|
| 98 | (x1 - x1_rp(1,:)')'
|
---|
| 99 | (y1 - x1_rp(2,:)')'
|
---|
| 100 |
|
---|
| 101 | x2_rp = P2 * X;
|
---|
| 102 | x2_rp(1,:) = x2_rp(1,:) ./ x2_rp(3,:) * m3;
|
---|
| 103 | x2_rp(2,:) = x2_rp(2,:) ./ x2_rp(3,:) * m3;
|
---|
| 104 | (x2 - x2_rp(1,:)')'
|
---|
| 105 | (y2 - x2_rp(2,:)')'
|
---|
| 106 | end |
---|