[37] | 1 | % By Philip Torr 2002
|
---|
| 2 | % copyright Microsoft Corp.
|
---|
| 3 |
|
---|
| 4 | %takes an essential matrix and a set of corrected matches, and outputs projection matrices, 3D points etc
|
---|
| 5 | %all via linear estimation; need camera calibration matrix too
|
---|
| 6 |
|
---|
| 7 | function [P1,P2,R,t,rot_axis,rot_angle,g] = torr_linear_EtoPX(E,matches,C,m3)
|
---|
| 8 |
|
---|
| 9 |
|
---|
| 10 | %stage 1 generate twisted pairs etc
|
---|
| 11 | [U,S,V] = svd(E);
|
---|
| 12 | %note that there is a one p[arameter family of SVD's for E
|
---|
| 13 |
|
---|
| 14 | if abs(S(3,3)) > 0.00001
|
---|
| 15 | error('E must be rank 2 to self calibrate');
|
---|
| 16 | end
|
---|
| 17 |
|
---|
| 18 | if abs(S(1,1) - S(2,2)) > 0.00001
|
---|
| 19 | error('E must have two equal singular values');
|
---|
| 20 | end
|
---|
| 21 |
|
---|
| 22 |
|
---|
| 23 |
|
---|
| 24 | %use Hartley matrices:
|
---|
| 25 | W = [0 -1 0; 1 0 0; 0 0 1];
|
---|
| 26 | Z = [0 1 0; -1 0 0; 0 0 0];
|
---|
| 27 |
|
---|
| 28 | Tx = U * Z * U';
|
---|
| 29 |
|
---|
| 30 | R1 = U * W * V';
|
---|
| 31 | R2 = U * W' * V';
|
---|
| 32 | R1 = R1 * sign(det(R1)) * sign(det(C));
|
---|
| 33 | R2 = R2 * sign(det(R2)) * sign(det(C));
|
---|
| 34 |
|
---|
| 35 |
|
---|
| 36 |
|
---|
| 37 | %the left epipole is, which gives the direction of translation
|
---|
| 38 | u3 = U(:,3);
|
---|
| 39 | %such that u3' * E = 0,
|
---|
| 40 |
|
---|
| 41 |
|
---|
| 42 | %next establish the four possible camera matrix pairs as points out in Maybank, Hartley & zisserman etc
|
---|
| 43 | %first camera is 3x4 at origin of the world system.
|
---|
| 44 | P1 = [C'; 0,0,0]';
|
---|
| 45 |
|
---|
| 46 | %given this there are four choices for the second, we normalize them so that the
|
---|
| 47 | %determinant of the first 3x3 is greater than zero, this is useful for determining chierality later
|
---|
| 48 | P21 = C * [ R1'; u3']';
|
---|
| 49 | P22 = C * [ R1'; -u3']';
|
---|
| 50 |
|
---|
| 51 | P23 = C * [ R2'; u3']';
|
---|
| 52 | P24 = C * [ R2'; -u3']';
|
---|
| 53 |
|
---|
| 54 | %next we take one point to determine the chierality of the camera
|
---|
| 55 |
|
---|
| 56 |
|
---|
| 57 | X1 = torr_triangulate(matches, m3, P1, P21);
|
---|
| 58 | X2 = torr_triangulate(matches, m3, P1, P22);
|
---|
| 59 | X3 = torr_triangulate(matches, m3, P1, P23);
|
---|
| 60 | X4 = torr_triangulate(matches, m3, P1, P24);
|
---|
| 61 |
|
---|
| 62 | %next reproject and compare with the images
|
---|
| 63 | %the chieral constraint is sign(det M) * sign (third homog coord of reprojected image) * sign (fourth homog coord X)
|
---|
| 64 | % to make sure we dont get any outliers we average the inequalities over all the points, ones with a bad sign
|
---|
| 65 | % can later be removed as outleirs.
|
---|
| 66 | %we want chieral for both cameras to be positive
|
---|
| 67 |
|
---|
| 68 | ax1 = P1 * X1;
|
---|
| 69 | %ax1 = ax1 *m3/ax1(3)
|
---|
| 70 |
|
---|
| 71 |
|
---|
| 72 | ax2 = P1 * X2;
|
---|
| 73 | %ax2 = ax2 *m3/ax2(3)
|
---|
| 74 |
|
---|
| 75 | ax3 = P1 * X3;
|
---|
| 76 | %ax3 = ax3 *m3/ax3(3)
|
---|
| 77 |
|
---|
| 78 | ax4 = P1 * X4;
|
---|
| 79 | %ax4 = ax4 *m3/ax4(3);
|
---|
| 80 |
|
---|
| 81 |
|
---|
| 82 | bx1 = P21 * X1;
|
---|
| 83 | %bx1 = bx1 *m3/bx1(3)
|
---|
| 84 |
|
---|
| 85 | bx2 = P22 * X2;
|
---|
| 86 | %bx2 = bx2 *m3/bx2(3)
|
---|
| 87 |
|
---|
| 88 | bx3 = P23 * X3;
|
---|
| 89 | %bx3 = bx3 *m3/bx3(3)
|
---|
| 90 |
|
---|
| 91 | bx4 = P24 * X4;
|
---|
| 92 | %bx4 = bx4 *m3/bx4(3);
|
---|
| 93 |
|
---|
| 94 | chieral1 = (sign(ax1(3,:) ) .* sign (X1(4,:))) + (sign(bx1(3,:) ) .* sign (X1(4,:)));
|
---|
| 95 | chieral2 = (sign(ax2(3,:) ) .* sign (X1(4,:))) + (sign(bx2(3,:) ) .* sign (X2(4,:)));
|
---|
| 96 | chieral3 = (sign(ax3(3,:) ) .* sign (X1(4,:))) + (sign(bx3(3,:) ) .* sign (X3(4,:)));
|
---|
| 97 | chieral4 = (sign(ax4(3,:) ) .* sign (X1(4,:))) + (sign(bx4(3,:) ) .* sign (X4(4,:)));
|
---|
| 98 |
|
---|
| 99 |
|
---|
| 100 | chieral_sum = [sum(chieral1) sum(chieral2) sum(chieral3) sum(chieral4)];
|
---|
| 101 |
|
---|
| 102 | [max_ch correct_interpretation] = max(chieral_sum);
|
---|
| 103 |
|
---|
| 104 | switch correct_interpretation
|
---|
| 105 | case 1
|
---|
| 106 | R = R1;
|
---|
| 107 | t = u3;
|
---|
| 108 | P2 = P21;
|
---|
| 109 |
|
---|
| 110 | case 2
|
---|
| 111 | R = R1;
|
---|
| 112 | t = -u3;
|
---|
| 113 | P2 = P22;
|
---|
| 114 |
|
---|
| 115 | case 3
|
---|
| 116 | R = R2;
|
---|
| 117 | t = u3;
|
---|
| 118 | P2 = P23;
|
---|
| 119 |
|
---|
| 120 | case 4
|
---|
| 121 | R = R2;
|
---|
| 122 | t = -u3;
|
---|
| 123 | P2 = P24;
|
---|
| 124 | end
|
---|
| 125 |
|
---|
| 126 | %next recover the parameters of the rotation...
|
---|
| 127 | % [VR,DR] = eig(R);
|
---|
| 128 | %
|
---|
| 129 | % dd = [DR(1,1), DR(2,2), DR(3,3)];
|
---|
| 130 | % [Y Index] = find(dd==1);
|
---|
| 131 | %
|
---|
| 132 | % %determine axis of rotation
|
---|
| 133 | % axis = VR(:,Index(1));
|
---|
| 134 | rot_axis = [R(3,2)-R(2,3), R(1,3) - R(3,1), R(2,1) - R(1,2)];
|
---|
| 135 | rot_axis = rot_axis /norm(rot_axis);
|
---|
| 136 | rot_angle = acos( (trace(R)-1)/2);
|
---|
| 137 |
|
---|
| 138 | [a b] = torr_unit2sphere(rot_axis);
|
---|
| 139 | [ta tb] = torr_unit2sphere(t);
|
---|
| 140 |
|
---|
| 141 | %put together intrinisc and extrinsic parameters
|
---|
| 142 | %
|
---|
| 143 | % %here p is the set of paramets such that
|
---|
| 144 | % g(1) = focal length
|
---|
| 145 | % g(2-3) rotation axis
|
---|
| 146 | % g(4) rotation angle
|
---|
| 147 | % g(5-6) translation direction
|
---|
| 148 | g(1) = 1/C(3,3);
|
---|
| 149 | g(2) = a;
|
---|
| 150 | g(3) = b;
|
---|
| 151 | g(4) = rot_angle;
|
---|
| 152 | g(5) = ta;
|
---|
| 153 | g(6) = tb;
|
---|
| 154 |
|
---|
| 155 |
|
---|
| 156 | %
|
---|
| 157 | % CCC = C;
|
---|
| 158 | % %convert intrinsic and extinsics to a F matrix
|
---|
| 159 | % C(3,3) = 1/g(1);
|
---|
| 160 | % rot_axis2 = torr_sphere2unit([g(2) g(3)]);
|
---|
| 161 | % tt = torr_sphere2unit([g(5) g(6)]);
|
---|
| 162 | % rot_angle2 = g(4);
|
---|
| 163 | %
|
---|
| 164 | % %Rogregues
|
---|
| 165 | % II = [1 0 0; 0 1 0; 0 0 1];
|
---|
| 166 | % AX = torr_skew_sym(rot_axis2);
|
---|
| 167 | %
|
---|
| 168 | % %note -sin produce RR'
|
---|
| 169 | % RR = (cos(rot_angle2) * II +sin(rot_angle2) * AX + (1 - cos(rot_angle2)) * rot_axis2 * rot_axis2');
|
---|
| 170 | %
|
---|
| 171 | % TX = torr_skew_sym(tt);
|
---|
| 172 | % nnE = TX * RR;
|
---|
| 173 | %
|
---|
| 174 | % %F = inv(C') * nnE * inv(C);
|
---|
| 175 | % F = inv(C') * nnE * inv(C);
|
---|
| 176 | % f = reshape(F,9,1);
|
---|
| 177 |
|
---|
| 178 |
|
---|
| 179 |
|
---|
| 180 |
|
---|
| 181 |
|
---|