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

Generated on Mon Sep 1 01:25:40 2014 for Hugintrunk by  doxygen 1.3.9.1