SpaceTransform.cpp

Go to the documentation of this file.
00001 // -*- c-basic-offset: 4 -*-
00002 
00027 #include "SpaceTransform.h"
00028 
00029 namespace HuginBase {
00030 namespace Nona {
00031         
00032 
00034 SpaceTransform::SpaceTransform() : m_srcTX(0), m_srcTY(0), m_destTX(0), m_destTY(0)
00035 {
00036 
00037         m_Initialized = false;
00038 }
00039 
00041 SpaceTransform::~SpaceTransform()
00042 {
00043 }
00044 
00045 void SpaceTransform::AddTransform( trfn function_name, double var0, double var1, double var2, double var3, double var4, double var5, double var6, double var7 )
00046 {
00047         fDescription fD;
00048         fD.param.var0   = var0;
00049         fD.param.var1   = var1;
00050         fD.param.var2   = var2;
00051         fD.param.var3   = var3;
00052         fD.param.var4   = var4;
00053         fD.param.var5   = var5;
00054         fD.param.var6   = var6;
00055         fD.param.var7   = var7;
00056         fD.func                 = function_name;
00057         m_Stack.push_back( fD );
00058 }
00059 
00060 void SpaceTransform::AddTransform( trfn function_name, Matrix3 m, double var0, double var1, double var2, double var3)
00061 {
00062         fDescription fD;
00063         fD.param.distance       = var0;
00064         fD.param.var1   = var1;
00065         fD.param.var2   = var2;
00066         fD.param.var3   = var3;
00067         fD.param.mt                     = m;
00068         fD.func                         = function_name;
00069         m_Stack.push_back( fD );
00070 }
00071 
00072 
00073 
00074 
00075 //==============================================================================
00076 // Pano12.dll (math.c) functions, helmut dersch
00077 
00078 #define MAXITER 100
00079 #define R_EPS 1.0e-6
00080 
00081 // Rotate equirectangular image
00082 void rotate_erect( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams &params)
00083 {
00084         // params: double 180degree_turn(screenpoints), double turn(screenpoints);
00085         *x_src = x_dest + params.var1;
00086         while( *x_src < -params.var0 )
00087                 *x_src += 2 * params.var0;
00088         while( *x_src >  params.var0 )
00089                 *x_src -= 2 * params.var0;
00090         *y_src = y_dest;
00091 }
00092 
00093 // Calculate inverse 4th order polynomial correction using Newton
00094 // Don't use on large image (slow)!
00095 void inv_radial( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams &params)
00096 {
00097         // params: double coefficients[5]
00098         register double rs, rd, f, scale;
00099         int iter = 0;
00100 
00101         rd      = (sqrt( x_dest*x_dest + y_dest*y_dest )) / params.var4; // Normalized
00102 
00103         rs      = rd;                           
00104         f       = (((params.var3 * rs + params.var2) * rs + params.var1) * rs + params.var0) * rs;
00105 
00106         while( std::abs(f - rd) > R_EPS && iter++ < MAXITER )
00107         {
00108                 rs = rs - (f - rd) / ((( 4 * params.var3 * rs + 3 * params.var2) * rs  +
00109                                                   2 * params.var1) * rs + params.var0);
00110 
00111                 f       = (((params.var3 * rs + params.var2) * rs +
00112                                 params.var1) * rs + params.var0) * rs;
00113         }
00114 
00115         scale = rs / rd;
00116 //      printf("scale = %lg iter = %d\n", scale,iter);  
00117         
00118         *x_src = x_dest * scale  ;
00119         *y_src = y_dest * scale  ;
00120 }
00121 
00122 /*
00123 static void inv_vertical( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams &params)
00124 {
00125         // params: double coefficients[4]
00126         register double rs, rd, f, scale;
00127         int iter = 0;
00128 
00129         rd      = abs( y_dest ) / params.var4; // Normalized
00130         rs      = rd;                           
00131         f       = (((params.var3 * rs + params.var2) * rs + params.var1) * rs + params.var0) * rs;
00132 
00133         while( abs(f - rd) > R_EPS && iter++ < MAXITER )
00134         {
00135                 rs = rs - (f - rd) / ((( 4 * params.var3 * rs + 3 * params.var2) * rs  +
00136                                                   2 * params.var1) * rs + params.var0);
00137 
00138                 f       = (((params.var3 * rs + params.var2) * rs + params.var1) * rs + params.var0) * rs;
00139         }
00140 
00141         scale = rs / rd;
00142 //      printf("scale = %lg iter = %d\n", scale,iter);  
00143         
00144         *x_src = x_dest  ;
00145         *y_src = y_dest * scale  ;
00146 }
00147 */
00148 
00149 // scale
00150 void resize( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00151 {
00152         // params: double scale_horizontal, double scale_vertical;
00153         *x_src = x_dest * params.var0;
00154         *y_src = y_dest * params.var1;
00155 }
00156 
00157 /*
00158 // shear
00159 static void shear( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00160 {
00161         // params: double shear_horizontal, double shear_vertical;
00162         *x_src  = x_dest + params.var0 * y_dest;
00163         *y_src  = y_dest + params.var1 * x_dest;
00164 }
00165 */
00166 
00167 // horiz shift
00168 void horiz( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00169 {
00170         // params: double horizontal params.shift
00171         *x_src  = x_dest + params.shift;        
00172         *y_src  = y_dest;
00173 }
00174 
00175 // vertical shift
00176 void vert( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00177 {
00178         // params: double vertical params.shift
00179         *x_src  = x_dest;       
00180         *y_src  = y_dest + params.shift;
00181 }
00182 
00183 // radial
00184 void radial( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00185 {
00186         // params: double coefficients[4], scale, correction_radius
00187         register double r, scale;
00188 
00189         r = (sqrt( x_dest*x_dest + y_dest*y_dest )) / params.var4;
00190         if( r < params.var5 )
00191         {
00192                 scale = ((params.var3 * r + params.var2) * r + params.var1) * r + params.var0;
00193         }
00194         else
00195                 scale = 1000.0;
00196         
00197         *x_src = x_dest * scale  ;
00198         *y_src = y_dest * scale  ;
00199 }
00200 
00201 /*
00202 //
00203 static void vertical( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00204 {
00205         // params: double coefficients[4]
00206         register double r, scale;
00207 
00208         r = y_dest / params.var4;
00209 
00210         if( r < 0.0 ) r = -r;
00211 
00212         scale = ((params.var3 * r + params.var2) * r + params.var1) * r + params.var0;
00213         
00214         *x_src = x_dest;
00215         *y_src = y_dest * scale  ;
00216 }
00217 */
00218 
00219 /*
00220 //
00221 static void deregister( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00222 {
00223         // params: double coefficients[4]
00224         register double r, scale;
00225 
00226         r = y_dest / params.var4;
00227 
00228         if( r < 0.0 ) r = -r;
00229 
00230         scale   = (params.var3 * r + params.var2) * r + params.var1 ;
00231         
00232         *x_src = x_dest + abs( y_dest ) * scale;
00233         *y_src = y_dest ;
00234 }
00235 */
00236 // perspective
00237 void persp_sphere( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00238 {
00239         // params :  double Matrix[3][3], double params.distance
00240         register double theta,s,r;
00241         Vector3 v, v2;
00242 
00243         r = sqrt( x_dest * x_dest + y_dest * y_dest );
00244         theta   = r / params.distance;
00245         if( r == 0.0 )
00246                 s = 0.0;
00247         else
00248                 s = sin( theta ) / r;
00249 
00250         v.x =  s * x_dest ;
00251         v.y =  s * y_dest ;
00252         v.z =  cos( theta );
00253 
00254         v2 = params.mt.TransformVector( v );
00255 
00256         r = sqrt( v2.x*v2.x + v2.y*v2.y );
00257         if( r == 0.0 )
00258                 theta = 0.0;
00259         else
00260                 theta   = params.distance * atan2( r, v2.z ) / r;
00261         *x_src  = theta * v2.x;
00262         *y_src  = theta * v2.y;
00263 }       
00264 
00265 
00266 // perspective rect
00267 void persp_rect( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00268 {
00269         // params :  double Matrix[3][3], double params.distance, double x-offset, double y-offset
00270         Vector3 v;
00271         v.x = x_dest + params.var2;
00272         v.y = y_dest + params.var3;
00273         v.z = params.var1;
00274         v = params.mt.TransformVector( v );
00275         *x_src = v.x * params.var1 / v.z;
00276         *y_src = v.y * params.var1 / v.z;
00277 }
00278 
00279 /*
00280 //
00281 static void rect_pano( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00282 {                                                                       
00283         *x_src = params.distance * tan( x_dest / params.distance ) ;
00284         *y_src = y_dest / cos( x_dest / params.distance );
00285 }
00286 */
00287 
00288 /*
00289 //
00290 static void pano_rect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00291 {       
00292         *x_src = params.distance * atan ( x_dest / params.distance );
00293         *y_src = y_dest * cos( *x_src / params.distance );
00294 }
00295 */
00296 
00297 //
00298 void rect_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00299 {       
00300         // params: double params.distance
00301         register double  phi, theta;
00302 
00303         phi     = x_dest / params.distance;
00304         theta   =  - y_dest / params.distance  + PI / 2.0;
00305         if(theta < 0)
00306         {
00307                 theta = - theta;
00308                 phi += PI;
00309         }
00310         if(theta > PI)
00311         {
00312                 theta = PI - (theta - PI);
00313                 phi += PI;
00314         }
00315 
00316         *x_src = params.distance * tan(phi);
00317         *y_src = params.distance / (tan( theta ) * cos(phi));
00318 }
00319 
00320 //
00321 void pano_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00322 {       
00323         // params: double params.distance
00324         *x_src = x_dest;
00325         *y_src = params.distance * tan( y_dest / params.distance);
00326 }
00327 
00328 //
00329 void erect_pano( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00330 {       
00331         // params: double params.distance
00332         *x_src = x_dest;
00333         *y_src = params.distance * atan( y_dest / params.distance);
00334 }
00335 
00336 // FIXME: implement!
00337 void transpano_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00338 {       
00339     // params: double params.distance
00340     *x_src = x_dest;
00341     *y_src = params.distance * tan( y_dest / params.distance);
00342 }
00343 
00344 // FIXME: implement!
00345 void erect_transpano( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00346 {       
00347         // params: double params.distance
00348     *x_src = x_dest;
00349     *y_src = params.distance * atan( y_dest / params.distance);
00350 }
00351 
00352 /*
00353 //
00354 static void sphere_cp_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00355 {
00356         // params: double params.distance, double b
00357         register double phi, theta;
00358         phi     = - x_dest /  ( params.var0 * PI / 2.0);
00359         theta   =  - ( y_dest + params.var1 ) / ( PI / 2.0) ;
00360         *x_src =  theta * cos( phi );
00361         *y_src =  theta * sin( phi );
00362 }
00363 */
00364 
00365 //
00366 void sphere_tp_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00367 {
00368         // params: double params.distance
00369         register double phi, theta, r,s;
00370         double v[3];
00371         phi     = x_dest / params.distance;
00372         theta   =  - y_dest / params.distance  + PI / 2;
00373         if(theta < 0)
00374         {
00375                 theta = - theta;
00376                 phi += PI;
00377         }
00378         if(theta > PI)
00379         {
00380                 theta = PI - (theta - PI);
00381                 phi += PI;
00382         }
00383         s = sin( theta );
00384         v[0] =  s * sin( phi ); //  y' -> x
00385         v[1] =  cos( theta );                           //  z' -> y
00386         r = sqrt( v[1]*v[1] + v[0]*v[0]);       
00387         theta = params.distance * atan2( r , s * cos( phi ) );
00388 
00389         *x_src =  theta * v[0] / r;
00390         *y_src =  theta * v[1] / r;
00391 }
00392 
00393 /*
00394 //
00395 static void erect_sphere_cp( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00396 {
00397         // params: double params.distance, double b
00398         register double phi, theta;
00399         theta = sqrt( x_dest * x_dest + y_dest * y_dest ) ;
00400         phi   = atan2( y_dest , -x_dest );
00401         *x_src = params.var0 * phi;
00402         *y_src = theta - params.var1;
00403 }
00404 */
00405 
00406 //
00407 void rect_sphere_tp( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00408 {
00409         // params: double params.distance
00410         register double rho, theta,r;
00411         r = sqrt( x_dest*x_dest + y_dest*y_dest );
00412         theta   = r / params.distance;
00413 
00414     if( theta >= PI /2.0 )
00415         rho = 1.6e16 ;
00416     else if( theta == 0.0 )
00417                 rho = 1.0;
00418         else
00419                 rho =  tan( theta ) / theta;
00420         *x_src = rho * x_dest ;
00421         *y_src = rho * y_dest ;
00422 }
00423 
00424 //
00425 void sphere_tp_rect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00426 {       
00427         // params: double params.distance
00428         register double  theta, r;
00429         r = sqrt(x_dest*x_dest + y_dest*y_dest) / params.distance;
00430         if( r== 0.0 )
00431                 theta = 1.0;
00432         else
00433                 theta   = atan( r ) / r;
00434         *x_src =  theta * x_dest ;
00435         *y_src =  theta * y_dest ;
00436 }
00437 
00438 //
00439 void sphere_tp_pano( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00440 {
00441         // params: double params.distance
00442         register double r, s, Phi, theta;
00443         Phi = x_dest / params.distance;
00444         s =  params.distance * sin( Phi ) ;     //  y' -> x
00445         r = sqrt( s*s + y_dest*y_dest );
00446         theta = params.distance * atan2( r , (params.distance * cos( Phi )) ) / r;
00447         *x_src =  theta * s ;
00448         *y_src =  theta * y_dest ;
00449 }
00450 
00451 //
00452 void pano_sphere_tp( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00453 {
00454         // params: double params.distance
00455         register double r,s, theta;
00456         double v[3];
00457         r = sqrt( x_dest * x_dest + y_dest * y_dest );
00458         theta = r / params.distance;
00459         if( theta == 0.0 )
00460                 s = 1.0 / params.distance;
00461         else
00462                 s = sin( theta ) /r;
00463         v[1] =  s * x_dest ;   //  x' -> y
00464         v[0] =  cos( theta );                           //  z' -> x
00465         *x_src = params.distance * atan2( v[1], v[0] );
00466         *y_src = params.distance * s * y_dest / sqrt( v[0]*v[0] + v[1]*v[1] );
00467 }
00468 
00469 /*
00470 //
00471 static void sphere_cp_pano( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00472 {
00473         // params: double params.distance
00474         register double phi, theta;
00475         phi     = -x_dest / (params.distance * PI / 2.0) ;
00476         theta   = PI /2.0 + atan( y_dest / (params.distance * PI/2.0) );
00477         *x_src = params.distance * theta * cos( phi );
00478         *y_src = params.distance * theta * sin( phi );
00479 }
00480 */
00481 
00482 //
00483 void erect_rect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00484 {
00485         // params: double params.distance
00486         *x_src = params.distance * atan2( x_dest, params.distance );
00487         *y_src = params.distance * atan2(  y_dest, sqrt( params.distance*params.distance + x_dest*x_dest ) );
00488 }
00489 
00490 //
00491 void erect_sphere_tp( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00492 {
00493         // params: double params.distance
00494         register double  theta,r,s;
00495         double  v[3];
00496         r = sqrt( x_dest * x_dest + y_dest * y_dest );
00497         theta = r / params.distance;
00498         if(theta == 0.0)
00499                 s = 1.0 / params.distance;
00500         else
00501                 s = sin( theta) / r;
00502         
00503         v[1] =  s * x_dest;
00504         v[0] =  cos( theta );                           
00505         
00506         *x_src = params.distance * atan2( v[1], v[0] );
00507         *y_src = params.distance * atan( s * y_dest /sqrt( v[0]*v[0] + v[1]*v[1] ) );
00508 }
00509 
00511 void mercator_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00512 {
00513     // params: distance
00514     *x_src = x_dest;
00515     *y_src = params.distance*log(tan(y_dest/params.distance)+1/cos(y_dest/params.distance));
00516 }
00517 
00519 void erect_mercator( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00520 {
00521     // params: distance
00522     *x_src = x_dest;
00523     *y_src = params.distance*atan(sinh(y_dest/params.distance));
00524 }
00525 
00526 
00528 void transmercator_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00529 {
00530     // params: distance
00531     x_dest /= params.distance;
00532     y_dest /= params.distance;
00533     double B = cos(y_dest)*sin(x_dest);
00534     *x_src = params.distance / tanh(B);
00535     *y_src = params.distance * atan(tan(y_dest)/cos(x_dest));
00536 }
00537 
00539 void erect_transmercator( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00540 {
00541     // params: distance
00542     x_dest /= params.distance;
00543     y_dest /= params.distance;
00544     *x_src = params.distance * atan(sinh(x_dest)/cos(y_dest));
00545     *y_src = params.distance * asin(sin(y_dest)/cosh(x_dest));
00546 }
00547 
00549 void sinusoidal_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00550 {
00551     // params: distance
00552 
00553     *x_src = params.distance * (x_dest/params.distance*cos(y_dest/params.distance));
00554     *y_src = y_dest;
00555 }
00556 
00558 void erect_sinusoidal( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00559 {
00560     // params: distance
00561 
00562     *y_src = y_dest;
00563     *x_src = x_dest/cos(y_dest/params.distance);
00564 }
00565 
00567 void stereographic_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00568 {
00569     // params: distance
00570     double lon = x_dest / params.distance;
00571     double lat = y_dest / params.distance;
00572 
00573     // use: R = 1
00574     double k=2.0/(1+cos(lat)*cos(lon));
00575     *x_src = params.distance * k*cos(lat)*sin(lon);
00576     *y_src = params.distance * k*sin(lat);
00577 }
00578 
00580 void erect_stereographic( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00581 {
00582     // params: distance
00583 
00584     // use: R = 1
00585     double p=sqrt(x_dest*x_dest + y_dest*y_dest) / params.distance;
00586     double c= 2.0*atan(p/2.0);
00587 
00588     *x_src = params.distance * atan2(x_dest/params.distance*sin(c),(p*cos(c)));
00589     *y_src = params.distance * asin(y_dest/params.distance*sin(c)/p);
00590 }
00591 
00592 
00593 /*
00594 //
00595 static void mirror_sphere_cp( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00596 {
00597         // params: double params.distance, double b
00598         register double rho, phi, theta;
00599         theta = sqrt( x_dest * x_dest + y_dest * y_dest ) / params.var0;
00600         phi   = atan2( y_dest , x_dest );
00601         rho = params.var1 * sin( theta / 2.0 );
00602         *x_src = - rho * cos( phi );
00603         *y_src = rho * sin( phi );
00604 }
00605 */
00606 
00607 /*
00608 //
00609 static void mirror_erect( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00610 {
00611         // params: double params.distance, double b, double b2
00612         register double phi, theta, rho;
00613         phi     =  x_dest / ( params.var0 * PI/2.0) ;
00614         theta   =  - ( y_dest + params.var2 ) / (params.var0 * PI/2.0)  ;
00615         rho = params.var1 * sin( theta / 2.0 );
00616         *x_src = - rho * cos( phi );
00617         *y_src = rho * sin( phi );
00618 }
00619 */
00620 
00621 /*
00622 //
00623 static void mirror_pano( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00624 {
00625         // params: double params.distance, double b
00626         register double phi, theta, rho;
00627         phi     = -x_dest / (params.var0 * PI/2.0) ;
00628         theta   = PI /2.0 + atan( y_dest / (params.var0 * PI/2.0) );
00629         rho = params.var1 * sin( theta / 2.0 );
00630         *x_src = rho * cos( phi );
00631         *y_src = rho * sin( phi );
00632 }
00633 */
00634 
00635 /*
00636 //
00637 static void sphere_cp_mirror( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00638 {
00639         // params: double params.distance, double b
00640         register double phi, theta, rho;
00641         rho = sqrt( x_dest*x_dest + y_dest*y_dest );
00642         theta = 2 * asin( rho/params.var1 );
00643         phi   = atan2( y_dest , x_dest );
00644         *x_src = params.var0 * theta * cos( phi );
00645         *y_src = params.var0 * theta * sin( phi );
00646 }
00647 */
00648 
00649 /*
00650 //
00651 static void shift_scale_rotate( double x_dest,double  y_dest, double* x_src, double* y_src, const _FuncParams & params)
00652 {
00653         // params: double params.shift_x, params.shift_y, scale, cos_phi, sin_phi
00654         register double x = x_dest - params.var0;
00655         register double y = y_dest - params.var1;
00656         *x_src = (x * params.var3 - y * params.var4) * params.var2;
00657         *y_src = (x * params.var4 + y * params.var3) * params.var2;
00658 }
00659 */
00660 
00661 /*
00662 
00663 // Correct radial luminance change using parabel
00664 unsigned char radlum( unsigned char srcPixel, int xc, int yc, void *params )
00665 {
00666         // params: second and zero order polynomial coeff
00667         register double result;
00668 
00669         result = (xc * xc + yc * yc) * params.var0 + params.var1;
00670         result = ((double)srcPixel) - result;
00671 
00672         if(result < 0.0) return 0;
00673         if(result > 255.0) return 255;
00674 
00675         return( (unsigned char)(result+0.5) );
00676 }
00677 
00678 
00679 // Get smallest positive (non-zero) root of polynomial with degree deg and
00680 // (n+1) real coefficients p[i]. Return it, or 1000.0 if none exists or error occurred
00681 // Changed to only allow degree 3
00682 #if 0
00683 double smallestRoot( double *p )
00684 {
00685         doublecomplex           root[3], poly[4];
00686         doublereal                      radius[3], apoly[4], apolyr[4];
00687         logical                         myErr[3];
00688         double                          sRoot = 1000.0;
00689         doublereal                      theEps, theBig, theSmall;
00690         integer                         nitmax;
00691         integer                         iter;
00692         integer                         n,i;
00693         
00694         n               = 3;
00695 
00696         
00697         for( i=0; i< n+1; i++)
00698         {
00699                 poly[i].r = p[i];
00700                 poly[i].i = 0.0;
00701         }
00702         
00703         theEps   = DBL_EPSILON;                 // machine precision
00704         theSmall = DBL_MIN ;                    // smallest positive real*8
00705         theBig   = DBL_MAX ;                    // largest real*8
00706 
00707         nitmax  = 100;
00708 
00709     polzeros_(&n, poly, &theEps, &theBig, &theSmall, &nitmax, root, radius, myErr, &iter, apoly, apolyr);
00710 
00711         for( i = 0; i < n; i++ )
00712         {
00713 //              PrintError("No %d : Real %g, Imag %g, radius %g, myErr %ld", i, root[i].r, root[i].i, radius[i], myErr[i]);
00714                 if( (root[i].r > 0.0) && (dabs( root[i].i ) <= radius[i]) && (root[i].r < sRoot) )
00715                         sRoot = root[i].r;
00716         }
00717 
00718         return sRoot;
00719 }
00720 #endif
00721 
00722 void cubeZero( double *a, int *n, double *root );
00723 void squareZero( double *a, int *n, double *root );
00724 double cubeRoot( double x );
00725 
00726 
00727 void cubeZero( double *a, int *n, double *root ){
00728         if( a[3] == 0.0 ){ // second order polynomial
00729                 squareZero( a, n, root );
00730         }else{
00731                 double p = ((-1.0/3.0) * (a[2]/a[3]) * (a[2]/a[3]) + a[1]/a[3]) / 3.0;
00732                 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;
00733                 
00734                 if( q*q + p*p*p >= 0.0 ){
00735                         *n = 1;
00736                         root[0] = cubeRoot(-q + sqrt(q*q + p*p*p)) + cubeRoot(-q - sqrt(q*q + p*p*p)) - a[2] / (3.0 * a[3]);
00737                 }else{
00738                         double phi = acos( -q / sqrt(-p*p*p) );
00739                         *n = 3;
00740                         root[0] =  2.0 * sqrt(-p) * cos(phi/3.0) - a[2] / (3.0 * a[3]);
00741                         root[1] = -2.0 * sqrt(-p) * cos(phi/3.0 + PI/3.0) - a[2] / (3.0 * a[3]);
00742                         root[2] = -2.0 * sqrt(-p) * cos(phi/3.0 - PI/3.0) - a[2] / (3.0 * a[3]);
00743                 }
00744         }
00745         // PrintError("%lg, %lg, %lg, %lg root = %lg", a[3], a[2], a[1], a[0], root[0]);
00746 }
00747 
00748 void squareZero( double *a, int *n, double *root ){
00749         if( a[2] == 0.0 ){ // linear equation
00750                 if( a[1] == 0.0 ){ // constant
00751                         if( a[0] == 0.0 ){
00752                                 *n = 1; root[0] = 0.0;
00753                         }else{
00754                                 *n = 0;
00755                         }
00756                 }else{
00757                         *n = 1; root[0] = - a[0] / a[1];
00758                 }
00759         }else{
00760                 if( 4.0 * a[2] * a[0] > a[1] * a[1] ){
00761                         *n = 0;
00762                 }else{
00763                         *n = 2;
00764                         root[0] = (- a[1] + sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
00765                         root[1] = (- a[1] - sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
00766                 }
00767         }
00768 
00769 }
00770 
00771 double cubeRoot( double x ){
00772         if( x == 0.0 )
00773                 return 0.0;
00774         else if( x > 0.0 )
00775                 return pow(x, 1.0/3.0);
00776         else
00777                 return - pow(-x, 1.0/3.0);
00778 }
00779 
00780 double smallestRoot( double *p ){
00781         int n,i;
00782         double root[3], sroot = 1000.0;
00783         
00784         cubeZero( p, &n, root );
00785         
00786         for( i=0; i<n; i++){
00787                 // PrintError("Root %d = %lg", i,root[i]);
00788                 if(root[i] > 0.0 && root[i] < sroot)
00789                         sroot = root[i];
00790         }
00791         
00792         // PrintError("Smallest Root  = %lg", sroot);
00793         return sroot;
00794 }
00795 
00796 */
00797 
00798 
00799 
00800 // really strange. the pano12.dll for windows doesn't seem to
00801 // contain the SetCorrectionRadius function, so it is included here
00802 
00803 static void cubeZero_copy( double *a, int *n, double *root );
00804 static void squareZero_copy( double *a, int *n, double *root );
00805 static double cubeRoot_copy( double x );
00806 
00807 
00808 static void cubeZero_copy( double *a, int *n, double *root ){
00809         if( a[3] == 0.0 ){ // second order polynomial
00810                 squareZero_copy( a, n, root );
00811         }else{
00812                 double p = ((-1.0/3.0) * (a[2]/a[3]) * (a[2]/a[3]) + a[1]/a[3]) / 3.0;
00813                 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;
00814                 
00815                 if( q*q + p*p*p >= 0.0 ){
00816                         *n = 1;
00817                         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]);
00818                 }else{
00819                         double phi = acos( -q / sqrt(-p*p*p) );
00820                         *n = 3;
00821                         root[0] =  2.0 * sqrt(-p) * cos(phi/3.0) - a[2] / (3.0 * a[3]);
00822                         root[1] = -2.0 * sqrt(-p) * cos(phi/3.0 + PI/3.0) - a[2] / (3.0 * a[3]);
00823                         root[2] = -2.0 * sqrt(-p) * cos(phi/3.0 - PI/3.0) - a[2] / (3.0 * a[3]);
00824                 }
00825         }
00826         // PrintError("%lg, %lg, %lg, %lg root = %lg", a[3], a[2], a[1], a[0], root[0]);
00827 }
00828 
00829 static void squareZero_copy( double *a, int *n, double *root ){
00830         if( a[2] == 0.0 ){ // linear equation
00831                 if( a[1] == 0.0 ){ // constant
00832                         if( a[0] == 0.0 ){
00833                                 *n = 1; root[0] = 0.0;
00834                         }else{
00835                                 *n = 0;
00836                         }
00837                 }else{
00838                         *n = 1; root[0] = - a[0] / a[1];
00839                 }
00840         }else{
00841                 if( 4.0 * a[2] * a[0] > a[1] * a[1] ){
00842                         *n = 0;
00843                 }else{
00844                         *n = 2;
00845                         root[0] = (- a[1] + sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
00846                         root[1] = (- a[1] - sqrt( a[1] * a[1] - 4.0 * a[2] * a[0] )) / (2.0 * a[2]);
00847                 }
00848         }
00849 
00850 }
00851 
00852 static double cubeRoot_copy( double x ){
00853         if( x == 0.0 )
00854                 return 0.0;
00855         else if( x > 0.0 )
00856                 return pow(x, 1.0/3.0);
00857         else
00858                 return - pow(-x, 1.0/3.0);
00859 }
00860 
00861 static double smallestRoot_copy( double *p ){
00862         int n,i;
00863         double root[3], sroot = 1000.0;
00864         
00865         cubeZero_copy( p, &n, root );
00866         
00867         for( i=0; i<n; i++){
00868                 // PrintError("Root %d = %lg", i,root[i]);
00869                 if(root[i] > 0.0 && root[i] < sroot)
00870                         sroot = root[i];
00871         }
00872         
00873         // PrintError("Smallest Root  = %lg", sroot);
00874         return sroot;
00875 }
00876 
00877 
00878 // Restrict radial correction to monotonous interval
00879 static double CalcCorrectionRadius_copy(double *coeff )
00880 {
00881     double a[4];
00882     int k;
00883         
00884     for( k=0; k<4; k++ )
00885     {
00886         a[k] = 0.0;//1.0e-10;
00887         if( coeff[k] != 0.0 )
00888         {
00889             a[k] = (k+1) * coeff[k];
00890         }
00891     }
00892     return smallestRoot_copy( a );
00893 }
00894 
00895 // radial and shift
00896 static void radial_shift( double x_dest, double y_dest, double* x_src, double* y_src, const _FuncParams & params)
00897 {
00898     // params: double coefficients[4], scale, correction_radius, shift_x, shift_y
00899     register double r, scale;
00900 
00901     r = (sqrt( x_dest*x_dest + y_dest*y_dest )) / params.var4;
00902     if( r < params.var5 )
00903     {
00904         scale = ((params.var3 * r + params.var2) * r + params.var1) * r + params.var0;
00905     }
00906     else
00907         scale = 1000.0;
00908         
00909     *x_src = x_dest * scale  + params.var6;
00910     *y_src = y_dest * scale  + params.var7;
00911 }
00912 
00913 
00914 //==============================================================================
00915 
00916 
00917 Matrix3 SetMatrix( double a, double b, double c, int cl )
00918 {
00919     Matrix3 mx, my, mz;
00920     //    Matrix3 dummy;
00921         
00922     // Calculate Matrices;
00923     mx.SetRotationX( a );
00924     my.SetRotationY( b );
00925     mz.SetRotationZ( c );
00926         
00927     if (cl)
00928         return ( (mz * mx) * my );
00929     else
00930         return ( (mx * mz) * my );
00931         
00932         //if( cl )
00933         //              matrix_matrix_mult( mz, mx,     dummy);
00934         //else
00935         //      matrix_matrix_mult( mx, mz,     dummy);
00936         //matrix_matrix_mult( dummy, my, m);
00937 }
00938 
00939 
00940 double estScaleFactorForFullFrame(const SrcPanoImage & src)
00941 {
00942     SpaceTransform transf;
00943     transf.InitInvRadialCorrect(src, 1);
00944     vigra::Rect2D inside;
00945     vigra::Rect2D insideTemp;
00946     vigra::Rect2D boundingBox;
00947     traceImageOutline(src.getSize(), transf, inside, boundingBox);
00948     if (src.getCorrectTCA()) {
00949         transf.InitInvRadialCorrect(src, 0);
00950         traceImageOutline(src.getSize(), transf, insideTemp, boundingBox);
00951         inside &= insideTemp;
00952         transf.InitInvRadialCorrect(src, 2);
00953         traceImageOutline(src.getSize(), transf, insideTemp, boundingBox);
00954         inside &= insideTemp;
00955     }
00956     double width2 = src.getSize().x/2.0;
00957     double height2 = src.getSize().y/2.0;
00958     double sx = std::max(width2/(width2-inside.left()), width2/(inside.right()-width2));
00959     double sy = std::max(height2/(height2-inside.top()), height2/(inside.bottom()-height2));
00960     return 1/std::max(sx,sy);
00961 }
00962 
00963 
00964 double estRadialScaleCrop(const std::vector<double> &coeff, int width, int height)
00965 {
00966     double r_test[4];
00967     double p, r;
00968     int test_points, i;
00969     double a, b, c, d;
00970 
00971     // truncate black edges
00972     // algorithm courtesy of Paul Wilkinson, paul.wilkinson@ntlworld.com
00973     // modified by dangelo to just calculate the scaling factor -> better results
00974 
00975     a = coeff[0];
00976     b = coeff[1];
00977     c = coeff[2];
00978     d = coeff[3];
00979 
00980     if (width > height)
00981         p = (double)(width) / (double)(height);
00982     else
00983         p = (double)(height) / (double)(width);
00984 
00985     //***************************************************
00986     //* Set the test point for the far corner.          *
00987     //***************************************************
00988     r_test[0] = sqrt(1 + p * p);
00989     test_points = 1;
00990 
00991     //***************************************************
00992     //* For non-zero values of a, there are two other   *
00993     //* possible test points. (local extrema)           *
00994     //***************************************************
00995     //
00996 
00997     if (a != 0.0)
00998     {
00999         r = (-b + sqrt(b * b - 3 * a * c)) / (3 * a);
01000         if (r >= 1 && r <= r_test[0])
01001         {
01002             r_test[test_points] = r;
01003             test_points = test_points + 1;
01004         }
01005         r = (-b - sqrt(b * b - 3 * a * c)) / (3 * a);
01006         if (r >= 1 && r <= r_test[0])
01007         {
01008             r_test[test_points] = r;
01009             test_points = test_points + 1;
01010         }
01011     }
01012 
01013     //***************************************************
01014     //* For zero a and non-zero b, there is one other   *
01015     //* possible test point.                            *
01016     //***************************************************
01017     if (a == 0.0 && b != 0.0)
01018     {
01019         r = -c / (2 * b);
01020         if (r >= 1 && r <= r_test[0])
01021         {
01022             r_test[test_points] = r;
01023             test_points = test_points + 1;
01024         }
01025     }
01026 
01027     // check the scaling factor at the test points.
01028     // start with a very high scaling factor.
01029     double scalefactor = 0.1;
01030     for (i = 0; i <= test_points - 1; i++)
01031     {
01032         r = r_test[i];
01033         double scale = d + r * (c + r * (b + r * a));
01034         if ( scalefactor < scale)
01035             scalefactor = scale;
01036     }
01037     return scalefactor;
01038 }
01039 
01040 
01041 
01042 //==============================================================================
01043 
01044 
01046 void SpaceTransform::InitRadialCorrect(const vigra::Size2D & sz, const std::vector<double> & radDist, 
01047                                  const hugin_utils::FDiff2D & centerShift)
01048 {
01049     double mprad[6];
01050     
01051 //    double  imwidth = src.getSize().x;
01052 //    double  imheight= src.getSize().y;
01053 
01054     m_Stack.clear();
01055     m_srcTX = sz.x/2.0;
01056     m_srcTY = sz.y/2.0;
01057     m_destTX = sz.x/2.0;
01058     m_destTY = sz.y/2.0;
01059 
01060     // green channel, always correct
01061     for (int i=0; i < 4; i++) {
01062         mprad[3-i] = radDist[i];
01063     }
01064     mprad[4] = ( sz.x < sz.y ? sz.x: sz.y)  / 2.0;
01065     // calculate the correction radius.
01066     mprad[5] = CalcCorrectionRadius_copy(mprad);
01067 
01068     // radial correction if nonzero radial coefficients
01069     if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
01070         AddTransform (&radial_shift, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5],
01071                      centerShift.x, centerShift.y);
01072     }
01073 }
01074 
01076 void SpaceTransform::InitInvRadialCorrect(const SrcPanoImage & src, int channel)
01077 {
01078     double mprad[6];
01079 
01080 //    double  imwidth = src.getSize().x;
01081 //    double  imheight= src.getSize().y;
01082 
01083     m_Stack.clear();
01084     m_srcTX = src.getSize().x/2.0;
01085     m_srcTY = src.getSize().y/2.0;
01086     m_destTX = src.getSize().x/2.0;
01087     m_destTY = src.getSize().y/2.0;
01088 
01089     if (src.getRadialDistortionCenterShift().x != 0.0) {
01090         AddTransform(&horiz, -src.getRadialDistortionCenterShift().x);
01091     }
01092 
01093     // shift optical center if needed
01094     if (src.getRadialDistortionCenterShift().y != 0.0) {
01095         AddTransform(&vert, -src.getRadialDistortionCenterShift().y);
01096     }
01097 
01098     if (src.getCorrectTCA() && (channel == 0 || channel == 2)) {
01099         for (int i=0; i < 4; i++) {
01100             if (channel == 0) {
01101                 // correct red channel (TCA)
01102                 mprad[3-i] = src.getRadialDistortionRed()[i];
01103             } else {
01104                 // correct blue channel (TCA)
01105                 mprad[3-i] = src.getRadialDistortionBlue()[i];
01106             }
01107         }
01108         mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y)  / 2.0;
01109         // calculate the correction radius.
01110         mprad[5] = CalcCorrectionRadius_copy(mprad);
01111 
01112         // radial correction if nonzero radial coefficients
01113         if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
01114             AddTransform (&inv_radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
01115         }
01116     }
01117 
01118     // green channel, always correct
01119     for (int i=0; i < 4; i++) {
01120         mprad[3-i] = src.getRadialDistortion()[i];
01121     }
01122     mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y)  / 2.0;
01123     // calculate the correction radius.
01124     mprad[5] = CalcCorrectionRadius_copy(mprad);
01125 
01126     // radial correction if nonzero radial coefficients
01127     if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
01128         AddTransform (&inv_radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
01129     }
01130 }
01131 
01132 
01134 void SpaceTransform::InitRadialCorrect(const SrcPanoImage & src, int channel)
01135 {
01136     double mprad[6];
01137 
01138 //    double  imwidth = src.getSize().x;
01139 //    double  imheight= src.getSize().y;
01140 
01141     m_Stack.clear();
01142     m_srcTX = src.getSize().x/2.0;
01143     m_srcTY = src.getSize().y/2.0;
01144     m_destTX = src.getSize().x/2.0;
01145     m_destTY = src.getSize().y/2.0;
01146 
01147     // green channel, always correct
01148     for (int i=0; i < 4; i++) {
01149         mprad[3-i] = src.getRadialDistortion()[i];
01150     }
01151     mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y)  / 2.0;
01152     // calculate the correction radius.
01153     mprad[5] = CalcCorrectionRadius_copy(mprad);
01154 
01155     // radial correction if nonzero radial coefficients
01156     if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
01157         AddTransform (&radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
01158         DEBUG_DEBUG("Init Radial (green): " 
01159                 << "g: " << mprad[0] << " " << mprad[1] << " " << mprad[2] 
01160                 << " " << mprad[3] << " " << mprad[4] << " " << mprad[5]);
01161     }
01162 
01163     if (src.getCorrectTCA() && (channel == 0 || channel == 2)) {
01164         for (int i=0; i < 4; i++) {
01165             if (channel == 0) {
01166                 // correct red channel (TCA)
01167                 mprad[3-i] = src.getRadialDistortionRed()[i];
01168             } else {
01169                 // correct blue channel (TCA)
01170                 mprad[3-i] = src.getRadialDistortionBlue()[i];
01171             }
01172         }
01173         mprad[4] = ( src.getSize().x < src.getSize().y ? src.getSize().x: src.getSize().y)  / 2.0;
01174         // calculate the correction radius.
01175         mprad[5] = CalcCorrectionRadius_copy(mprad);
01176 
01177         // radial correction if nonzero radial coefficients
01178         if ( mprad[0] != 1.0 || mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
01179             AddTransform (&radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
01180             DEBUG_DEBUG("Init Radial (channel " << channel << "): " 
01181                     << "g: " << mprad[0] << " " << mprad[1] << " " << mprad[2] 
01182                     << " " << mprad[3] << " " << mprad[4] << " " << mprad[5]);
01183         }
01184     }
01185 
01186     // shift optical center if needed
01187     if (src.getRadialDistortionCenterShift().y != 0.0) {
01188         AddTransform(&vert, src.getRadialDistortionCenterShift().y);
01189     }
01190     if (src.getRadialDistortionCenterShift().x != 0.0) {
01191         AddTransform(&horiz, src.getRadialDistortionCenterShift().x);
01192     }
01193 }
01194 
01198 void SpaceTransform::Init(
01199     const SrcPanoImage & image,
01200     const vigra::Diff2D &destSize,
01201     PanoramaOptions::ProjectionFormat destProj,
01202     double destHFOV )
01203 {
01204     //int       i;
01205     double      a, b;
01206     Matrix3 mpmt;
01207     double  mpdistance, mpscale[2], mprot[2], mprad[6];
01208     // double mpshear[2], mpperspect[2];
01209     double  mphorizontal, mpvertical;
01210 
01211     double  imhfov  = image.getHFOV();
01212     vigra::Size2D srcSize = image.getSize();
01213     double  imwidth = srcSize.x;
01214     double  imheight= srcSize.y;
01215     double  imyaw   = image.getYaw();
01216     double  impitch = image.getPitch();
01217     double  imroll  = image.getRoll();
01218     double  ima = image.getRadialDistortion()[0];
01219     double  imb = image.getRadialDistortion()[1];
01220     double  imc = image.getRadialDistortion()[2];
01221     double  imd = image.getRadialDistortionCenterShift().x;
01222     double  ime = image.getRadialDistortionCenterShift().y;
01223     double  img = image.getShear().x;
01224     double  imt = image.getShear().y;
01225     double  pnhfov  = destHFOV;
01226     double  pnwidth = destSize.x;
01227     SrcPanoImage::Projection srcProj = image.getProjection();
01228 //    double  pnheight= destSize.y;
01229 
01230     m_Stack.clear();
01231     m_srcTX = destSize.x/2.0;
01232     m_srcTY = destSize.y/2.0;    
01233     m_destTX = srcSize.x/2.0;
01234     m_destTY = srcSize.y/2.0;
01235 
01236     a = DEG_TO_RAD( imhfov );   // field of view in rad         
01237     b = DEG_TO_RAD( pnhfov );
01238 
01239     mpmt = SetMatrix(   -DEG_TO_RAD( impitch ),
01240                         0.0,
01241                         - DEG_TO_RAD( imroll ),
01242                         0 );
01243 
01244     if (destProj == PanoramaOptions::RECTILINEAR)       // rectilinear panorama
01245     {
01246         mpdistance = pnwidth / (2.0 * tan(b/2.0));
01247         if (srcProj == SrcPanoImage::RECTILINEAR)       // rectilinear image
01248         {
01249             mpscale[0] = (pnhfov / imhfov) * (a /(2.0 * tan(a/2.0))) * (imwidth/pnwidth) * 2.0 * tan(b/2.0) / b;
01250 
01251         }
01252         else //  pamoramic or fisheye image
01253         {
01254             mpscale[0] = (pnhfov / imhfov) * (imwidth/pnwidth) * 2.0 * tan(b/2.0) / b;
01255         }
01256     }
01257     else // equirectangular or panoramic or fisheye or other.
01258     {
01259         mpdistance = pnwidth / b;
01260         if(srcProj == SrcPanoImage::RECTILINEAR)        // rectilinear image
01261         {
01262             mpscale[0] = (pnhfov / imhfov) * (a /(2.0 * tan(a/2.0)))*( imwidth / pnwidth );
01263         }
01264         else //  pamoramic or fisheye image
01265         {
01266             mpscale[0] = (pnhfov / imhfov) * ( imwidth / pnwidth );
01267         }
01268     }
01269     mpscale[1]          = mpscale[0];
01270     // mpshear[0]               = img / imheight; // TODO : im->cP.shear_x / imheight;
01271     // mpshear[1]               = imt / imwidth; // TODO : im->cP.shear_y / imwidth;
01272     mprot[0]            = mpdistance * PI;                                                              // 180 in screenpoints
01273     mprot[1]            = -imyaw *  mpdistance * PI / 180.0;                    //    rotation angle in screenpoints
01274 
01275     // add radial correction
01276     mprad[3] = ima;
01277     mprad[2] = imb;
01278     mprad[1] = imc;
01279     mprad[0] = 1.0 - (ima+imb+imc);
01280     mprad[4] = ( imwidth < imheight ? imwidth : imheight)  / 2.0;
01281     // calculate the correction radius.
01282     mprad[5] = CalcCorrectionRadius_copy(mprad);
01283 
01284         /*for(i=0; i<4; i++)
01285                 mprad[i]        = im->cP.radial_params[color][i];
01286         mprad[5] = im->cP.radial_params[color][4];
01287 
01288         if( (im->cP.correction_mode & 3) == correction_mode_radial )
01289                 mp->rad[4]      = ( (double)( im->width < im->height ? im->width : im->height) ) / 2.0;
01290         else
01291                 mp->rad[4]      = ((double) im->height) / 2.0;*/
01292                 
01293     mphorizontal = imd;
01294     mpvertical = ime;
01295         //mp->horizontal        = im->cP.horizontal_params[color];
01296         //mp->vertical  = im->cP.vertical_params[color];
01297         
01298     //i = 0;
01299 
01300     switch (destProj)
01301     {
01302     case PanoramaOptions::RECTILINEAR :
01303         // Convert rectilinear to equirect
01304         AddTransform( &erect_rect, mpdistance );
01305         break;
01306 
01307     case PanoramaOptions::CYLINDRICAL:
01308         // Convert panoramic to equirect
01309         AddTransform( &erect_pano, mpdistance );
01310         break;
01311     case PanoramaOptions::EQUIRECTANGULAR:
01312         // do nothing... coordinates are already equirect
01313         break;
01314     //case PanoramaOptions::FULL_FRAME_FISHEYE:
01315     case PanoramaOptions::FULL_FRAME_FISHEYE:
01316         // Convert panoramic to sphere
01317         AddTransform( &erect_sphere_tp, mpdistance );
01318         break;
01319     case PanoramaOptions::STEREOGRAPHIC:
01320         AddTransform( &erect_stereographic, mpdistance );
01321         break;
01322     case PanoramaOptions::MERCATOR:
01323         AddTransform( &erect_mercator, mpdistance );
01324         break;
01325     case PanoramaOptions::TRANSVERSE_MERCATOR:
01326         AddTransform( &erect_transmercator, mpdistance );
01327         break;
01328 //    case PanoramaOptions::TRANSVERSE_CYLINDRICAL:
01329 //        AddTransform( &erect_transpano, mpdistance );
01330 //        break;
01331     case PanoramaOptions::SINUSOIDAL:
01332         AddTransform( &erect_sinusoidal, mpdistance );
01333         break;
01334     default:
01335         DEBUG_FATAL("Fatal error: Unknown projection " << destProj);
01336         break;
01337     }
01338 
01339     // Rotate equirect. image horizontally
01340     AddTransform( &rotate_erect, mprot[0], mprot[1] );
01341         
01342     // Convert spherical image to equirect.
01343     AddTransform( &sphere_tp_erect, mpdistance );
01344         
01345     // Perspective Control spherical Image
01346     AddTransform( &persp_sphere, mpmt, mpdistance );
01347 
01348     switch (srcProj)
01349     {
01350     case SrcPanoImage::RECTILINEAR :
01351         // Convert rectilinear to spherical
01352         AddTransform( &rect_sphere_tp, mpdistance);
01353         break;
01354     case SrcPanoImage::PANORAMIC :
01355         // Convert panoramic to spherical
01356         AddTransform( &pano_sphere_tp, mpdistance );
01357         break;
01358     case SrcPanoImage::EQUIRECTANGULAR:
01359         // Convert PSphere to spherical
01360         AddTransform( &erect_sphere_tp, mpdistance );
01361         break;
01362     default:
01363         // do nothing for CIRCULAR & FULL_FRAME_FISHEYE
01364         break;
01365     }
01366 
01367     // Scale Image
01368     AddTransform( &resize, mpscale[0], mpscale[1] );
01369 
01370     // radial correction if nonzero radial coefficients
01371     if ( mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
01372         AddTransform (&radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
01373     }
01374 
01375     // shift optical center if needed
01376     if (mpvertical != 0.0) {
01377         AddTransform(&vert, mpvertical);
01378     }
01379     if (mphorizontal != 0.0) {
01380         AddTransform(&horiz, mphorizontal);
01381     }
01382 
01383     /*if( im->cP.radial )
01384       {
01385                 switch( im->cP.correction_mode & 3 )
01386                 {
01387                         case correction_mode_radial:    SetDesc(stack[i],radial,mp->rad);         i++; break;
01388                         case correction_mode_vertical:  SetDesc(stack[i],vertical,mp->rad);   i++; break;
01389                         case correction_mode_deregister:SetDesc(stack[i],deregister,mp->rad); i++; break;
01390                 }
01391         }
01392         if (  im->cP.vertical)
01393         {
01394                 SetDesc(stack[i],vert,                          &(mp->vertical));       i++;
01395         }
01396         if ( im->cP.horizontal )
01397         {
01398                 SetDesc(stack[i],horiz,                         &(mp->horizontal)); i++;
01399         }
01400         if( im->cP.shear )
01401         {
01402                 SetDesc( stack[i],shear,                        mp->shear               ); i++;
01403         }
01404 
01405         stack[i].func  = (trfn)NULL;*/
01406 
01407 }
01408 
01409 void SpaceTransform::InitInv(
01410     const SrcPanoImage & image,
01411     const vigra::Diff2D &destSize,
01412     PanoramaOptions::ProjectionFormat destProj,
01413     double destHFOV )
01414 {
01415     double      a, b;
01416     Matrix3 mpmt;
01417     double  mpdistance, mpscale[2], mprot[2], mprad[6];
01418 //    double  mpshear[2], mpperspect[2];
01419     double mphorizontal, mpvertical;
01420 
01421     double  imhfov  = image.getHFOV();
01422     vigra::Size2D srcSize = image.getSize();
01423     double  imwidth = srcSize.x;
01424     double  imheight= srcSize.y;
01425     double  imyaw   = image.getYaw();
01426     double  impitch = image.getPitch();
01427     double  imroll  = image.getRoll();
01428     double  ima = image.getRadialDistortion()[0];
01429     double  imb = image.getRadialDistortion()[1];
01430     double  imc = image.getRadialDistortion()[2];
01431     double  imd = image.getRadialDistortionCenterShift().x;
01432     double  ime = image.getRadialDistortionCenterShift().y;
01433     SrcPanoImage::Projection srcProj = image.getProjection();
01434     
01435     double  pnhfov  = destHFOV;
01436     double  pnwidth = destSize.x;
01437 //    double  pnheight= destSize.y;
01438 
01439     m_Stack.clear();
01440     m_srcTX = destSize.x/2.0;
01441     m_srcTY = destSize.y/2.0;
01442     m_destTX = srcSize.x/2.0;
01443     m_destTY = srcSize.y/2.0;
01444 
01445 
01446     a =  DEG_TO_RAD( imhfov );  // field of view in rad         
01447     b =  DEG_TO_RAD( pnhfov );
01448 
01449     mpmt = SetMatrix(   DEG_TO_RAD( impitch ),
01450                         0.0,
01451                         DEG_TO_RAD( imroll ),
01452                         1 );
01453 
01454     if(destProj == PanoramaOptions::RECTILINEAR)        // rectilinear panorama
01455     {
01456         mpdistance      = pnwidth / (2.0 * tan(b/2.0));
01457         if(srcProj == SrcPanoImage::RECTILINEAR)        // rectilinear image
01458         {
01459             mpscale[0] = ( pnhfov/imhfov ) * (a /(2.0 * tan(a/2.0))) * ( imwidth/pnwidth ) * 2.0 * tan(b/2.0) / b;
01460         }
01461         else //  pamoramic or fisheye image
01462         {
01463             mpscale[0] = ( pnhfov/imhfov ) * ( imwidth/pnwidth ) * 2.0 * tan(b/2.0) / b;
01464         }
01465     }
01466     else // equirectangular or panoramic
01467     {
01468         mpdistance      = pnwidth / b;
01469         if(srcProj == SrcPanoImage::RECTILINEAR ) // rectilinear image
01470         {
01471             mpscale[0] = ( pnhfov/imhfov ) * (a /(2.0 * tan(a/2.0))) * ( imwidth/pnwidth );
01472         }
01473         else //  pamoramic or fisheye image
01474         {
01475             mpscale[0] = ( pnhfov/imhfov ) * ( imwidth/pnwidth );
01476         }
01477     }
01478     // mpshear[0]       = 0.0f; // TODO -im->cP.shear_x / im->height;
01479     // mpshear[1]       = 0.0f; // -im->cP.shear_y / im->width;
01480         
01481     mpscale[0] = 1.0 / mpscale[0];
01482     mpscale[1] = mpscale[0];
01483 
01484     // principal point shift
01485     mphorizontal = - imd;
01486     mpvertical = - ime;
01487 
01488     // radial correction parameters
01489     mprad[3] = ima;
01490     mprad[2] = imb;
01491     mprad[1] = imc;
01492     mprad[0] = 1.0 - (ima+imb+imc);
01493     mprad[4] = ( imwidth < imheight ? imwidth : imheight)  / 2.0;
01494     // calculate the correction radius.
01495     mprad[5] = CalcCorrectionRadius_copy(mprad);
01496 
01497 
01498     /*mp->horizontal    = -im->cP.horizontal_params[color];
01499         mp->vertical    = -im->cP.vertical_params[color];
01500         for(i=0; i<4; i++)
01501                 mp->rad[i]      = im->cP.radial_params[color][i];
01502         mp->rad[5] = im->cP.radial_params[color][4];
01503         
01504         switch( im->cP.correction_mode & 3 )
01505         {
01506                 case correction_mode_radial: mp->rad[4] = ((double)(im->width < im->height ? im->width : im->height) ) / 2.0;break;
01507                 case correction_mode_vertical:
01508                 case correction_mode_deregister: mp->rad[4] = ((double) im->height) / 2.0;break;
01509         }
01510         */
01511 
01512     mprot[0]    = mpdistance * PI;                                                              // 180 in screenpoints
01513     mprot[1]    = imyaw *  mpdistance * PI / 180.0;                     //    rotation angle in screenpoints
01514 
01515     //mp->perspect[0] = (void*)(mp->mt);
01516     //mp->perspect[1] = (void*)&(mp->distance);
01517 
01518     // Perform radial correction
01519     //if( im->cP.shear )
01520     //{
01521     //  SetDesc( stack[i],shear,                        mp->shear               ); i++;
01522     //}
01523     //  
01524 
01525     // principal point shift
01526     if (mphorizontal != 0.0) {
01527         AddTransform(&horiz, mphorizontal);
01528     }
01529     if (mpvertical != 0.0) {
01530         AddTransform(&vert, mpvertical);
01531     }
01532 
01533     // radial distortion
01534     if ( mprad[1] != 0.0 || mprad[2] != 0.0 || mprad[3] != 0.0) {
01535         AddTransform (&inv_radial, mprad[0], mprad[1], mprad[2], mprad[3], mprad[4], mprad[5]);
01536     }
01537         
01538     // Scale image
01539     AddTransform( &resize, mpscale[0], mpscale[1] );
01540                 
01541     switch (srcProj)
01542     {
01543     case SrcPanoImage::RECTILINEAR :
01544         // rectilinear image
01545         AddTransform( &sphere_tp_rect, mpdistance );
01546         break;
01547     case SrcPanoImage::PANORAMIC :
01548         // Convert panoramic to spherical
01549         AddTransform( &sphere_tp_pano, mpdistance );
01550         break;
01551     case SrcPanoImage::EQUIRECTANGULAR:
01552         // Convert PSphere to spherical
01553         AddTransform( &sphere_tp_erect, mpdistance );
01554         break;
01555     default:
01556         // do nothing for fisheye lenses
01557         break;
01558     }
01559 
01560     // Perspective Control spherical Image
01561     AddTransform( &persp_sphere, mpmt, mpdistance );
01562     // Convert spherical image to equirect.
01563     AddTransform( &erect_sphere_tp, mpdistance );
01564     // Rotate equirect. image horizontally
01565     AddTransform( &rotate_erect, mprot[0], mprot[1] );
01566 
01567     switch (destProj)
01568     {
01569     case PanoramaOptions::RECTILINEAR :
01570         // Convert rectilinear to spherical
01571         AddTransform( &rect_erect, mpdistance);
01572         break;
01573     case PanoramaOptions::CYLINDRICAL :
01574         // Convert panoramic to spherical
01575         AddTransform( &pano_erect, mpdistance );
01576         break;
01577     case PanoramaOptions::EQUIRECTANGULAR:
01578         break;
01579     case PanoramaOptions::FULL_FRAME_FISHEYE:
01580         // Convert PSphere to spherical
01581         AddTransform( &sphere_tp_erect, mpdistance );
01582         break;
01583     case PanoramaOptions::STEREOGRAPHIC:
01584         // Convert PSphere to spherical
01585         AddTransform( &stereographic_erect, mpdistance );
01586         break;
01587     case PanoramaOptions::MERCATOR:
01588         AddTransform( &mercator_erect, mpdistance );
01589         break;
01590     case PanoramaOptions::TRANSVERSE_MERCATOR:
01591         AddTransform( &transmercator_erect, mpdistance );
01592         break;
01593 //    case PanoramaOptions::TRANSVERSE_CYLINDRICAL:
01594 //        AddTransform( &transpano_erect, mpdistance );
01595 //        break;
01596     case PanoramaOptions::SINUSOIDAL:
01597         AddTransform( &transpano_erect, mpdistance );
01598         break;
01599     default:
01600         DEBUG_FATAL("Fatal error: Unknown projection " << destProj);
01601         break;
01602     }
01603 }
01604 
01605 //
01606 void SpaceTransform::createTransform(const SrcPanoImage & src, const PanoramaOptions & dest)
01607 {
01608     Init(src,
01609          vigra::Size2D(dest.getWidth(), dest.getHeight()),
01610          dest.getProjection(),
01611          dest.getHFOV());
01612 }
01613 
01614 //
01615 void SpaceTransform::createInvTransform(const SrcPanoImage & src, const PanoramaOptions & dest)
01616 {
01617     InitInv(src,
01618             vigra::Size2D(dest.getWidth(), dest.getHeight()),
01619             dest.getProjection(),
01620             dest.getHFOV());
01621 }
01622 
01623 void SpaceTransform::createTransform(const PanoramaData & pano, unsigned int imgNr,
01624                      const PanoramaOptions & dest,
01625                      vigra::Diff2D srcSize)
01626 {
01627     const SrcPanoImage & img = pano.getImage(imgNr);
01628     if (srcSize.x == 0 && srcSize.y == 0)
01629     {
01630         srcSize = img.getSize();
01631     }
01632     Init(pano.getImage(imgNr),
01633          vigra::Diff2D(dest.getWidth(), dest.getHeight()),
01634          dest.getProjection(), dest.getHFOV() );
01635 }
01636 
01637 
01638 // create image->pano transformation
01639 void SpaceTransform::createInvTransform(const PanoramaData & pano, unsigned int imgNr,
01640                         const PanoramaOptions & dest,
01641                         vigra::Diff2D srcSize)
01642 {
01643     const SrcPanoImage & img = pano.getImage(imgNr);
01644     if (srcSize.x == 0 && srcSize.y == 0)
01645     {
01646         srcSize = img.getSize();
01647     }
01648     InitInv(pano.getImage(imgNr),
01649             vigra::Diff2D(dest.getWidth(), dest.getHeight()),
01650             dest.getProjection(), dest.getHFOV() );
01651 }
01652 
01654 void SpaceTransform::createTransform(const vigra::Diff2D & srcSize,
01655                      const VariableMap & srcVars,
01656                      Lens::LensProjectionFormat srcProj,
01657                      const vigra::Diff2D &destSize,
01658                      PanoramaOptions::ProjectionFormat destProj,
01659                      double destHFOV)
01660 {
01661     SrcPanoImage src_image;
01662     src_image.setSize(vigra::Size2D(srcSize.x, srcSize.y));
01663     src_image.setProjection((SrcPanoImage::Projection)srcProj);
01664     for (VariableMap::const_iterator i = srcVars.begin(); i != srcVars.end(); ++i)
01665     {
01666         src_image.setVar((*i).first, (*i).second.getValue());
01667     }
01668     Init(src_image, destSize, destProj, destHFOV);
01669 }
01670 
01671 // create image->pano transformation
01673 void SpaceTransform::createInvTransform(const vigra::Diff2D & srcSize,
01674                         const VariableMap & srcVars,
01675                         Lens::LensProjectionFormat srcProj,
01676                         const vigra::Diff2D & destSize,
01677                         PanoramaOptions::ProjectionFormat destProj,
01678                         double destHFOV)
01679 {
01680     // make a SrcPanoImage with the necessary data.
01681     SrcPanoImage src_image;
01682     src_image.setSize(vigra::Size2D(srcSize.x, srcSize.y));
01683     src_image.setProjection((SrcPanoImage::Projection)srcProj);
01684     for (VariableMap::const_iterator i = srcVars.begin(); i != srcVars.end(); ++i)
01685     {
01686         src_image.setVar((*i).first, (*i).second.getValue());
01687     }
01688     InitInv(src_image, destSize, destProj, destHFOV);
01689 }
01690 
01691 
01692 //
01693 bool SpaceTransform::transform(hugin_utils::FDiff2D& dest, const hugin_utils::FDiff2D & src) const
01694 {
01695         double xd = src.x, yd = src.y;
01696         std::vector<fDescription>::const_iterator tI;
01697         
01698     dest.x = xd;
01699     dest.y = yd;
01700     for (tI = m_Stack.begin(); tI != m_Stack.end(); ++tI)
01701     {
01702         (tI->func)( xd, yd, &dest.x, &dest.y, tI->param );
01703         xd = dest.x;    
01704         yd = dest.y;
01705     }
01706     return true;
01707 }
01708 
01709 //
01710 bool SpaceTransform::transformImgCoord(double & x_dest, double & y_dest, double x_src, double y_src) const
01711 {
01712     hugin_utils::FDiff2D dest, src;
01713         src.x = x_src - m_srcTX + 0.5;
01714         src.y = y_src - m_srcTY + 0.5;
01715         transform( dest, src );
01716         x_dest = dest.x + m_destTX - 0.5;
01717         y_dest = dest.y + m_destTY - 0.5;
01718         return true;
01719 }
01720 
01721 
01722 } // namespace
01723 } // namespace

Generated on 7 Dec 2016 for Hugintrunk by  doxygen 1.4.7