SpaceTransform.cpp

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

Generated on 29 Jul 2015 for Hugintrunk by  doxygen 1.4.7