OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

GKTransformNode.cxx

Go to the documentation of this file.
00001 /* ========================================================================
00002  * Copyright (c) 2006,
00003  * Institute for Computer Graphics and Vision
00004  * Graz University of Technology
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are
00009  * met:
00010  *
00011  * Redistributions of source code must retain the above copyright notice,
00012  * this list of conditions and the following disclaimer.
00013  *
00014  * Redistributions in binary form must reproduce the above copyright
00015  * notice, this list of conditions and the following disclaimer in the
00016  * documentation and/or other materials provided with the distribution.
00017  *
00018  * Neither the name of the Graz University of Technology nor the names of
00019  * its contributors may be used to endorse or promote products derived from
00020  * this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
00023  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
00024  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
00025  * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
00026  * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00027  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00028  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00029  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00030  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00031  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033  * ========================================================================
00034  * PROJECT: OpenTracker
00035  * ======================================================================== */
00042 /* ======================================================================= */
00043 
00044 // this will remove the warning 4786
00045 #include "../tool/disable4786.h"
00046 
00047 #include "GKTransformNode.h"
00048 #include <math.h>
00049 
00050 
00051 #ifndef OT_NO_GK_SUPPORT
00052 
00053 
00054 namespace ot {
00055 
00056     GKTransformNode::GKTransformNode( double a_ , double b_, double m_,
00057                                       double alpha_, double beta_, double gamma_, double delta_,
00058                                       Mode mode_ ) :
00059         a( a_ ),
00060         b( b_ ),
00061         meridian( m_ * MathUtils::GradToRad ),  // we get the meridian in degrees
00062         alpha( alpha_ ),
00063         beta( beta_ ),
00064         gamma( gamma_ ),
00065         delta( delta_ ),
00066         mode( mode_ )
00067     {}
00068 
00069     Event* GKTransformNode::transformEvent( Event* event)
00070     {
00071         // the zero meridian of the GK map projection goes through Ferro,
00072         // a Canarian island 17 40' left of Greenwhich zero meridian.
00073         const double ferro = (17.0 + 40.0 / 60.0) * MathUtils::GradToRad;
00074         // there are corrections applied to the resulting x (north) value
00075         // 5000 km ( 5000000 m ) are subtracted
00076         const double falseNorthing = 5000000.0;
00077 
00078         if( to == mode )
00079         {
00080             double B = event->getPosition()[0];
00081             double sinB = sin(B);
00082             double cosB = cos(B);
00083             double L = event->getPosition()[1];
00084             double dL = ( L + ferro - meridian );
00085 
00086             double Sm = alpha * B / MathUtils::GradToRad - beta * sin(2.0 * B) + gamma * sin(4.0 * B) - delta * sin(6.0 * B);
00087 
00088             double e2 = 1.0 - (b*b) / (a*a);
00089             double N = a / (sqrt( 1.0 - e2 * sinB * sinB ));
00090             double eta2 = ((a*a) / (b*b) - 1)*cosB*cosB;
00091             double t = tan( B );
00092             double corr_x = 1.0 + (dL*dL)*(cosB*cosB)*(5.0 - t*t + 9.0*eta2 + 4.0*eta2*eta2) / 12.0 +
00093                 pow(dL,4.0)*pow(cosB,4.0)*(61.0-58.0*t*t+pow(t,4.0)+270.0*eta2 - 330.0*t*t*eta2) / 360.0;
00094             double corr_y = 1.0 + (dL*dL)*(cosB*cosB)*(1.0 - t*t + eta2) / 6.0 +
00095                 pow(dL,4.0)*pow(cosB,4.0)*(5.0 - 18.0*t*t + pow(t,4.0) + 14.0*eta2 - 58.0*t*t*eta2) / 120.0;
00096             double x = Sm + N * dL * dL * sinB * cosB * corr_x / 2.0 - falseNorthing;
00097             double y = N * dL * cosB * corr_y;
00098 
00099 
00100             localEvent.getPosition()[0] = (float)x;
00101             localEvent.getPosition()[1] = (float)y;
00102         }
00103         else
00104         {
00105             double x = event->getPosition()[0];
00106             double y = event->getPosition()[1];
00107             double xa = x * MathUtils::GradToRad / alpha;
00108             double Bf = (xa + beta*sin(2*xa) + gamma*sin(4*xa) + delta*sin(6*xa)) * MathUtils::GradToRad;
00109             double t = tan(Bf);
00110             double t2 = t*t;
00111             double e2 = 1 - (b*b) / (a*a);
00112             double eta2 = ((a*a) / (b*b) - 1)*cos(Bf)*cos(Bf);
00113             double W = sqrt(1 - e2 *sin(Bf)*sin(Bf));
00114             double N = a / W;
00115             double M = b*b / (a*W*W*W);
00116             double corr_b = 1 - (y*y)*(5 + 3*t2 + eta2 - 9*t2*eta2)/(12*N*N) +
00117                 (y*y*y*y)*(61+90*t2+45*t2*t2)/(360*N*N*N*N);
00118             double corr_l = 1 - (y*y)*(1 + 2*t2 + eta2)/(6*N*N) +
00119                 (y*y*y*y)*(5 + 28*t2 + 24*t2*t2)/(120*N*N*N*N);
00120             double B = Bf - (y*y*t)*corr_b/(2*M*N);
00121             double L = y*corr_l/(N*cos(Bf)) + meridian - ferro;
00122 
00123             localEvent.getPosition()[0] = (float)B;
00124             localEvent.getPosition()[1] = (float)L;
00125         }
00126 
00127         // height over the ellipsoid surface is identical to the height over the map
00128         localEvent.getPosition()[2] = event->getPosition()[2];
00129 
00130         // copy the rest over
00131         // we don't deal with orientation so far...
00132         localEvent.copyAllButStdAttr(*event);
00133         localEvent.getOrientation() = event->getOrientation();
00134 
00135         localEvent.getConfidence() = event->getConfidence();
00136         localEvent.getButton() = event->getButton();
00137         localEvent.time = event->time;
00138         return & localEvent;
00139     }
00140 
00141 } // namespace ot
00142 
00143 
00144 #else
00145 #pragma message(">>> OT_NO_GK_SUPPORT")
00146 #endif //OT_NO_GK_SUPPORT
00147 
00148 /* 
00149  * ------------------------------------------------------------
00150  *   End of GKTransformNode.cxx
00151  * ------------------------------------------------------------
00152  *   Automatic Emacs configuration follows.
00153  *   Local Variables:
00154  *   mode:c++
00155  *   c-basic-offset: 4
00156  *   eval: (c-set-offset 'substatement-open 0)
00157  *   eval: (c-set-offset 'case-label '+)
00158  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00159  *   eval: (setq indent-tabs-mode nil)
00160  *   End:
00161  * ------------------------------------------------------------ 
00162  */

copyright (c) 2006 Graz University of Technology