Hugintrunk  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
SpaceTransform.cpp
Go to the documentation of this file.
1 // -*- c-basic-offset: 4 -*-
2 
27 #include "SpaceTransform.h"
28 
29 namespace HuginBase {
30 namespace Nona {
31 
32 
34 SpaceTransform::SpaceTransform() : m_srcTX(0), m_srcTY(0), m_destTX(0), m_destTY(0)
35 {
36 
37  m_Initialized = false;
38 }
39 
42 {
43 }
44 
45 void SpaceTransform::AddTransform( trfn function_name, double var0, double var1, double var2, double var3, double var4, double var5, double var6, double var7 )
46 {
47  fDescription fD;
48  fD.param.var0 = var0;
49  fD.param.var1 = var1;
50  fD.param.var2 = var2;
51  fD.param.var3 = var3;
52  fD.param.var4 = var4;
53  fD.param.var5 = var5;
54  fD.param.var6 = var6;
55  fD.param.var7 = var7;
56  fD.func = function_name;
57  m_Stack.push_back( fD );
58 }
59 
60 void SpaceTransform::AddTransform( trfn function_name, Matrix3 m, double var0, double var1, double var2, double var3)
61 {
62  fDescription fD;
63  fD.param.distance = var0;
64  fD.param.var1 = var1;
65  fD.param.var2 = var2;
66  fD.param.var3 = var3;
67  fD.param.mt = m;
68  fD.func = function_name;
69  m_Stack.push_back( fD );
70 }
71 
72 
73 
74 
75 //==============================================================================
76 // Pano12.dll (math.c) functions, helmut dersch
77 
78 #define MAXITER 100
79 #define R_EPS 1.0e-6
80 
81 // Rotate equirectangular image
82 void rotate_erect( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams &params)
83 {
84  // params: double 180degree_turn(screenpoints), double turn(screenpoints);
85  *x_src = x_dest + params.var1;
86  while( *x_src < -params.var0 )
87  *x_src += 2 * params.var0;
88  while( *x_src > params.var0 )
89  *x_src -= 2 * params.var0;
90  *y_src = y_dest;
91 }
92 
93 // Calculate inverse 4th order polynomial correction using Newton
94 // Don't use on large image (slow)!
95 void inv_radial( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams &params)
96 {
97  // params: double coefficients[5]
98  double rs, rd, f, scale;
99  int iter = 0;
100 
101  rd = (sqrt( x_dest*x_dest + y_dest*y_dest )) / params.var4; // Normalized
102 
103  rs = rd;
104  f = (((params.var3 * rs + params.var2) * rs + params.var1) * rs + params.var0) * rs;
105 
106  while( std::abs(f - rd) > R_EPS && iter++ < MAXITER )
107  {
108  rs = rs - (f - rd) / ((( 4 * params.var3 * rs + 3 * params.var2) * rs +
109  2 * params.var1) * rs + params.var0);
110 
111  f = (((params.var3 * rs + params.var2) * rs +
112  params.var1) * rs + params.var0) * rs;
113  }
114 
115  scale = rs / rd;
116 // printf("scale = %lg iter = %d\n", scale,iter);
117 
118  *x_src = x_dest * scale ;
119  *y_src = y_dest * scale ;
120 }
121 
122 /*
123 static void inv_vertical( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams &params)
124 {
125  // params: double coefficients[4]
126  register double rs, rd, f, scale;
127  int iter = 0;
128 
129  rd = abs( y_dest ) / params.var4; // Normalized
130  rs = rd;
131  f = (((params.var3 * rs + params.var2) * rs + params.var1) * rs + params.var0) * rs;
132 
133  while( abs(f - rd) > R_EPS && iter++ < MAXITER )
134  {
135  rs = rs - (f - rd) / ((( 4 * params.var3 * rs + 3 * params.var2) * rs +
136  2 * params.var1) * rs + params.var0);
137 
138  f = (((params.var3 * rs + params.var2) * rs + params.var1) * rs + params.var0) * rs;
139  }
140 
141  scale = rs / rd;
142 // printf("scale = %lg iter = %d\n", scale,iter);
143 
144  *x_src = x_dest ;
145  *y_src = y_dest * scale ;
146 }
147 */
148 
149 // scale
150 void resize( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
151 {
152  // params: double scale_horizontal, double scale_vertical;
153  *x_src = x_dest * params.var0;
154  *y_src = y_dest * params.var1;
155 }
156 
157 /*
158 // shear
159 static void shear( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
160 {
161  // params: double shear_horizontal, double shear_vertical;
162  *x_src = x_dest + params.var0 * y_dest;
163  *y_src = y_dest + params.var1 * x_dest;
164 }
165 */
166 
167 // horiz shift
168 void horiz( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
169 {
170  // params: double horizontal params.shift
171  *x_src = x_dest + params.shift;
172  *y_src = y_dest;
173 }
174 
175 // vertical shift
176 void vert( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
177 {
178  // params: double vertical params.shift
179  *x_src = x_dest;
180  *y_src = y_dest + params.shift;
181 }
182 
183 // radial
184 void radial( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
185 {
186  // params: double coefficients[4], scale, correction_radius
187  double r, scale;
188 
189  r = (sqrt( x_dest*x_dest + y_dest*y_dest )) / params.var4;
190  if( r < params.var5 )
191  {
192  scale = ((params.var3 * r + params.var2) * r + params.var1) * r + params.var0;
193  }
194  else
195  scale = 1000.0;
196 
197  *x_src = x_dest * scale ;
198  *y_src = y_dest * scale ;
199 }
200 
201 /*
202 //
203 static void vertical( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
204 {
205  // params: double coefficients[4]
206  register double r, scale;
207 
208  r = y_dest / params.var4;
209 
210  if( r < 0.0 ) r = -r;
211 
212  scale = ((params.var3 * r + params.var2) * r + params.var1) * r + params.var0;
213 
214  *x_src = x_dest;
215  *y_src = y_dest * scale ;
216 }
217 */
218 
219 /*
220 //
221 static void deregister( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
222 {
223  // params: double coefficients[4]
224  register double r, scale;
225 
226  r = y_dest / params.var4;
227 
228  if( r < 0.0 ) r = -r;
229 
230  scale = (params.var3 * r + params.var2) * r + params.var1 ;
231 
232  *x_src = x_dest + abs( y_dest ) * scale;
233  *y_src = y_dest ;
234 }
235 */
236 // perspective
237 void persp_sphere( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
238 {
239  // params : double Matrix[3][3], double params.distance
240  double theta,s,r;
241  Vector3 v, v2;
242 
243  r = sqrt( x_dest * x_dest + y_dest * y_dest );
244  theta = r / params.distance;
245  if( r == 0.0 )
246  s = 0.0;
247  else
248  s = sin( theta ) / r;
249 
250  v.x = s * x_dest ;
251  v.y = s * y_dest ;
252  v.z = cos( theta );
253 
254  v2 = params.mt.TransformVector( v );
255 
256  r = sqrt( v2.x*v2.x + v2.y*v2.y );
257  if( r == 0.0 )
258  theta = 0.0;
259  else
260  theta = params.distance * atan2( r, v2.z ) / r;
261  *x_src = theta * v2.x;
262  *y_src = theta * v2.y;
263 }
264 
265 
266 // perspective rect
267 void persp_rect( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
268 {
269  // params : double Matrix[3][3], double params.distance, double x-offset, double y-offset
270  Vector3 v;
271  v.x = x_dest + params.var2;
272  v.y = y_dest + params.var3;
273  v.z = params.var1;
274  v = params.mt.TransformVector( v );
275  *x_src = v.x * params.var1 / v.z;
276  *y_src = v.y * params.var1 / v.z;
277 }
278 
279 /*
280 //
281 static void rect_pano( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
282 {
283  *x_src = params.distance * tan( x_dest / params.distance ) ;
284  *y_src = y_dest / cos( x_dest / params.distance );
285 }
286 */
287 
288 /*
289 //
290 static void pano_rect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
291 {
292  *x_src = params.distance * atan ( x_dest / params.distance );
293  *y_src = y_dest * cos( *x_src / params.distance );
294 }
295 */
296 
297 //
298 void rect_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
299 {
300  // params: double params.distance
301  double phi, theta;
302 
303  phi = x_dest / params.distance;
304  theta = - y_dest / params.distance + PI / 2.0;
305  if(theta < 0)
306  {
307  theta = - theta;
308  phi += PI;
309  }
310  if(theta > PI)
311  {
312  theta = PI - (theta - PI);
313  phi += PI;
314  }
315 
316  *x_src = params.distance * tan(phi);
317  *y_src = params.distance / (tan( theta ) * cos(phi));
318 }
319 
320 //
321 void pano_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
322 {
323  // params: double params.distance
324  *x_src = x_dest;
325  *y_src = params.distance * tan( y_dest / params.distance);
326 }
327 
328 //
329 void erect_pano( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
330 {
331  // params: double params.distance
332  *x_src = x_dest;
333  *y_src = params.distance * atan( y_dest / params.distance);
334 }
335 
336 // FIXME: implement!
337 void transpano_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
338 {
339  // params: double params.distance
340  *x_src = x_dest;
341  *y_src = params.distance * tan( y_dest / params.distance);
342 }
343 
344 // FIXME: implement!
345 void erect_transpano( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
346 {
347  // params: double params.distance
348  *x_src = x_dest;
349  *y_src = params.distance * atan( y_dest / params.distance);
350 }
351 
352 /*
353 //
354 static void sphere_cp_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
355 {
356  // params: double params.distance, double b
357  register double phi, theta;
358  phi = - x_dest / ( params.var0 * PI / 2.0);
359  theta = - ( y_dest + params.var1 ) / ( PI / 2.0) ;
360  *x_src = theta * cos( phi );
361  *y_src = theta * sin( phi );
362 }
363 */
364 
365 //
366 void sphere_tp_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
367 {
368  // params: double params.distance
369  double phi, theta, r,s;
370  double v[3];
371  phi = x_dest / params.distance;
372  theta = - y_dest / params.distance + PI / 2;
373  if(theta < 0)
374  {
375  theta = - theta;
376  phi += PI;
377  }
378  if(theta > PI)
379  {
380  theta = PI - (theta - PI);
381  phi += PI;
382  }
383  s = sin( theta );
384  v[0] = s * sin( phi ); // y' -> x
385  v[1] = cos( theta ); // z' -> y
386  r = sqrt( v[1]*v[1] + v[0]*v[0]);
387  theta = params.distance * atan2( r , s * cos( phi ) );
388 
389  *x_src = theta * v[0] / r;
390  *y_src = theta * v[1] / r;
391 }
392 
393 /*
394 //
395 static void erect_sphere_cp( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
396 {
397  // params: double params.distance, double b
398  register double phi, theta;
399  theta = sqrt( x_dest * x_dest + y_dest * y_dest ) ;
400  phi = atan2( y_dest , -x_dest );
401  *x_src = params.var0 * phi;
402  *y_src = theta - params.var1;
403 }
404 */
405 
406 //
407 void rect_sphere_tp( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
408 {
409  // params: double params.distance
410  double rho, theta,r;
411  r = sqrt( x_dest*x_dest + y_dest*y_dest );
412  theta = r / params.distance;
413 
414  if( theta >= PI /2.0 )
415  rho = 1.6e16 ;
416  else if( theta == 0.0 )
417  rho = 1.0;
418  else
419  rho = tan( theta ) / theta;
420  *x_src = rho * x_dest ;
421  *y_src = rho * y_dest ;
422 }
423 
424 //
425 void sphere_tp_rect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
426 {
427  // params: double params.distance
428  double theta, r;
429  r = sqrt(x_dest*x_dest + y_dest*y_dest) / params.distance;
430  if( r== 0.0 )
431  theta = 1.0;
432  else
433  theta = atan( r ) / r;
434  *x_src = theta * x_dest ;
435  *y_src = theta * y_dest ;
436 }
437 
438 //
439 void sphere_tp_pano( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
440 {
441  // params: double params.distance
442  double r, s, Phi, theta;
443  Phi = x_dest / params.distance;
444  s = params.distance * sin( Phi ) ; // y' -> x
445  r = sqrt( s*s + y_dest*y_dest );
446  theta = params.distance * atan2( r , (params.distance * cos( Phi )) ) / r;
447  *x_src = theta * s ;
448  *y_src = theta * y_dest ;
449 }
450 
451 //
452 void pano_sphere_tp( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
453 {
454  // params: double params.distance
455  double r,s, theta;
456  double v[3];
457  r = sqrt( x_dest * x_dest + y_dest * y_dest );
458  theta = r / params.distance;
459  if( theta == 0.0 )
460  s = 1.0 / params.distance;
461  else
462  s = sin( theta ) /r;
463  v[1] = s * x_dest ; // x' -> y
464  v[0] = cos( theta ); // z' -> x
465  *x_src = params.distance * atan2( v[1], v[0] );
466  *y_src = params.distance * s * y_dest / sqrt( v[0]*v[0] + v[1]*v[1] );
467 }
468 
469 /*
470 //
471 static void sphere_cp_pano( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
472 {
473  // params: double params.distance
474  register double phi, theta;
475  phi = -x_dest / (params.distance * PI / 2.0) ;
476  theta = PI /2.0 + atan( y_dest / (params.distance * PI/2.0) );
477  *x_src = params.distance * theta * cos( phi );
478  *y_src = params.distance * theta * sin( phi );
479 }
480 */
481 
482 //
483 void erect_rect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
484 {
485  // params: double params.distance
486  *x_src = params.distance * atan2( x_dest, params.distance );
487  *y_src = params.distance * atan2( y_dest, sqrt( params.distance*params.distance + x_dest*x_dest ) );
488 }
489 
490 //
491 void erect_sphere_tp( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
492 {
493  // params: double params.distance
494  double theta,r,s;
495  double v[3];
496  r = sqrt( x_dest * x_dest + y_dest * y_dest );
497  theta = r / params.distance;
498  if(theta == 0.0)
499  s = 1.0 / params.distance;
500  else
501  s = sin( theta) / r;
502 
503  v[1] = s * x_dest;
504  v[0] = cos( theta );
505 
506  *x_src = params.distance * atan2( v[1], v[0] );
507  *y_src = params.distance * atan( s * y_dest /sqrt( v[0]*v[0] + v[1]*v[1] ) );
508 }
509 
511 void mercator_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
512 {
513  // params: distance
514  *x_src = x_dest;
515  *y_src = params.distance*log(tan(y_dest/params.distance)+1/cos(y_dest/params.distance));
516 }
517 
519 void erect_mercator( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
520 {
521  // params: distance
522  *x_src = x_dest;
523  *y_src = params.distance*atan(sinh(y_dest/params.distance));
524 }
525 
526 
528 void transmercator_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
529 {
530  // params: distance
531  x_dest /= params.distance;
532  y_dest /= params.distance;
533  double B = cos(y_dest)*sin(x_dest);
534  *x_src = params.distance / tanh(B);
535  *y_src = params.distance * atan(tan(y_dest)/cos(x_dest));
536 }
537 
539 void erect_transmercator( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
540 {
541  // params: distance
542  x_dest /= params.distance;
543  y_dest /= params.distance;
544  *x_src = params.distance * atan(sinh(x_dest)/cos(y_dest));
545  *y_src = params.distance * asin(sin(y_dest)/cosh(x_dest));
546 }
547 
549 void sinusoidal_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
550 {
551  // params: distance
552 
553  *x_src = params.distance * (x_dest/params.distance*cos(y_dest/params.distance));
554  *y_src = y_dest;
555 }
556 
558 void erect_sinusoidal( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
559 {
560  // params: distance
561 
562  *y_src = y_dest;
563  *x_src = x_dest/cos(y_dest/params.distance);
564 }
565 
567 void stereographic_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
568 {
569  // params: distance
570  double lon = x_dest / params.distance;
571  double lat = y_dest / params.distance;
572 
573  // use: R = 1
574  double k=2.0/(1+cos(lat)*cos(lon));
575  *x_src = params.distance * k*cos(lat)*sin(lon);
576  *y_src = params.distance * k*sin(lat);
577 }
578 
580 void erect_stereographic( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
581 {
582  // params: distance
583 
584  // use: R = 1
585  double p=sqrt(x_dest*x_dest + y_dest*y_dest) / params.distance;
586  double c= 2.0*atan(p/2.0);
587 
588  *x_src = params.distance * atan2(x_dest/params.distance*sin(c),(p*cos(c)));
589  *y_src = params.distance * asin(y_dest/params.distance*sin(c)/p);
590 }
591 
592 
593 /*
594 //
595 static void mirror_sphere_cp( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
596 {
597  // params: double params.distance, double b
598  register double rho, phi, theta;
599  theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / params.var0;
600  phi = atan2( y_dest , x_dest );
601  rho = params.var1 * sin( theta / 2.0 );
602  *x_src = - rho * cos( phi );
603  *y_src = rho * sin( phi );
604 }
605 */
606 
607 /*
608 //
609 static void mirror_erect( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
610 {
611  // params: double params.distance, double b, double b2
612  register double phi, theta, rho;
613  phi = x_dest / ( params.var0 * PI/2.0) ;
614  theta = - ( y_dest + params.var2 ) / (params.var0 * PI/2.0) ;
615  rho = params.var1 * sin( theta / 2.0 );
616  *x_src = - rho * cos( phi );
617  *y_src = rho * sin( phi );
618 }
619 */
620 
621 /*
622 //
623 static void mirror_pano( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
624 {
625  // params: double params.distance, double b
626  register double phi, theta, rho;
627  phi = -x_dest / (params.var0 * PI/2.0) ;
628  theta = PI /2.0 + atan( y_dest / (params.var0 * PI/2.0) );
629  rho = params.var1 * sin( theta / 2.0 );
630  *x_src = rho * cos( phi );
631  *y_src = rho * sin( phi );
632 }
633 */
634 
635 /*
636 //
637 static void sphere_cp_mirror( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
638 {
639  // params: double params.distance, double b
640  register double phi, theta, rho;
641  rho = sqrt( x_dest*x_dest + y_dest*y_dest );
642  theta = 2 * asin( rho/params.var1 );
643  phi = atan2( y_dest , x_dest );
644  *x_src = params.var0 * theta * cos( phi );
645  *y_src = params.var0 * theta * sin( phi );
646 }
647 */
648 
649 /*
650 //
651 static void shift_scale_rotate( double x_dest,double y_dest, double* x_src, double* y_src, const _FuncParams & params)
652 {
653  // params: double params.shift_x, params.shift_y, scale, cos_phi, sin_phi
654  register double x = x_dest - params.var0;
655  register double y = y_dest - params.var1;
656  *x_src = (x * params.var3 - y * params.var4) * params.var2;
657  *y_src = (x * params.var4 + y * params.var3) * params.var2;
658 }
659 */
660 
661 /*
662 
663 // Correct radial luminance change using parabel
664 unsigned char radlum( unsigned char srcPixel, int xc, int yc, void *params )
665 {
666  // params: second and zero order polynomial coeff
667  register double result;
668 
669  result = (xc * xc + yc * yc) * params.var0 + params.var1;
670  result = ((double)srcPixel) - result;
671 
672  if(result < 0.0) return 0;
673  if(result > 255.0) return 255;
674 
675  return( (unsigned char)(result+0.5) );
676 }
677 
678 
679 // Get smallest positive (non-zero) root of polynomial with degree deg and
680 // (n+1) real coefficients p[i]. Return it, or 1000.0 if none exists or error occurred
681 // Changed to only allow degree 3
682 #if 0
683 double smallestRoot( double *p )
684 {
685  doublecomplex root[3], poly[4];
686  doublereal radius[3], apoly[4], apolyr[4];
687  logical myErr[3];
688  double sRoot = 1000.0;
689  doublereal theEps, theBig, theSmall;
690  integer nitmax;
691  integer iter;
692  integer n,i;
693 
694  n = 3;
695 
696 
697  for( i=0; i< n+1; i++)
698  {
699  poly[i].r = p[i];
700  poly[i].i = 0.0;
701  }
702 
703  theEps = DBL_EPSILON; // machine precision
704  theSmall = DBL_MIN ; // smallest positive real*8
705  theBig = DBL_MAX ; // largest real*8
706 
707  nitmax = 100;
708 
709  polzeros_(&n, poly, &theEps, &theBig, &theSmall, &nitmax, root, radius, myErr, &iter, apoly, apolyr);
710 
711  for( i = 0; i < n; i++ )
712  {
713 // PrintError("No %d : Real %g, Imag %g, radius %g, myErr %ld", i, root[i].r, root[i].i, radius[i], myErr[i]);
714  if( (root[i].r > 0.0) && (dabs( root[i].i ) <= radius[i]) && (root[i].r < sRoot) )
715  sRoot = root[i].r;
716  }
717 
718  return sRoot;
719 }
720 #endif
721 
722 void cubeZero( double *a, int *n, double *root );
723 void squareZero( double *a, int *n, double *root );
724 double cubeRoot( double x );
725 
726 
727 void cubeZero( double *a, int *n, double *root ){
728  if( a[3] == 0.0 ){ // second order polynomial
729  squareZero( a, n, root );
730  }else{
731  double p = ((-1.0/3.0) * (a[2]/a[3]) * (a[2]/a[3]) + a[1]/a[3]) / 3.0;
732  double q = ((2.0/27.0) * (a[2]/a[3]) * (a[2]/a[3]) * (a[2]/a[3]) - (1.0/3.0) * (a[2]/a[3]) * (a[1]/a[3]) + a[0]/a[3]) / 2.0;
733 
734  if( q*q + p*p*p >= 0.0 ){
735  *n = 1;
736  root[0] = cubeRoot(-q + sqrt(q*q + p*p*p)) + cubeRoot(-q - sqrt(q*q + p*p*p)) - a[2] / (3.0 * a[3]);
737  }else{
738  double phi = acos( -q / sqrt(-p*p*p) );
739  *n = 3;
740  root[0] = 2.0 * sqrt(-p) * cos(phi/3.0) - a[2] / (3.0 * a[3]);
741  root[1] = -2.0 * sqrt(-p) * cos(phi/3.0 + PI/3.0) - a[2] / (3.0 * a[3]);
742  root[2] = -2.0 * sqrt(-p) * cos(phi/3.0 - PI/3.0) - a[2] / (3.0 * a[3]);
743  }
744  }
745  // PrintError("%lg, %lg, %lg, %lg root = %lg", a[3], a[2], a[1], a[0], root[0]);
746 }
747 
748 void squareZero( double *a, int *n, double *root ){
749  if( a[2] == 0.0 ){ // linear equation
750  if( a[1] == 0.0 ){ // constant
751  if( a[0] == 0.0 ){
752  *n = 1; root[0] = 0.0;
753  }else{
754  *n = 0;
755  }
756  }else{
757  *n = 1; root[0] = - a[0] / a[1];
758  }
759  }else{
760  if( 4.0 * a[2] * a[0] > a[1] * a[1] ){
761  *n = 0;
762  }else{
763  *n = 2;
764  root[0] = (- a[1] + sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
765  root[1] = (- a[1] - sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
766  }
767  }
768 
769 }
770 
771 double cubeRoot( double x ){
772  if( x == 0.0 )
773  return 0.0;
774  else if( x > 0.0 )
775  return pow(x, 1.0/3.0);
776  else
777  return - pow(-x, 1.0/3.0);
778 }
779 
780 double smallestRoot( double *p ){
781  int n,i;
782  double root[3], sroot = 1000.0;
783 
784  cubeZero( p, &n, root );
785 
786  for( i=0; i<n; i++){
787  // PrintError("Root %d = %lg", i,root[i]);
788  if(root[i] > 0.0 && root[i] < sroot)
789  sroot = root[i];
790  }
791 
792  // PrintError("Smallest Root = %lg", sroot);
793  return sroot;
794 }
795 
796 */
797 
798 
799 
800 // really strange. the pano12.dll for windows doesn't seem to
801 // contain the SetCorrectionRadius function, so it is included here
802 
803 static void cubeZero_copy( double *a, int *n, double *root );
804 static void squareZero_copy( double *a, int *n, double *root );
805 static double cubeRoot_copy( double x );
806 
807 
808 static void cubeZero_copy( double *a, int *n, double *root ){
809  if( a[3] == 0.0 ){ // second order polynomial
810  squareZero_copy( a, n, root );
811  }else{
812  double p = ((-1.0/3.0) * (a[2]/a[3]) * (a[2]/a[3]) + a[1]/a[3]) / 3.0;
813  double q = ((2.0/27.0) * (a[2]/a[3]) * (a[2]/a[3]) * (a[2]/a[3]) - (1.0/3.0) * (a[2]/a[3]) * (a[1]/a[3]) + a[0]/a[3]) / 2.0;
814 
815  if( q*q + p*p*p >= 0.0 ){
816  *n = 1;
817  root[0] = cubeRoot_copy(-q + sqrt(q*q + p*p*p)) + cubeRoot_copy(-q - sqrt(q*q + p*p*p)) - a[2] / (3.0 * a[3]);
818  }else{
819  double phi = acos( -q / sqrt(-p*p*p) );
820  *n = 3;
821  root[0] = 2.0 * sqrt(-p) * cos(phi/3.0) - a[2] / (3.0 * a[3]);
822  root[1] = -2.0 * sqrt(-p) * cos(phi/3.0 + PI/3.0) - a[2] / (3.0 * a[3]);
823  root[2] = -2.0 * sqrt(-p) * cos(phi/3.0 - PI/3.0) - a[2] / (3.0 * a[3]);
824  }
825  }
826  // PrintError("%lg, %lg, %lg, %lg root = %lg", a[3], a[2], a[1], a[0], root[0]);
827 }
828 
829 static void squareZero_copy( double *a, int *n, double *root ){
830  if( a[2] == 0.0 ){ // linear equation
831  if( a[1] == 0.0 ){ // constant
832  if( a[0] == 0.0 ){
833  *n = 1; root[0] = 0.0;
834  }else{
835  *n = 0;
836  }
837  }else{
838  *n = 1; root[0] = - a[0] / a[1];
839  }
840  }else{
841  if( 4.0 * a[2] * a[0] > a[1] * a[1] ){
842  *n = 0;
843  }else{
844  *n = 2;
845  root[0] = (- a[1] + sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
846  root[1] = (- a[1] - sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
847  }
848  }
849 
850 }
851 
852 static double cubeRoot_copy( double x ){
853  if( x == 0.0 )
854  return 0.0;
855  else if( x > 0.0 )
856  return pow(x, 1.0/3.0);
857  else
858  return - pow(-x, 1.0/3.0);
859 }
860 
861 static double smallestRoot_copy( double *p ){
862  int n,i;
863  double root[3], sroot = 1000.0;
864 
865  cubeZero_copy( p, &n, root );
866 
867  for( i=0; i<n; i++){
868  // PrintError("Root %d = %lg", i,root[i]);
869  if(root[i] > 0.0 && root[i] < sroot)
870  sroot = root[i];
871  }
872 
873  // PrintError("Smallest Root = %lg", sroot);
874  return sroot;
875 }
876 
877 
878 // Restrict radial correction to monotonous interval
879 static double CalcCorrectionRadius_copy(double *coeff )
880 {
881  double a[4];
882  int k;
883 
884  for( k=0; k<4; k++ )
885  {
886  a[k] = 0.0;//1.0e-10;
887  if( coeff[k] != 0.0 )
888  {
889  a[k] = (k+1) * coeff[k];
890  }
891  }
892  return smallestRoot_copy( a );
893 }
894 
895 // radial and shift
896 static void radial_shift( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
897 {
898  // params: double coefficients[4], scale, correction_radius, shift_x, shift_y
899  double r, scale;
900 
901  r = (sqrt( x_dest*x_dest + y_dest*y_dest )) / params.var4;
902  if( r < params.var5 )
903  {
904  scale = ((params.var3 * r + params.var2) * r + params.var1) * r + params.var0;
905  }
906  else
907  scale = 1000.0;
908 
909  *x_src = x_dest * scale + params.var6;
910  *y_src = y_dest * scale + params.var7;
911 }
912 
913 
914 //==============================================================================
915 
916 
917 Matrix3 SetMatrix( double a, double b, double c, int cl )
918 {
919  Matrix3 mx, my, mz;
920  // Matrix3 dummy;
921 
922  // Calculate Matrices;
923  mx.SetRotationX( a );
924  my.SetRotationY( b );
925  mz.SetRotationZ( c );
926 
927  if (cl)
928  return ( (mz * mx) * my );
929  else
930  return ( (mx * mz) * my );
931 
932  //if( cl )
933  // matrix_matrix_mult( mz, mx, dummy);
934  //else
935  // matrix_matrix_mult( mx, mz, dummy);
936  //matrix_matrix_mult( dummy, my, m);
937 }
938 
939 
941 {
942  SpaceTransform transf;
943  transf.InitInvRadialCorrect(src, 1);
944  vigra::Rect2D inside;
945  vigra::Rect2D insideTemp;
946  vigra::Rect2D boundingBox;
947  traceImageOutline(src.getSize(), transf, inside, boundingBox);
948  if (src.getCorrectTCA()) {
949  transf.InitInvRadialCorrect(src, 0);
950  traceImageOutline(src.getSize(), transf, insideTemp, boundingBox);
951  inside &= insideTemp;
952  transf.InitInvRadialCorrect(src, 2);
953  traceImageOutline(src.getSize(), transf, insideTemp, boundingBox);
954  inside &= insideTemp;
955  }
956  double width2 = src.getSize().x/2.0;
957  double height2 = src.getSize().y/2.0;
958  double sx = std::max(width2/(width2-inside.left()), width2/(inside.right()-width2));
959  double sy = std::max(height2/(height2-inside.top()), height2/(inside.bottom()-height2));
960  return 1/std::max(sx,sy);
961 }
962 
963 
964 double estRadialScaleCrop(const std::vector<double> &coeff, int width, int height)
965 {
966  double r_test[4];
967  double p, r;
968  int test_points, i;
969  double a, b, c, d;
970 
971  // truncate black edges
972  // algorithm courtesy of Paul Wilkinson, paul.wilkinson@ntlworld.com
973  // modified by dangelo to just calculate the scaling factor -> better results
974 
975  a = coeff[0];
976  b = coeff[1];
977  c = coeff[2];
978  d = coeff[3];
979 
980  if (width > height)
981  p = (double)(width) / (double)(height);
982  else
983  p = (double)(height) / (double)(width);
984 
985  //***************************************************
986  //* Set the test point for the far corner. *
987  //***************************************************
988  r_test[0] = sqrt(1 + p * p);
989  test_points = 1;
990 
991  //***************************************************
992  //* For non-zero values of a, there are two other *
993  //* possible test points. (local extrema) *
994  //***************************************************
995  //
996 
997  if (a != 0.0)
998  {
999  r = (-b + sqrt(b * b - 3 * a * c)) / (3 * a);
1000  if (r >= 1 && r <= r_test[0])
1001  {
1002  r_test[test_points] = r;
1003  test_points = test_points + 1;
1004  }
1005  r = (-b - sqrt(b * b - 3 * a * c)) / (3 * a);
1006  if (r >= 1 && r <= r_test[0])
1007  {
1008  r_test[test_points] = r;
1009  test_points = test_points + 1;
1010  }
1011  }
1012 
1013  //***************************************************
1014  //* For zero a and non-zero b, there is one other *
1015  //* possible test point. *
1016  //***************************************************
1017  if (a == 0.0 && b != 0.0)
1018  {
1019  r = -c / (2 * b);
1020  if (r >= 1 && r <= r_test[0])
1021  {
1022  r_test[test_points] = r;
1023  test_points = test_points + 1;
1024  }
1025  }
1026 
1027  // check the scaling factor at the test points.
1028  // start with a very high scaling factor.
1029  double scalefactor = 0.1;
1030  for (i = 0; i <= test_points - 1; i++)
1031  {
1032  r = r_test[i];
1033  double scale = d + r * (c + r * (b + r * a));
1034  if ( scalefactor < scale)
1035  scalefactor = scale;
1036  }
1037  return scalefactor;
1038 }
1039 
1040 
1041 
1042 //==============================================================================
1043 
1044 
1046 void SpaceTransform::InitRadialCorrect(const vigra::Size2D & sz, const std::vector<double> & radDist,
1047  const hugin_utils::FDiff2D & centerShift)
1048 {
1049  double mprad[6];
1050 
1051 // double imwidth = src.getSize().x;
1052 // double imheight= src.getSize().y;
1053 
1054  m_Stack.clear();
1055  m_srcTX = sz.x/2.0;
1056  m_srcTY = sz.y/2.0;
1057  m_destTX = sz.x/2.0;
1058  m_destTY = sz.y/2.0;
1059 
1060  // green channel, always correct
1061  for (int i=0; i < 4; i++) {
1062  mprad[3-i] = radDist[i];
1063  }
1064  mprad[4] = ( sz.x < sz.y ? sz.x: sz.y) / 2.0;
1065  // calculate the correction radius.
1066  mprad[5] = CalcCorrectionRadius_copy(mprad);
1067 
1068  // radial correction if nonzero radial coefficients
1069  if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
1070  AddTransform (&radial_shift, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5],
1071  centerShift.x, centerShift.y);
1072  }
1073 }
1074 
1077 {
1078  double mprad[6];
1079 
1080 // double imwidth = src.getSize().x;
1081 // double imheight= src.getSize().y;
1082 
1083  m_Stack.clear();
1084  m_srcTX = src.getSize().x/2.0;
1085  m_srcTY = src.getSize().y/2.0;
1086  m_destTX = src.getSize().x/2.0;
1087  m_destTY = src.getSize().y/2.0;
1088 
1089  if (src.getRadialDistortionCenterShift().x != 0.0) {
1090  AddTransform(&horiz, -src.getRadialDistortionCenterShift().x);
1091  }
1092 
1093  // shift optical center if needed
1094  if (src.getRadialDistortionCenterShift().y != 0.0) {
1095  AddTransform(&vert, -src.getRadialDistortionCenterShift().y);
1096  }
1097 
1098  if (src.getCorrectTCA() && (channel == 0 || channel == 2)) {
1099  for (int i=0; i < 4; i++) {
1100  if (channel == 0) {
1101  // correct red channel (TCA)
1102  mprad[3-i] = src.getRadialDistortionRed()[i];
1103  } else {
1104  // correct blue channel (TCA)
1105  mprad[3-i] = src.getRadialDistortionBlue()[i];
1106  }
1107  }
1108  mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y) / 2.0;
1109  // calculate the correction radius.
1110  mprad[5] = CalcCorrectionRadius_copy(mprad);
1111 
1112  // radial correction if nonzero radial coefficients
1113  if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
1114  AddTransform (&inv_radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
1115  }
1116  }
1117 
1118  // green channel, always correct
1119  for (int i=0; i < 4; i++) {
1120  mprad[3-i] = src.getRadialDistortion()[i];
1121  }
1122  mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y) / 2.0;
1123  // calculate the correction radius.
1124  mprad[5] = CalcCorrectionRadius_copy(mprad);
1125 
1126  // radial correction if nonzero radial coefficients
1127  if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
1128  AddTransform (&inv_radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
1129  }
1130 }
1131 
1132 
1134 void SpaceTransform::InitRadialCorrect(const SrcPanoImage & src, int channel)
1135 {
1136  double mprad[6];
1137 
1138 // double imwidth = src.getSize().x;
1139 // double imheight= src.getSize().y;
1140 
1141  m_Stack.clear();
1142  m_srcTX = src.getSize().x/2.0;
1143  m_srcTY = src.getSize().y/2.0;
1144  m_destTX = src.getSize().x/2.0;
1145  m_destTY = src.getSize().y/2.0;
1146 
1147  // green channel, always correct
1148  for (int i=0; i < 4; i++) {
1149  mprad[3-i] = src.getRadialDistortion()[i];
1150  }
1151  mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y) / 2.0;
1152  // calculate the correction radius.
1153  mprad[5] = CalcCorrectionRadius_copy(mprad);
1154 
1155  // radial correction if nonzero radial coefficients
1156  if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
1157  AddTransform (&radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
1158  DEBUG_DEBUG("Init Radial (green): "
1159  << "g: " << mprad[0] << " " << mprad[1] << " " << mprad[2]
1160  << " " << mprad[3] << " " << mprad[4] << " " << mprad[5]);
1161  }
1162 
1163  if (src.getCorrectTCA() && (channel == 0 || channel == 2)) {
1164  for (int i=0; i < 4; i++) {
1165  if (channel == 0) {
1166  // correct red channel (TCA)
1167  mprad[3-i] = src.getRadialDistortionRed()[i];
1168  } else {
1169  // correct blue channel (TCA)
1170  mprad[3-i] = src.getRadialDistortionBlue()[i];
1171  }
1172  }
1173  mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y) / 2.0;
1174  // calculate the correction radius.
1175  mprad[5] = CalcCorrectionRadius_copy(mprad);
1176 
1177  // radial correction if nonzero radial coefficients
1178  if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
1179  AddTransform (&radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
1180  DEBUG_DEBUG("Init Radial (channel " << channel << "): "
1181  << "g: " << mprad[0] << " " << mprad[1] << " " << mprad[2]
1182  << " " << mprad[3] << " " << mprad[4] << " " << mprad[5]);
1183  }
1184  }
1185 
1186  // shift optical center if needed
1187  if (src.getRadialDistortionCenterShift().y != 0.0) {
1188  AddTransform(&vert, src.getRadialDistortionCenterShift().y);
1189  }
1190  if (src.getRadialDistortionCenterShift().x != 0.0) {
1191  AddTransform(&horiz, src.getRadialDistortionCenterShift().x);
1192  }
1193 }
1194 
1199  const SrcPanoImage & image,
1200  const vigra::Diff2D &destSize,
1202  double destHFOV )
1203 {
1204  //int i;
1205  double a, b;
1206  Matrix3 mpmt;
1207  double mpdistance, mpscale[2], mprot[2], mprad[6];
1208  // double mpshear[2], mpperspect[2];
1209  double mphorizontal, mpvertical;
1210 
1211  double imhfov = image.getHFOV();
1212  vigra::Size2D srcSize = image.getSize();
1213  double imwidth = srcSize.x;
1214  double imheight= srcSize.y;
1215  double imyaw = image.getYaw();
1216  double impitch = image.getPitch();
1217  double imroll = image.getRoll();
1218  double ima = image.getRadialDistortion()[0];
1219  double imb = image.getRadialDistortion()[1];
1220  double imc = image.getRadialDistortion()[2];
1221  double imd = image.getRadialDistortionCenterShift().x;
1222  double ime = image.getRadialDistortionCenterShift().y;
1223  double img = image.getShear().x;
1224  double imt = image.getShear().y;
1225  double pnhfov = destHFOV;
1226  double pnwidth = destSize.x;
1227  SrcPanoImage::Projection srcProj = image.getProjection();
1228 // double pnheight= destSize.y;
1229 
1230  m_Stack.clear();
1231  m_srcTX = destSize.x/2.0;
1232  m_srcTY = destSize.y/2.0;
1233  m_destTX = srcSize.x/2.0;
1234  m_destTY = srcSize.y/2.0;
1235 
1236  a = DEG_TO_RAD( imhfov ); // field of view in rad
1237  b = DEG_TO_RAD( pnhfov );
1238 
1239  mpmt = SetMatrix( -DEG_TO_RAD( impitch ),
1240  0.0,
1241  - DEG_TO_RAD( imroll ),
1242  0 );
1243 
1244  if (destProj == PanoramaOptions::RECTILINEAR) // rectilinear panorama
1245  {
1246  mpdistance = pnwidth / (2.0 * tan(b/2.0));
1247  if (srcProj == SrcPanoImage::RECTILINEAR) // rectilinear image
1248  {
1249  mpscale[0] = (pnhfov / imhfov) * (a /(2.0 * tan(a/2.0))) * (imwidth/pnwidth) * 2.0 * tan(b/2.0) / b;
1250 
1251  }
1252  else // pamoramic or fisheye image
1253  {
1254  mpscale[0] = (pnhfov / imhfov) * (imwidth/pnwidth) * 2.0 * tan(b/2.0) / b;
1255  }
1256  }
1257  else // equirectangular or panoramic or fisheye or other.
1258  {
1259  mpdistance = pnwidth / b;
1260  if(srcProj == SrcPanoImage::RECTILINEAR) // rectilinear image
1261  {
1262  mpscale[0] = (pnhfov / imhfov) * (a /(2.0 * tan(a/2.0)))*( imwidth / pnwidth );
1263  }
1264  else // pamoramic or fisheye image
1265  {
1266  mpscale[0] = (pnhfov / imhfov) * ( imwidth / pnwidth );
1267  }
1268  }
1269  mpscale[1] = mpscale[0];
1270  // mpshear[0] = img / imheight; // TODO : im->cP.shear_x / imheight;
1271  // mpshear[1] = imt / imwidth; // TODO : im->cP.shear_y / imwidth;
1272  mprot[0] = mpdistance * PI; // 180 in screenpoints
1273  mprot[1] = -imyaw * mpdistance * PI / 180.0; // rotation angle in screenpoints
1274 
1275  // add radial correction
1276  mprad[3] = ima;
1277  mprad[2] = imb;
1278  mprad[1] = imc;
1279  mprad[0] = 1.0 - (ima+imb+imc);
1280  mprad[4] = ( imwidth < imheight ? imwidth : imheight) / 2.0;
1281  // calculate the correction radius.
1282  mprad[5] = CalcCorrectionRadius_copy(mprad);
1283 
1284  /*for(i=0; i<4; i++)
1285  mprad[i] = im->cP.radial_params[color][i];
1286  mprad[5] = im->cP.radial_params[color][4];
1287 
1288  if( (im->cP.correction_mode & 3) == correction_mode_radial )
1289  mp->rad[4] = ( (double)( im->width < im->height ? im->width : im->height) ) / 2.0;
1290  else
1291  mp->rad[4] = ((double) im->height) / 2.0;*/
1292 
1293  mphorizontal = imd;
1294  mpvertical = ime;
1295  //mp->horizontal = im->cP.horizontal_params[color];
1296  //mp->vertical = im->cP.vertical_params[color];
1297 
1298  //i = 0;
1299 
1300  switch (destProj)
1301  {
1303  // Convert rectilinear to equirect
1304  AddTransform( &erect_rect, mpdistance );
1305  break;
1306 
1308  // Convert panoramic to equirect
1309  AddTransform( &erect_pano, mpdistance );
1310  break;
1312  // do nothing... coordinates are already equirect
1313  break;
1314  //case PanoramaOptions::FULL_FRAME_FISHEYE:
1316  // Convert panoramic to sphere
1317  AddTransform( &erect_sphere_tp, mpdistance );
1318  break;
1320  AddTransform( &erect_stereographic, mpdistance );
1321  break;
1323  AddTransform( &erect_mercator, mpdistance );
1324  break;
1326  AddTransform( &erect_transmercator, mpdistance );
1327  break;
1328 // case PanoramaOptions::TRANSVERSE_CYLINDRICAL:
1329 // AddTransform( &erect_transpano, mpdistance );
1330 // break;
1332  AddTransform( &erect_sinusoidal, mpdistance );
1333  break;
1334  default:
1335  DEBUG_FATAL("Fatal error: Unknown projection " << destProj);
1336  break;
1337  }
1338 
1339  // Rotate equirect. image horizontally
1340  AddTransform( &rotate_erect, mprot[0], mprot[1] );
1341 
1342  // Convert spherical image to equirect.
1343  AddTransform( &sphere_tp_erect, mpdistance );
1344 
1345  // Perspective Control spherical Image
1346  AddTransform( &persp_sphere, mpmt, mpdistance );
1347 
1348  switch (srcProj)
1349  {
1351  // Convert rectilinear to spherical
1352  AddTransform( &rect_sphere_tp, mpdistance);
1353  break;
1355  // Convert panoramic to spherical
1356  AddTransform( &pano_sphere_tp, mpdistance );
1357  break;
1359  // Convert PSphere to spherical
1360  AddTransform( &erect_sphere_tp, mpdistance );
1361  break;
1362  default:
1363  // do nothing for CIRCULAR & FULL_FRAME_FISHEYE
1364  break;
1365  }
1366 
1367  // Scale Image
1368  AddTransform( &resize, mpscale[0], mpscale[1] );
1369 
1370  // radial correction if nonzero radial coefficients
1371  if ( mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
1372  AddTransform (&radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
1373  }
1374 
1375  // shift optical center if needed
1376  if (mpvertical != 0.0) {
1377  AddTransform(&vert, mpvertical);
1378  }
1379  if (mphorizontal != 0.0) {
1380  AddTransform(&horiz, mphorizontal);
1381  }
1382 
1383  /*if( im->cP.radial )
1384  {
1385  switch( im->cP.correction_mode & 3 )
1386  {
1387  case correction_mode_radial: SetDesc(stack[i],radial,mp->rad); i++; break;
1388  case correction_mode_vertical: SetDesc(stack[i],vertical,mp->rad); i++; break;
1389  case correction_mode_deregister:SetDesc(stack[i],deregister,mp->rad); i++; break;
1390  }
1391  }
1392  if ( im->cP.vertical)
1393  {
1394  SetDesc(stack[i],vert, &(mp->vertical)); i++;
1395  }
1396  if ( im->cP.horizontal )
1397  {
1398  SetDesc(stack[i],horiz, &(mp->horizontal)); i++;
1399  }
1400  if( im->cP.shear )
1401  {
1402  SetDesc( stack[i],shear, mp->shear ); i++;
1403  }
1404 
1405  stack[i].func = (trfn)NULL;*/
1406 
1407 }
1408 
1410  const SrcPanoImage & image,
1411  const vigra::Diff2D &destSize,
1413  double destHFOV )
1414 {
1415  double a, b;
1416  Matrix3 mpmt;
1417  double mpdistance, mpscale[2], mprot[2], mprad[6];
1418 // double mpshear[2], mpperspect[2];
1419  double mphorizontal, mpvertical;
1420 
1421  double imhfov = image.getHFOV();
1422  vigra::Size2D srcSize = image.getSize();
1423  double imwidth = srcSize.x;
1424  double imheight= srcSize.y;
1425  double imyaw = image.getYaw();
1426  double impitch = image.getPitch();
1427  double imroll = image.getRoll();
1428  double ima = image.getRadialDistortion()[0];
1429  double imb = image.getRadialDistortion()[1];
1430  double imc = image.getRadialDistortion()[2];
1431  double imd = image.getRadialDistortionCenterShift().x;
1432  double ime = image.getRadialDistortionCenterShift().y;
1433  SrcPanoImage::Projection srcProj = image.getProjection();
1434 
1435  double pnhfov = destHFOV;
1436  double pnwidth = destSize.x;
1437 // double pnheight= destSize.y;
1438 
1439  m_Stack.clear();
1440  m_srcTX = destSize.x/2.0;
1441  m_srcTY = destSize.y/2.0;
1442  m_destTX = srcSize.x/2.0;
1443  m_destTY = srcSize.y/2.0;
1444 
1445 
1446  a = DEG_TO_RAD( imhfov ); // field of view in rad
1447  b = DEG_TO_RAD( pnhfov );
1448 
1449  mpmt = SetMatrix( DEG_TO_RAD( impitch ),
1450  0.0,
1451  DEG_TO_RAD( imroll ),
1452  1 );
1453 
1454  if(destProj == PanoramaOptions::RECTILINEAR) // rectilinear panorama
1455  {
1456  mpdistance = pnwidth / (2.0 * tan(b/2.0));
1457  if(srcProj == SrcPanoImage::RECTILINEAR) // rectilinear image
1458  {
1459  mpscale[0] = ( pnhfov/imhfov ) * (a /(2.0 * tan(a/2.0))) * ( imwidth/pnwidth ) * 2.0 * tan(b/2.0) / b;
1460  }
1461  else // pamoramic or fisheye image
1462  {
1463  mpscale[0] = ( pnhfov/imhfov ) * ( imwidth/pnwidth ) * 2.0 * tan(b/2.0) / b;
1464  }
1465  }
1466  else // equirectangular or panoramic
1467  {
1468  mpdistance = pnwidth / b;
1469  if(srcProj == SrcPanoImage::RECTILINEAR ) // rectilinear image
1470  {
1471  mpscale[0] = ( pnhfov/imhfov ) * (a /(2.0 * tan(a/2.0))) * ( imwidth/pnwidth );
1472  }
1473  else // pamoramic or fisheye image
1474  {
1475  mpscale[0] = ( pnhfov/imhfov ) * ( imwidth/pnwidth );
1476  }
1477  }
1478  // mpshear[0] = 0.0f; // TODO -im->cP.shear_x / im->height;
1479  // mpshear[1] = 0.0f; // -im->cP.shear_y / im->width;
1480 
1481  mpscale[0] = 1.0 / mpscale[0];
1482  mpscale[1] = mpscale[0];
1483 
1484  // principal point shift
1485  mphorizontal = - imd;
1486  mpvertical = - ime;
1487 
1488  // radial correction parameters
1489  mprad[3] = ima;
1490  mprad[2] = imb;
1491  mprad[1] = imc;
1492  mprad[0] = 1.0 - (ima+imb+imc);
1493  mprad[4] = ( imwidth < imheight ? imwidth : imheight) / 2.0;
1494  // calculate the correction radius.
1495  mprad[5] = CalcCorrectionRadius_copy(mprad);
1496 
1497 
1498  /*mp->horizontal = -im->cP.horizontal_params[color];
1499  mp->vertical = -im->cP.vertical_params[color];
1500  for(i=0; i<4; i++)
1501  mp->rad[i] = im->cP.radial_params[color][i];
1502  mp->rad[5] = im->cP.radial_params[color][4];
1503 
1504  switch( im->cP.correction_mode & 3 )
1505  {
1506  case correction_mode_radial: mp->rad[4] = ((double)(im->width < im->height ? im->width : im->height) ) / 2.0;break;
1507  case correction_mode_vertical:
1508  case correction_mode_deregister: mp->rad[4] = ((double) im->height) / 2.0;break;
1509  }
1510  */
1511 
1512  mprot[0] = mpdistance * PI; // 180 in screenpoints
1513  mprot[1] = imyaw * mpdistance * PI / 180.0; // rotation angle in screenpoints
1514 
1515  //mp->perspect[0] = (void*)(mp->mt);
1516  //mp->perspect[1] = (void*)&(mp->distance);
1517 
1518  // Perform radial correction
1519  //if( im->cP.shear )
1520  //{
1521  // SetDesc( stack[i],shear, mp->shear ); i++;
1522  //}
1523  //
1524 
1525  // principal point shift
1526  if (mphorizontal != 0.0) {
1527  AddTransform(&horiz, mphorizontal);
1528  }
1529  if (mpvertical != 0.0) {
1530  AddTransform(&vert, mpvertical);
1531  }
1532 
1533  // radial distortion
1534  if ( mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
1535  AddTransform (&inv_radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
1536  }
1537 
1538  // Scale image
1539  AddTransform( &resize, mpscale[0], mpscale[1] );
1540 
1541  switch (srcProj)
1542  {
1544  // rectilinear image
1545  AddTransform( &sphere_tp_rect, mpdistance );
1546  break;
1548  // Convert panoramic to spherical
1549  AddTransform( &sphere_tp_pano, mpdistance );
1550  break;
1552  // Convert PSphere to spherical
1553  AddTransform( &sphere_tp_erect, mpdistance );
1554  break;
1555  default:
1556  // do nothing for fisheye lenses
1557  break;
1558  }
1559 
1560  // Perspective Control spherical Image
1561  AddTransform( &persp_sphere, mpmt, mpdistance );
1562  // Convert spherical image to equirect.
1563  AddTransform( &erect_sphere_tp, mpdistance );
1564  // Rotate equirect. image horizontally
1565  AddTransform( &rotate_erect, mprot[0], mprot[1] );
1566 
1567  switch (destProj)
1568  {
1570  // Convert rectilinear to spherical
1571  AddTransform( &rect_erect, mpdistance);
1572  break;
1574  // Convert panoramic to spherical
1575  AddTransform( &pano_erect, mpdistance );
1576  break;
1578  break;
1580  // Convert PSphere to spherical
1581  AddTransform( &sphere_tp_erect, mpdistance );
1582  break;
1584  // Convert PSphere to spherical
1585  AddTransform( &stereographic_erect, mpdistance );
1586  break;
1588  AddTransform( &mercator_erect, mpdistance );
1589  break;
1591  AddTransform( &transmercator_erect, mpdistance );
1592  break;
1593 // case PanoramaOptions::TRANSVERSE_CYLINDRICAL:
1594 // AddTransform( &transpano_erect, mpdistance );
1595 // break;
1597  AddTransform( &transpano_erect, mpdistance );
1598  break;
1599  default:
1600  DEBUG_FATAL("Fatal error: Unknown projection " << destProj);
1601  break;
1602  }
1603 }
1604 
1605 //
1607 {
1608  Init(src,
1609  vigra::Size2D(dest.getWidth(), dest.getHeight()),
1610  dest.getProjection(),
1611  dest.getHFOV());
1612 }
1613 
1614 //
1616 {
1617  InitInv(src,
1618  vigra::Size2D(dest.getWidth(), dest.getHeight()),
1619  dest.getProjection(),
1620  dest.getHFOV());
1621 }
1622 
1623 void SpaceTransform::createTransform(const PanoramaData & pano, unsigned int imgNr,
1624  const PanoramaOptions & dest,
1625  vigra::Diff2D srcSize)
1626 {
1627  const SrcPanoImage & img = pano.getImage(imgNr);
1628  if (srcSize.x == 0 && srcSize.y == 0)
1629  {
1630  srcSize = img.getSize();
1631  }
1632  Init(pano.getImage(imgNr),
1633  vigra::Diff2D(dest.getWidth(), dest.getHeight()),
1634  dest.getProjection(), dest.getHFOV() );
1635 }
1636 
1637 
1638 // create image->pano transformation
1639 void SpaceTransform::createInvTransform(const PanoramaData & pano, unsigned int imgNr,
1640  const PanoramaOptions & dest,
1641  vigra::Diff2D srcSize)
1642 {
1643  const SrcPanoImage & img = pano.getImage(imgNr);
1644  if (srcSize.x == 0 && srcSize.y == 0)
1645  {
1646  srcSize = img.getSize();
1647  }
1648  InitInv(pano.getImage(imgNr),
1649  vigra::Diff2D(dest.getWidth(), dest.getHeight()),
1650  dest.getProjection(), dest.getHFOV() );
1651 }
1652 
1654 void SpaceTransform::createTransform(const vigra::Diff2D & srcSize,
1655  const VariableMap & srcVars,
1657  const vigra::Diff2D &destSize,
1659  double destHFOV)
1660 {
1661  SrcPanoImage src_image;
1662  src_image.setSize(vigra::Size2D(srcSize.x, srcSize.y));
1663  src_image.setProjection((SrcPanoImage::Projection)srcProj);
1664  for (VariableMap::const_iterator i = srcVars.begin(); i != srcVars.end(); ++i)
1665  {
1666  src_image.setVar((*i).first, (*i).second.getValue());
1667  }
1668  Init(src_image, destSize, destProj, destHFOV);
1669 }
1670 
1671 // create image->pano transformation
1673 void SpaceTransform::createInvTransform(const vigra::Diff2D & srcSize,
1674  const VariableMap & srcVars,
1676  const vigra::Diff2D & destSize,
1678  double destHFOV)
1679 {
1680  // make a SrcPanoImage with the necessary data.
1681  SrcPanoImage src_image;
1682  src_image.setSize(vigra::Size2D(srcSize.x, srcSize.y));
1683  src_image.setProjection((SrcPanoImage::Projection)srcProj);
1684  for (VariableMap::const_iterator i = srcVars.begin(); i != srcVars.end(); ++i)
1685  {
1686  src_image.setVar((*i).first, (*i).second.getValue());
1687  }
1688  InitInv(src_image, destSize, destProj, destHFOV);
1689 }
1690 
1691 
1692 //
1694 {
1695  double xd = src.x, yd = src.y;
1696  std::vector<fDescription>::const_iterator tI;
1697 
1698  dest.x = xd;
1699  dest.y = yd;
1700  for (tI = m_Stack.begin(); tI != m_Stack.end(); ++tI)
1701  {
1702  (tI->func)( xd, yd, &dest.x, &dest.y, tI->param );
1703  xd = dest.x;
1704  yd = dest.y;
1705  }
1706  return true;
1707 }
1708 
1709 //
1710 bool SpaceTransform::transformImgCoord(double & x_dest, double & y_dest, double x_src, double y_src) const
1711 {
1712  hugin_utils::FDiff2D dest, src;
1713  src.x = x_src - m_srcTX + 0.5;
1714  src.y = y_src - m_srcTY + 0.5;
1715  transform( dest, src );
1716  x_dest = dest.x + m_destTX - 0.5;
1717  y_dest = dest.y + m_destTY - 0.5;
1718  return true;
1719 }
1720 
1721 
1722 } // namespace
1723 } // namespace
PanoramaOptions::ProjectionFormat getProjection() const
void stereographic_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from erect to stereographic
double y
Definition: Vector3.h:47
void persp_rect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void sphere_tp_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
bool transformImgCoord(double &x_dest, double &y_dest, double x_src, double y_src) const
like transform, but return image coordinates, not cartesian coordinates
void sinusoidal_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from erect to sinusoidal
void sphere_tp_pano(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void traceImageOutline(vigra::Size2D sz, TRANSFORM &transf, vigra::Rect2D &inside, vigra::Rect2D &boundingBox)
Internal function to estimate the image scaling required to avoid black stripes at the image borders...
#define var3
void InitInv(const SrcPanoImage &img, const vigra::Diff2D &destSize, PanoramaOptions::ProjectionFormat destProj, double destHFOV)
Init Inv Transform Create the stack of matrices for reverse transform.
#define MAXITER
void radial(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void transmercator_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from erect to transverse mercator
unsigned int getHeight() const
get panorama height
general : Matrix3 is a class for handling 3x3 Matrix manipulation.
Definition: Matrix3.h:37
void erect_rect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
#define var4
void erect_sinusoidal(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from sinusoidal to erect
void Init(const SrcPanoImage &img, const vigra::Diff2D &destSize, PanoramaOptions::ProjectionFormat destProj, double destHFOV)
Init Transform Create the stack of matrices for direct transform.
Vector3 TransformVector(const Vector3 &V) const
transforms a vector
Definition: Matrix3.h:143
void(* trfn)(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
Transformation function type.
#define PI
Header file for Khan&#39;s deghosting algorithm Copyright (C) 2009 Lukáš Jirkovský l...
Definition: khan.h:43
void SetRotationX(double a)
set the matrice to rotation around X
Definition: Matrix3.cpp:173
Parameters for transformation calls Can be just one double, two double, 4 double, a matrix...
#define DEBUG_FATAL(msg)
Definition: utils.h:78
void erect_stereographic(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from stereographic to erect
Matrix3 SetMatrix(double a, double b, double c, int cl)
void createTransform(const SrcPanoImage &src, const PanoramaOptions &dest)
implementation of Space Transformation
static double CalcCorrectionRadius_copy(double *coeff)
void rotate_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
static void radial_shift(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
static void squareZero_copy(double *a, int *n, double *root)
#define var5
void setVar(const std::string &name, double val)
Function descriptor to be executed by exec_function.
bool getCorrectTCA() const
void erect_sphere_tp(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
float pow(float a, double b)
Definition: utils.h:181
#define var0
bool transform(hugin_utils::FDiff2D &dest, const hugin_utils::FDiff2D &src) const
transform Get the new coordinates
double m_srcTX
used to convert from screen to cartesian coordinates
void pano_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void SetRotationZ(double a)
Definition: Matrix3.cpp:187
Model for a panorama.
Definition: PanoramaData.h:81
virtual const SrcPanoImage & getImage(std::size_t nr) const =0
get a panorama image, counting starts with 0
void rect_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
std::vector< fDescription > m_Stack
vector of transformations
void vert(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void sphere_tp_rect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void mercator_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from erect to mercator
void InitRadialCorrect(const vigra::Size2D &sz, const std::vector< double > &radDist, const hugin_utils::FDiff2D &centerShift)
transformation for radial correction only
std::map< std::string, Variable > VariableMap
void pano_sphere_tp(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void InitInvRadialCorrect(const SrcPanoImage &src, int channel=1)
Create a transform stack for distortion &amp; TCA correction only.
void SetRotationY(double a)
Definition: Matrix3.cpp:180
#define R_EPS
vigra::RGBValue< T, RIDX, GIDX, BIDX > log(vigra::RGBValue< T, RIDX, GIDX, BIDX > const &v)
component-wise logarithm
Definition: utils.h:209
unsigned int getWidth() const
void transpano_erect(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
static double cubeRoot_copy(double x)
double z
Definition: Vector3.h:47
void createInvTransform(const SrcPanoImage &src, const PanoramaOptions &dest)
static T max(T x, T y)
Definition: svm.cpp:65
#define DEBUG_DEBUG(msg)
Definition: utils.h:68
void AddTransform(trfn function_name, double var0, double var1=0.0f, double var2=0.0f, double var3=0.0f, double var4=0.0f, double var5=0.0f, double var6=0.0f, double var7=0.0f)
add a new transformation
void setSize(vigra::Size2D val)
Set the image size in pixels.
double estRadialScaleCrop(const std::vector< double > &coeff, int width, int height)
Calculate effective scaling factor.
#define var2
double estScaleFactorForFullFrame(const SrcPanoImage &src)
Calculate effective scaling factor for a given source image.
void erect_transpano(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void resize(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
#define DEG_TO_RAD(x)
Definition: hugin_math.h:44
void persp_sphere(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
general : Vector3 is a class for handling 3D Vectors manipulation.
Definition: Vector3.h:43
ProjectionFormat
Projection of final panorama.
void erect_pano(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
double x
x,y,z coordinates, 0 at the initialisation
Definition: Vector3.h:47
All variables of a source image.
Definition: SrcPanoImage.h:194
void rect_sphere_tp(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void erect_mercator(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from mercator to erect
Panorama image options.
bool m_Initialized
was the class initialized ?
static void cubeZero_copy(double *a, int *n, double *root)
static double smallestRoot_copy(double *p)
#define var1
void inv_radial(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void horiz(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
void erect_transmercator(double x_dest, double y_dest, double *x_src, double *y_src, const _FuncParams &params)
convert from erect to transverse mercator