bazar  1.3.1
gradient.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <iostream>
3 #include <cv.h>
4 
5 using namespace std;
6 
14 void showmatrix_ch(CvMat &M){
15  //ios::sync_with_stdio();
16 
17  for(int i=0; i < M.cols; i++) {
18  bool detected = false;
19  for(int j=0; j < M.rows; j++) {
20  if( cvmGet(&M,j,i) != 0 )
21  detected = true;
22  }
23  if( detected ){
24  printf("View %i:",i);
25  for(int j=0; j < M.rows; j++) {
26  printf(" %i",(int)cvmGet(&M,j,i) );
27  }
28  printf("\n");
29  }
30  }
31  printf("\n");
32 }
33 
40 void showmatrix(CvMat &M){
41  //ios::sync_with_stdio();
42 
43  for(int i=0; i < M.rows; i++) {
44  for(int j=0; j < M.cols; j++) {
45  printf(" %12.6f ",cvmGet(&M,i,j) );
46  }
47  printf("\n");
48  }
49  if (M.rows >=3 && M.cols >=3 && (M.rows == M.cols)) {
50  CvMat sub;
51  cvGetSubRect(&M, &sub, cvRect(0,0, 3, 3));
52  printf("Det: %f\n", cvDet(&sub));
53  }
54  printf("\n");
55 }
56 
64 void showmatrix(CvMat &M,const char *header){
65  cout << header << "\n";
66  showmatrix(M);
67 }
68 
76 void showmatrix(const char *header,CvMat &M){
77  showmatrix(M,header);
78 }
79 
85 void showmatrix_file(CvMat &M,FILE *stream){
86  //ios::sync_with_stdio();
87  char buffer[1000];
88  int length;
89  for(int i=0; i < M.rows; i++) {
90  for(int j=0; j < M.cols; j++) {
91  length = sprintf(buffer," %12.6f ",cvmGet(&M,i,j));
92  fwrite( buffer, sizeof(char), length, stream );
93  }
94  length = sprintf(buffer,"\n");
95  fwrite( buffer, sizeof(char), length, stream );
96  }
97  length = sprintf(buffer,"\n");
98  fwrite( buffer, sizeof(char), length, stream );
99 }
100 
101 void twomat_gradian (
102  double fx,
103  double fy,
104  double cx,
105  double cy,
106  double R_matrix[3][3],
107  double Rx,
108  double Ry,
109  double Rz,
110  CvMat *R_jacobian,
111  double S_matrix[3][3],
112  double Sx,
113  double Sy,
114  double Sz,
115  CvMat *S_jacobian,
116  double p[3],
117  double cgret[24],
118  double uv[2] )
119 {
120 
121  double t24;
122 
123  double t7;
124 
125  double t208;
126 
127  double t2;
128 
129  double t158;
130 
131  double t26;
132 
133  double t8;
134 
135  double t276;
136 
137  double t5;
138 
139  double t29;
140 
141  double t1;
142 
143  double t60;
144 
145  double t280;
146 
147  double t282;
148 
149  double t284;
150 
151  double t86;
152 
153  double t11;
154 
155  double t327;
156 
157  double t17;
158 
159  double t168;
160 
161  double t458;
162 
163  double t460;
164 
165  double t376;
166 
167  double t380;
168 
169  double t266;
170 
171  double t57;
172 
173  double t81;
174 
175  double t82;
176 
177  double t13;
178 
179  double t137;
180 
181  double t402;
182 
183  double t404;
184 
185  double t187;
186 
187  double t189;
188 
189  double t191;
190 
191  double t4;
192 
193  double t109;
194 
195  double t110;
196 
197  double t36;
198 
199  double t62;
200 
201  double t111;
202 
203  double t114;
204 
205  double t116;
206 
207  double t38;
208 
209  double t235;
210 
211  double t237;
212 
213  double t217;
214 
215  double t219;
216 
217  double t221;
218 
219  double t40;
220 
221  double t15;
222 
223  double t64;
224 
225  double t141;
226  double t108;
227 
228  double t139;
229 
230  double t22;
231 
232  double t88;
233 
234  double t311;
235 
236  double t313;
237 
238  double t315;
239 
240  double t319;
241 
242  double t229;
243 
244  double t258;
245 
246  double t264;
247 
248  double t430;
249 
250  double t426;
251 
252  double t432;
253 
254  double t434;
255 
256  double t268;
257 
258  double t272;
259 
260  double t274;
261 
262  double t20;
263 
264  double t305;
265 
266  double t225;
267 
268  double t227;
269 
270  double t233;
271 
272  double t406;
273 
274  double t84;
275 
276  double t321;
277 
278  double t323;
279 
280  double t329;
281 
282  double t331;
283 
284  double t352;
285 
286  double t164;
287 
288  double t166;
289 
290  double t378;
291 
292  double t118;
293 
294  double t427;
295 
296  double t462;
297 
298  t1 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 1-1 + (1-1)*3);
299 
300  t2 = S_matrix[1-1][1-1];
301 
302  t4 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 2-1 + (1-1)*3);
303 
304  t5 = S_matrix[2-1][1-1];
305 
306  t7 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 3-1 + (1-1)*3);
307 
308  t8 = S_matrix[3-1][1-1];
309 
310  t11 = p[0];
311 
312  t13 = S_matrix[1-1][2-1];
313 
314  t15 = S_matrix[2-1][2-1];
315 
316  t17 = S_matrix[3-1][2-1];
317 
318  t20 = p[1];
319 
320  t22 = S_matrix[1-1][3-1];
321 
322  t24 = S_matrix[2-1][3-1];
323 
324  t26 = S_matrix[3-1][3-1];
325 
326  t29 = p[2];
327 
328  t36 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 1-1 + (3-1)*3);
329 
330  t38 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 2-1 + (3-1)*3);
331 
332  t40 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 3-1 + (3-1)*3);
333 
334  t57 = (t36 * t2 + t38 * t5 + t40 * t8) * t11 + (t36 * t13 + t38 * t15 + t40 * t17) * t20 + (t36 * t22 + t38 * t24 + t40 * t26) * t29 + t36 * Sx + t38 * Sy + t40 * Sz;
335 
336  t60 = R_matrix[3-1][1-1];
337 
338  t62 = R_matrix[3-1][2-1];
339 
340  t64 = R_matrix[3-1][3-1];
341 
342  t81 = (t60 * t2 + t62 * t5 + t64 * t8) * t11 + (t60 * t13 + t62 * t15 + t64 * t17) * t20 + (t60 * t22 + t62 * t24 + t64 * t26) * t29 + t60 * Sx + t62 * Sy + t64 * Sz + Rz;
343 
344  t82 = 0.1e1 / t81;
345 
346  t84 = R_matrix[1-1][1-1];
347 
348  t86 = R_matrix[1-1][2-1];
349 
350  t88 = R_matrix[1-1][3-1];
351  t108 = fx * ((t84 * t2 + t86 * t5 + t88 * t8) * t11 + (t84 * t13 + t86 * t15 + t88 * t17) * t20 + (t84 * t22 + t86 * t24 + t88 * t26) * t29 + t84 * Sx + t86 * Sy + t88 * Sz + Rx) + cx * t81;
352  t109 = t81 * t81;
353 
354  t110 = 0.1e1 / t109;
355 
356  t111 = (fx * ((t84 * t2 + t86 * t5 + t88 * t8) * t11 + (t84 * t13 + t86 * t15 + t88 * t17) * t20 + (t84 * t22 + t86 * t24 + t88 * t26) * t29 + t84 * Sx + t86 * Sy + t88 * Sz + Rx) + cx * t81) * t110;
357 
358  t114 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 1-1 + (1-1)*3);
359 
360  t116 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 2-1 + (1-1)*3);
361 
362  t118 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 3-1 + (1-1)*3);
363 
364  t137 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 1-1 + (3-1)*3);
365 
366  t139 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 2-1 + (3-1)*3);
367 
368  t141 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 3-1 + (3-1)*3);
369 
370  t158 = (t137 * t2 + t139 * t5 + t141 * t8) * t11 + (t137 * t13 + t139 * t15 + t141 * t17) * t20 + (t137 * t22 + t139 * t24 + t141 * t26) * t29 + t137 * Sx + t139 * Sy + t141 * Sz;
371 
372  t164 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 1-1 + (1-1)*3);
373 
374  t166 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 2-1 + (1-1)*3);
375 
376  t168 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 3-1 + (1-1)*3);
377 
378  t187 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 1-1 + (3-1)*3);
379 
380  t189 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 2-1 + (3-1)*3);
381 
382  t191 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 3-1 + (3-1)*3);
383 
384  t208 = (t187 * t2 + t189 * t5 + t191 * t8) * t11 + (t187 * t13 + t189 * t15 + t191 * t17) * t20 + (t187 * t22 + t189 * t24 + t191 * t26) * t29 + t187 * Sx + t189 * Sy + t191 * Sz;
385 
386  t217 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 1-1 + (1-1)*3);
387 
388  t219 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 1-1 + (2-1)*3);
389 
390  t221 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 1-1 + (3-1)*3);
391 
392  t225 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 2-1 + (1-1)*3);
393 
394  t227 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 2-1 + (2-1)*3);
395 
396  t229 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 2-1 + (3-1)*3);
397 
398  t233 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 3-1 + (1-1)*3);
399 
400  t235 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 3-1 + (2-1)*3);
401 
402  t237 = CV_MAT_ELEM(*S_jacobian, double, 1-1, 3-1 + (3-1)*3);
403 
404  t258 = (t60 * t217 + t62 * t219 + t64 * t221) * t11 + (t60 * t225 + t62 * t227 + t64 * t229) * t20 + (t60 * t233 + t62 * t235 + t64 * t237) * t29;
405 
406  t264 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 1-1 + (1-1)*3);
407 
408  t266 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 1-1 + (2-1)*3);
409 
410  t268 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 1-1 + (3-1)*3);
411 
412  t272 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 2-1 + (1-1)*3);
413 
414  t274 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 2-1 + (2-1)*3);
415 
416  t276 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 2-1 + (3-1)*3);
417 
418  t280 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 3-1 + (1-1)*3);
419 
420  t282 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 3-1 + (2-1)*3);
421 
422  t284 = CV_MAT_ELEM(*S_jacobian, double, 2-1, 3-1 + (3-1)*3);
423 
424  t305 = (t60 * t264 + t62 * t266 + t64 * t268) * t11 + (t60 * t272 + t62 * t274 + t64 * t276) * t20 + (t60 * t280 + t62 * t282 + t64 * t284) * t29;
425 
426  t311 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 1-1 + (1-1)*3);
427 
428  t313 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 1-1 + (2-1)*3);
429 
430  t315 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 1-1 + (3-1)*3);
431 
432  t319 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 2-1 + (1-1)*3);
433 
434  t321 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 2-1 + (2-1)*3);
435 
436  t323 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 2-1 + (3-1)*3);
437 
438  t327 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 3-1 + (1-1)*3);
439 
440  t329 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 3-1 + (2-1)*3);
441 
442  t331 = CV_MAT_ELEM(*S_jacobian, double, 3-1, 3-1 + (3-1)*3);
443 
444  t352 = (t60 * t311 + t62 * t313 + t64 * t315) * t11 + (t60 * t319 + t62 * t321 + t64 * t323) * t20 + (t60 * t327 + t62 * t329 + t64 * t331) * t29;
445 
446  t376 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 1-1 + (2-1)*3);
447 
448  t378 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 2-1 + (2-1)*3);
449 
450  t380 = CV_MAT_ELEM(*R_jacobian, double, 1-1, 3-1 + (2-1)*3);
451 
452  t402 = R_matrix[2-1][1-1];
453 
454  t404 = R_matrix[2-1][2-1];
455 
456  t406 = R_matrix[2-1][3-1];
457 
458  t426 = fy * ((t402 * t2 + t404 * t5 + t406 * t8) * t11 + (t402 * t13 + t404 * t15 + t406 * t17) * t20 + (t402 * t22 + t404 * t24 + t406 * t26) * t29 + t402 * Sx + t404 * Sy + t406 * Sz + Ry) + cy * t81;
459 
460  t427 = (fy * ((t402 * t2 + t404 * t5 + t406 * t8) * t11 + (t402 * t13 + t404 * t15 + t406 * t17) * t20 + (t402 * t22 + t404 * t24 + t406 * t26) * t29 + t402 * Sx + t404 * Sy + t406 * Sz + Ry) + cy * t81) * t110;
461 
462  t430 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 1-1 + (2-1)*3);
463 
464  t432 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 2-1 + (2-1)*3);
465 
466  t434 = CV_MAT_ELEM(*R_jacobian, double, 2-1, 3-1 + (2-1)*3);
467 
468  t458 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 1-1 + (2-1)*3);
469 
470  t460 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 2-1 + (2-1)*3);
471 
472  t462 = CV_MAT_ELEM(*R_jacobian, double, 3-1, 3-1 + (2-1)*3);
473 
474  cgret[0] = (fx * ((t1 * t2 + t4 * t5 + t7 * t8) * t11 + (t1 * t13 + t4 * t15 + t7 * t17) * t20 + (t1 * t22 + t4 * t24 + t7 * t26) * t29 + t1 * Sx + t4 * Sy + t7 * Sz) + cx * t57) * t82 - t111 * t57;
475 
476  cgret[1] = (fx * ((t114 * t2 + t116 * t5 + t118 * t8) * t11 + (t114 * t13 + t116 * t15 + t118 * t17) * t20 + (t114 * t22 + t116 * t24 + t118 * t26) * t29 + t114 * Sx + t116 * Sy + t118 * Sz) + cx * t158) * t82 - t111 * t158;
477 
478  cgret[2] = (fx * ((t164 * t2 + t166 * t5 + t168 * t8) * t11 + (t164 * t13 + t166 * t15 + t168 * t17) * t20 + (t164 * t22 + t166 * t24 + t168 * t26) * t29 + t164 * Sx + t166 * Sy + t168 * Sz) + cx * t208) * t82 - t111 * t208;
479 
480  cgret[3] = fx * t82;
481 
482  cgret[4] = 0;
483 
484  cgret[5] = cx * t82 - t111;
485 
486  cgret[6] = (fx * ((t84 * t217 + t86 * t219 + t88 * t221) * t11 + (t84 * t225 + t86 * t227 + t88 * t229) * t20 + (t84 * t233 + t86 * t235 + t88 * t237) * t29) + cx * t258) * t82 - t111 * t258;
487 
488  cgret[7] = (fx * ((t84 * t264 + t86 * t266 + t88 * t268) * t11 + (t84 * t272 + t86 * t274 + t88 * t276) * t20 + (t84 * t280 + t86 * t282 + t88 * t284) * t29) + cx * t305) * t82 - t111 * t305;
489 
490  cgret[8] = (fx * ((t84 * t311 + t86 * t313 + t88 * t315) * t11 + (t84 * t319 + t86 * t321 + t88 * t323) * t20 + (t84 * t327 + t86 * t329 + t88 * t331) * t29) + cx * t352) * t82 - t111 * t352;
491 
492  cgret[9] = (fx * t84 + cx * t60) * t82 - t111 * t60;
493 
494  cgret[10] = (fx * t86 + cx * t62) * t82 - t111 * t62;
495 
496  cgret[11] = (fx * t88 + cx * t64) * t82 - t111 * t64;
497 
498  cgret[12] = (fy * ((t376 * t2 + t378 * t5 + t380 * t8) * t11 + (t376 * t13 + t378 * t15 + t380 * t17) * t20 + (t376 * t22 + t378 * t24 + t380 * t26) * t29 + t376 * Sx + t378 * Sy + t380 * Sz) + cy * t57) * t82 - t427 * t57;
499 
500  cgret[13] = (fy * ((t430 * t2 + t432 * t5 + t434 * t8) * t11 + (t430 * t13 + t432 * t15 + t434 * t17) * t20 + (t430 * t22 + t432 * t24 + t434 * t26) * t29 + t430 * Sx + t432 * Sy + t434 * Sz) + cy * t158) * t82 - t427 * t158;
501 
502  cgret[14] = (fy * ((t458 * t2 + t460 * t5 + t462 * t8) * t11 + (t458 * t13 + t460 * t15 + t462 * t17) * t20 + (t458 * t22 + t460 * t24 + t462 * t26) * t29 + t458 * Sx + t460 * Sy + t462 * Sz) + cy * t208) * t82 - t427 * t208;
503 
504  cgret[15] = 0;
505 
506  cgret[16] = fy * t82;
507 
508  cgret[17] = cy * t82 - t427;
509 
510  cgret[18] = (fy * ((t402 * t217 + t404 * t219 + t406 * t221) * t11 + (t402 * t225 + t404 * t227 + t406 * t229) * t20 + (t402 * t233 + t404 * t235 + t406 * t237) * t29) + cy * t258) * t82 - t427 * t258;
511 
512  cgret[19] = (fy * ((t402 * t264 + t404 * t266 + t406 * t268) * t11 + (t402 * t272 + t404 * t274 + t406 * t276) * t20 + (t402 * t280 + t404 * t282 + t406 * t284) * t29) + cy * t305) * t82 - t427 * t305;
513 
514  cgret[20] = (fy * ((t402 * t311 + t404 * t313 + t406 * t315) * t11 + (t402 * t319 + t404 * t321 + t406 * t323) * t20 + (t402 * t327 + t404 * t329 + t406 * t331) * t29) + cy * t352) * t82 - t427 * t352;
515  cgret[21] = (fy * t402 + cy * t60) * t82 - t427 * t60;
516  cgret[22] = (fy * t404 + cy * t62) * t82 - t427 * t62;
517  cgret[23] = (fy * t406 + cy * t64) * t82 - t427 * t64;
518 
519 
520  uv[0] = t108 * t82;
521  uv[1] = t426 * t82;
522 
523 }
Generated on Tue Jan 15 2013 12:50:21 for bazar by   doxygen 1.8.1.2