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