source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/third_party/sba-1.3/demo/imgproj.c @ 37

Last change on this file since 37 was 37, checked in by (none), 14 years ago

Added original make3d
File size: 7.7 KB
Line 
1/* Code automatically generated by maple's codegen package */
2
3/* Computation of the predicted projection of a 3D point and its jacobians */
4
5#include <math.h>
6
7#define IDX3x3_11 0
8#define IDX3x3_12 1
9#define IDX3x3_13 2
10#define IDX3x3_21 3
11#define IDX3x3_22 4
12#define IDX3x3_23 5
13#define IDX3x3_31 6
14#define IDX3x3_32 7
15#define IDX3x3_33 8
16
17/* computes Q in "n", i.e. the predicted projection of a point "M" on a camera
18 * whose rotational and translational parameters are "qr" and "t". "a" is the
19 * camera intrisic calibration matrix
20 */
21void calcImgProj(a,qr,t,M,n)
22double a[9];
23double qr[4];
24double t[3];
25double M[3];
26double n[2];
27{
28  double t11;
29  double t13;
30  double t14;
31  double t15;
32  double t16;
33  double t17;
34  double t19;
35  double t2;
36  double t21;
37  double t25;
38  double t3;
39  double t30;
40  double t36;
41  double t5;
42  double t51;
43  double t6;
44  double t62;
45  double t65;
46  double t8;
47  double t9;
48  {
49    t2 = qr[1];
50    t3 = M[0];
51    t5 = qr[2];
52    t6 = M[1];
53    t8 = qr[3];
54    t9 = M[2];
55    t11 = -t2*t3-t5*t6-t8*t9;
56    t13 = qr[0];
57    t14 = t13*t13;
58    t15 = t2*t2;
59    t16 = t5*t5;
60    t17 = t8*t8;
61    t19 = 1/(t14+t15+t16+t17);
62    t21 = t13*t19;
63    t25 = t13*t3+t5*t9-t8*t6;
64    t30 = t13*t6+t8*t3-t2*t9;
65    t36 = t13*t9+t2*t6-t5*t3;
66    t51 = -t11*t5*t19+t21*t30-t36*t2*t19+t25*t8*t19+t[1];
67    t62 = -t11*t8*t19+t21*t36-t25*t5*t19+t30*t2*t19+t[2];
68    t65 = 1/t62;
69    n[0] = (a[IDX3x3_11]*(-t11*t2*t19+t25*t21-t30*t8*t19+t36*t5*t19+t[0])+a
70[IDX3x3_12]*t51+a[IDX3x3_13]*t62)*t65;
71    n[1] = (a[IDX3x3_22]*t51+a[IDX3x3_23]*t62)*t65;
72    return;
73  }
74}
75
76/* computes A and B in "jacmRT" and "jacmS". In other words, for a point "M"
77 * projecting on a camera whose rotational and translational parameters are "qr"
78 * and "t", "jacmRT" is the jacobian of its predicted image projection with
79 * regard to "qr", "t". "jacmS" is the jacobian of "M"'s predicted projection
80 * with regard to "M". "a" is the camera intrisic calibration matrix.
81 * Either "jacmRT" or "jacmS" can be NULL, in which case they are not computed
82 */
83void calcImgProjJacRTS(a,qr,t,M,jacmRT,jacmS)
84double a[9];
85double qr[4];
86double t[3];
87double M[3];
88double jacmRT[2][7];
89double jacmS[2][3];
90{
91  double t1;
92  double t10;
93  double t11;
94  double t110;
95  double t114;
96  double t115;
97  double t116;
98  double t118;
99  double t12;
100  double t120;
101  double t127;
102  double t129;
103  double t13;
104  double t130;
105  double t137;
106  double t138;
107  double t14;
108  double t140;
109  double t141;
110  double t143;
111  double t147;
112  double t15;
113  double t153;
114  double t156;
115  double t16;
116  double t162;
117  double t17;
118  double t172;
119  double t18;
120  double t187;
121  double t188;
122  double t19;
123  double t191;
124  double t197;
125  double t2;
126  double t20;
127  double t21;
128  double t210;
129  double t220;
130  double t228;
131  double t24;
132  double t240;
133  double t241;
134  double t242;
135  double t243;
136  double t247;
137  double t248;
138  double t250;
139  double t253;
140  double t254;
141  double t256;
142  double t26;
143  double t269;
144  double t27;
145  double t272;
146  double t273;
147  double t275;
148  double t28;
149  double t289;
150  double t29;
151  double t291;
152  double t3;
153  double t30;
154  double t33;
155  double t34;
156  double t35;
157  double t37;
158  double t38;
159  double t39;
160  double t4;
161  double t40;
162  double t42;
163  double t43;
164  double t45;
165  double t46;
166  double t47;
167  double t48;
168  double t5;
169  double t50;
170  double t53;
171  double t54;
172  double t57;
173  double t6;
174  double t60;
175  double t61;
176  double t62;
177  double t64;
178  double t65;
179  double t66;
180  double t68;
181  double t69;
182  double t7;
183  double t71;
184  double t72;
185  double t75;
186  double t78;
187  double t79;
188  double t8;
189  double t80;
190  double t82;
191  double t83;
192  double t84;
193  double t86;
194  double t87;
195  double t9;
196  double t95;
197  double t96;
198  {
199    t1 = a[IDX3x3_11];
200    t2 = qr[1];
201    t3 = M[0];
202    t4 = t2*t3;
203    t5 = qr[2];
204    t6 = M[1];
205    t7 = t5*t6;
206    t8 = qr[3];
207    t9 = M[2];
208    t10 = t8*t9;
209    t11 = -t4-t7-t10;
210    t12 = t11*t2;
211    t13 = qr[0];
212    t14 = t13*t13;
213    t15 = t2*t2;
214    t16 = t5*t5;
215    t17 = t8*t8;
216    t18 = t14+t15+t16+t17;
217    t19 = t18*t18;
218    t20 = 1/t19;
219    t21 = t20*t13;
220    t24 = 1/t18;
221    t26 = t5*t9;
222    t27 = t8*t6;
223    t28 = t13*t3+t26-t27;
224    t29 = t24*t28;
225    t30 = t14*t20;
226    t33 = t13*t24;
227    t34 = t33*t3;
228    t35 = t27*t24;
229    t37 = t8*t3;
230    t38 = t2*t9;
231    t39 = t13*t6+t37-t38;
232    t40 = t39*t8;
233    t42 = 2.0*t40*t21;
234    t43 = t26*t24;
235    t45 = t2*t6;
236    t46 = t5*t3;
237    t47 = t13*t9+t45-t46;
238    t48 = t47*t5;
239    t50 = 2.0*t48*t21;
240    t53 = a[IDX3x3_12];
241    t54 = t11*t5;
242    t57 = t24*t39;
243    t60 = t33*t6;
244    t61 = t38*t24;
245    t62 = t47*t2;
246    t64 = 2.0*t62*t21;
247    t65 = t37*t24;
248    t66 = t28*t8;
249    t68 = 2.0*t66*t21;
250    t69 = 2.0*t54*t21+t57-2.0*t30*t39+t60-t61+t64+t65-t68;
251    t71 = a[IDX3x3_13];
252    t72 = t11*t8;
253    t75 = t24*t47;
254    t78 = t33*t9;
255    t79 = t46*t24;
256    t80 = t28*t5;
257    t82 = 2.0*t80*t21;
258    t83 = t45*t24;
259    t84 = t39*t2;
260    t86 = 2.0*t21*t84;
261    t87 = 2.0*t21*t72+t75-2.0*t30*t47+t78-t79+t82+t83-t86;
262    t95 = -t72*t24+t33*t47-t80*t24+t84*t24+t[2];
263    t96 = 1/t95;
264    t110 = -t54*t24+t33*t39-t62*t24+t66*t24+t[1];
265    t114 = t95*t95;
266    t115 = 1/t114;
267    t116 = (t1*(-t12*t24+t33*t28-t40*t24+t48*t24+t[0])+t53*t110+t71*t95)*t115;
268    t118 = a[IDX3x3_22];
269    t120 = a[IDX3x3_23];
270    t127 = (t118*t110+t120*t95)*t115;
271
272    if(jacmRT){
273      jacmRT[0][0] = (t1*(2.0*t12*t21+t29-2.0*t30*t28+t34-t35+t42+t43-t50)+t53*
274    t69+t71*t87)*t96-t116*t87;
275      jacmRT[1][0] = (t118*t69+t120*t87)*t96-t127*t87;
276
277      t129 = t4*t24;
278      t130 = t11*t24;
279      t137 = t10*t24;
280      t138 = t20*t2;
281      t140 = 2.0*t40*t138;
282      t141 = t7*t24;
283      t143 = 2.0*t48*t138;
284      t147 = 2.0*t54*t138;
285      t153 = t79+t147-t86-t78-t83-t75+2.0*t47*t15*t20-2.0*t66*t138;
286      t156 = 2.0*t72*t138;
287      t162 = t65+t156-t64+t60+2.0*t80*t138-t61+t57-2.0*t39*t15*t20;
288      jacmRT[0][1] = (t1*(t129-t130+2.0*t11*t15*t20-2.0*t21*t28*t2+t137+t140+t141
289    -t143)+t53*t153+t71*t162)*t96-t116*t162;
290      jacmRT[1][1] = (t118*t153+t120*t162)*t96-t127*t162;
291      t172 = t20*t5;
292      t187 = 2.0*t66*t172;
293      t188 = t141-t130+2.0*t11*t16*t20-2.0*t21*t39*t5+t129+t143+t137-t187;
294      t191 = 2.0*t72*t172;
295      t197 = t35+t191-t50-t34-t43-t29+2.0*t28*t16*t20-2.0*t84*t172;
296      jacmRT[0][2] = (t1*(t83+t147-t82+t78+2.0*t40*t172-t79+t75-2.0*t47*t16*t20)+
297    t53*t188+t71*t197)*t96-t116*t197;
298      jacmRT[1][2] = (t118*t188+t120*t197)*t96-t127*t197;
299      t210 = t20*t8;
300      t220 = t43+t191-t42+t34+2.0*t62*t210-t35+t29-2.0*t28*t17*t20;
301      t228 = t137-t130+2.0*t11*t17*t20-2.0*t21*t47*t8+t141+t187+t129-t140;
302      jacmRT[0][3] = (t1*(t61+t156-t68-t60-t65-t57+2.0*t39*t17*t20-2.0*t48*t210)+
303    t53*t220+t71*t228)*t96-t116*t228;
304      jacmRT[1][3] = (t118*t220+t120*t228)*t96-t127*t228;
305      jacmRT[0][4] = t1*t96;
306      jacmRT[1][4] = 0.0;
307      jacmRT[0][5] = t53*t96;
308      jacmRT[1][5] = t96*t118;
309      jacmRT[0][6] = t71*t96-t116;
310      jacmRT[1][6] = t120*t96-t127;
311    }
312
313    if(jacmS){
314      t240 = t15*t24;
315      t241 = t14*t24;
316      t242 = t17*t24;
317      t243 = t16*t24;
318      t247 = t2*t5*t24;
319      t248 = t33*t8;
320      t250 = 2.0*t247+2.0*t248;
321      t253 = t2*t8*t24;
322      t254 = t33*t5;
323      t256 = 2.0*t253-2.0*t254;
324      jacmS[0][0] = (t1*(t240+t241-t242-t243)+t53*t250+t71*t256)*t96-t116*t256;
325      jacmS[1][0] = (t118*t250+t120*t256)*t96-t127*t256;
326      t269 = t243+t241-t240-t242;
327      t272 = t5*t8*t24;
328      t273 = t33*t2;
329      t275 = 2.0*t272+2.0*t273;
330      jacmS[0][1] = (t1*(2.0*t247-2.0*t248)+t53*t269+t71*t275)*t96-t116*t275;
331      jacmS[1][1] = (t118*t269+t120*t275)*t96-t127*t275;
332      t289 = 2.0*t272-2.0*t273;
333      t291 = t242+t241-t243-t240;
334      jacmS[0][2] = (t1*(2.0*t253+2.0*t254)+t53*t289+t71*t291)*t96-t116*t291;
335      jacmS[1][2] = (t118*t289+t120*t291)*t96-t127*t291;
336    }
337    return;
338  }
339}
Note: See TracBrowser for help on using the repository browser.