OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

P5GloveModule.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 "P5GloveModule.h"
00048 
00049 #ifdef USE_P5GLOVE
00050 
00051 #include <stdio.h>
00052 #include <string>
00053 #ifdef WIN32
00054 #include <iostream>    // VisualC++ uses STL based IOStream lib
00055 #else
00056 #include <iostream.h>
00057 #endif
00058 
00059 //using namespace std;
00060 
00061 #include <math.h>
00062 #include "P5GloveSource.h"
00063 
00064 namespace ot {
00065 
00066     //These variables contain the actual x, Y, Z position of the cursor
00067     int nXPos = 0, nYPos = 0, nZPos = 0;
00068 
00069     //These variables contain the frame to frame deltas of the cursor
00070     float fXMickey = 0.0f, fYMickey = 0.0f, fZMickey = 0.0f;
00071 
00072     //These variables contain the filtered oreintation information
00073     float fAbsYawPos, fAbsPitchPos, fAbsRollPos;
00074     float fRelYawPos, fRelPitchPos, fRelRollPos;
00075 
00076 
00077     P5GloveModule::P5GloveModule() : Module(), NodeFactory()
00078     {
00079         P5device=new CP5DLL();
00080     }
00081 
00082     // Destructor method
00083     P5GloveModule::~P5GloveModule()
00084     {
00085     nodes.clear();
00086     }
00087 
00088     // initializes trackers
00089     void P5GloveModule::init(StringTable& attributes, ConfigNode * localTree)
00090     {
00091         if( attributes.get("P5Id", &P5Id ) == 0 )
00092             P5Id = 0;
00093         printf("P5 Glove is configured with id %d\n",P5Id);
00094 
00095         Module::init( attributes, localTree );
00096     }
00097 
00098     // This method is called to construct a new Node.
00099     Node * P5GloveModule::createNode( const string& name, StringTable& attributes)
00100     {
00101         if( name.compare("P5GloveSource") == 0 )
00102         {       
00103             int num;
00104             int finger;
00105             num = attributes.get("finger", &finger);
00106             if (num == 0) finger=0;
00107             else if( finger < 0 || finger > 4)
00108             {
00110                 printf("Finger index must be between 0 and 4 \n");
00111                 return NULL;
00112             }
00113             P5GloveSource *source = new P5GloveSource(finger);
00114             source->event.getConfidence() = 1.0f;
00115             nodes.push_back( source );
00116             printf("Build P5GloveSource node for finger %d \n",finger);
00117             initialized = 1;
00118             return source;
00119         }
00120         return NULL;
00121     }
00122 
00123     // opens P5Glove library
00124     void P5GloveModule::start()
00125     {
00126         if( isInitialized() == 1 )
00127     {
00128             // The P5 can always be initialized, no matter if it is turned on or off
00129             // The P5_Init() method always returns 1
00130             // P5_Init() probably only checks if the device is registered in Windows.
00131             // printf("P5 status %d \n",P5device->P5_Init());
00132             if (P5device->P5_Init() != (P5BOOL)true)
00133         {
00134                 printf("No P5 detected...\n");
00135             }
00136         else
00137         {
00138                 printf("P5 Found...\n");
00139             P5device->P5_SetMouseEvent(P5Id, false);
00140         }
00141         }
00142     
00143     printf("P5Glove started\n");
00144     }
00145 
00146     // closes P5Glove library
00147     void P5GloveModule::close()
00148     {
00149     // if P5 glove is turned ON and actually used by stb, than mouseEvent must be set back, 
00150     // otherwise calling SetMouseEvent would cause a crash on exit
00151     if( isInitialized() == 1 )
00152             P5device->P5_SetMouseEvent(0, true);
00153     P5device->P5_Close();
00154     printf("Closing P5Glove \n");
00155     }
00156 
00157     // pushes events into the tracker tree.
00158     void P5GloveModule::pushEvent()
00159     {
00160         if( isInitialized() == 1 )
00161         {
00162             for( NodeVector::iterator it = nodes.begin(); it != nodes.end(); it++ )
00163             {
00164                 P5GloveSource *source = (P5GloveSource *)(*it);
00165             if( P5device->m_P5Devices != NULL )
00166             {
00167                     //              P5Motion_InvertMouse(P5MOTION_INVERTAXIS, P5MOTION_NORMALAXIS, P5MOTION_NORMALAXIS);
00168                     //              P5Motion_SetClipRegion((-dwHeight), dwHeight, (-dwWidth), dwWidth, 0, 1000);
00169 
00170                     P5Motion_Process();
00171 
00172                     // Rotation (Roll, Pitch, Yaw) is internally presented in Degrees
00173                     // Must convert it to radiant to pass it to the eulerToQuaternion conversion routine
00174                     // pure data without filtering would be 
00175                     // P5device->m_P5Devices[P5Id].m_froll, P5device->m_P5Devices[P5Id].m_fpitch
00176                     // P5device->m_P5Devices[P5Id].m_fyaw
00177                     MathUtils::eulerToQuaternion(fAbsRollPos*MathUtils::Pi/180, 
00178                                                  fAbsPitchPos*MathUtils::Pi/180, 
00179                                                  fAbsYawPos*MathUtils::Pi/180,
00180                                                  source->event.orientation);
00181                 }
00182 
00183                 source->event.getPosition()[0] = fFilterX;
00184                 source->event.getPosition()[1] = fFilterY;
00185                 source->event.getPosition()[2] = fFilterZ;
00186 
00190             
00191             
00192                 //            if (it == nodes.begin() && buttonEvent!=glove->buttonEvent()){
00193                 if (it == nodes.begin() && 
00194                     P5device->m_P5Devices[P5Id].m_byBendSensor_Data[P5_INDEX]>BEND_THRESHOLD)
00195                 {
00196                     source->event.getButton() = 1;
00197                 }
00198                 else 
00199                     source->event.getButton() = 0;
00200 
00201                 //            source->event.getButton() = 0;
00202 
00203                 source->event.timeStamp();
00204                 source->updateObservers( source->event );           
00205             }
00206     }
00207     }
00208 
00209 
00210     /***********************************************
00211 Function: P5Motion_SetClipRegion()
00212 Use: Call this function to initialize a clipping region
00213 Parameters: int xstart  - low side xclip point
00214             int xend    - high side xclip point
00215             int ystart  - low side yclip point
00216             int yend    - high side yclip point
00217             int zstart  - low side zclip point
00218             int zend    - high side zclip point
00219     ***********************************************/
00220     int nxclipstart = 0, nxclipend = 1024, nyclipstart = 0, nyclipend = 768, nzclipstart = 0, nzclipend = 1024;
00221 
00222     void P5GloveModule::P5Motion_SetClipRegion(int xstart, int xend, int ystart, int yend, int zstart, int zend)
00223     {
00224     nxclipstart = xstart;
00225     nxclipend = xend;
00226     nyclipstart = ystart;
00227     nyclipend = yend;
00228     nzclipstart = zstart;
00229     nzclipend = zend;
00230     }
00231 
00232     /***********************************************
00233 Function: P5Motion_InvertMouse()
00234 Use: Call this function to flip an axis if necessary
00235 Parameters: int xaxis   - P5MOTION_NORMALAXIS or P5MOTION_INVERTAXIS
00236             int yaxis   - P5MOTION_NORMALAXIS or P5MOTION_INVERTAXIS
00237             int zaxis   - P5MOTION_NORMALAXIS or P5MOTION_INVERTAXIS
00238     ***********************************************/
00239     int nxinvert = 0, nyinvert = 0, nzinvert = 0;
00240 
00241     void P5Motion_InvertMouse(int xaxis, int yaxis, int zaxis)
00242     {
00243     nxinvert = xaxis;
00244     nyinvert = yaxis;
00245     nzinvert = zaxis;
00246     }
00247 
00248     /***********************************************
00249 Function: P5Motion_FilterXYZ()
00250 Use: Internal Function.  Used to filter XYZ Data
00251 Parameter: None
00252     ***********************************************/
00253     float fXPos[P5MOTION_XYZFILTERSIZE], fYPos[P5MOTION_XYZFILTERSIZE], fZPos[P5MOTION_XYZFILTERSIZE];
00254     //float fFilterX, fFilterY, fFilterZ;
00255 
00256     void P5GloveModule::P5Motion_FilterXYZ()
00257     {
00258     static int firsttime = 1;
00259     if (P5device != NULL)
00260     {
00261             if (firsttime==1)
00262             {
00263                 //dont process anything on the first call, just init our filter arrays
00264                 for (int i=0; i<P5MOTION_XYZFILTERSIZE; i++)
00265                 {
00266                     fXPos[i] = P5device->m_P5Devices[P5Id].m_fx;
00267                     fYPos[i] = P5device->m_P5Devices[P5Id].m_fy;
00268                     fZPos[i] = P5device->m_P5Devices[P5Id].m_fz;
00269                 }
00270 
00271                 firsttime = 0;
00272             }
00273             else
00274             {
00275                 //We are going to implement a dynamic filter, which will flush the filter array values at different rates
00276                 //based on the rate of change of the users hand.  This will allow for greater latency of motion.
00277                 //The setpoint determines the number of pixel motion that will flush the entire filter.
00278                 //The idea is when the user doensnt move much, we average alot of frames, but during fast motion, we
00279                 //average fewer and fewer frames in order to reduce latency
00280 
00281 #define FLUSH_SETPOINT  30.0f
00282 
00283                 float xflushsize, yflushsize, zflushsize;
00284                 int i, j;
00285             
00286                 //Lets determine the number of frames we intend to average together
00287 
00288                 xflushsize = fabs(P5device->m_P5Devices[P5Id].m_fx - fXPos[P5MOTION_XYZFILTERSIZE-1])/2.0f;
00289                 xflushsize *= P5MOTION_XYZFILTERSIZE/FLUSH_SETPOINT;
00290                 xflushsize = floor(xflushsize+1.0f);
00291                 if (xflushsize>(P5MOTION_XYZFILTERSIZE-1))
00292                     xflushsize = P5MOTION_XYZFILTERSIZE-1;
00293 
00294                 yflushsize = fabs(P5device->m_P5Devices[P5Id].m_fy - fYPos[P5MOTION_XYZFILTERSIZE-1])/2.0f;
00295                 yflushsize *= P5MOTION_XYZFILTERSIZE/FLUSH_SETPOINT;
00296                 yflushsize = floor(yflushsize+1.0f);
00297                 if (yflushsize>(P5MOTION_XYZFILTERSIZE-1))
00298                     yflushsize = P5MOTION_XYZFILTERSIZE-1;
00299 
00300                 zflushsize = fabs(P5device->m_P5Devices[P5Id].m_fz - fZPos[P5MOTION_XYZFILTERSIZE-1])/2.0f;
00301                 zflushsize *= P5MOTION_XYZFILTERSIZE/FLUSH_SETPOINT;
00302                 zflushsize = floor(zflushsize+1.0f);
00303                 if (zflushsize>(P5MOTION_XYZFILTERSIZE-1))
00304                     zflushsize = P5MOTION_XYZFILTERSIZE-1;
00305 
00306                 //flush the array based on the number of values determined before.
00307                 for (j=0; j<(int)(xflushsize); j++)
00308                 {
00309                     for (i=0; i<(P5MOTION_XYZFILTERSIZE-1); i++)
00310                     {
00311                         fXPos[i] = fXPos[i+1];
00312                     }
00313                     fXPos[P5MOTION_XYZFILTERSIZE-1] = P5device->m_P5Devices[P5Id].m_fx;
00314                 }
00315 
00316                 for (j=0; j<(int)(yflushsize); j++)
00317                 {
00318                     for (i=0; i<(P5MOTION_XYZFILTERSIZE-1); i++)
00319                     {
00320                         fYPos[i] = fYPos[i+1];
00321                     }
00322                     fYPos[P5MOTION_XYZFILTERSIZE-1] = P5device->m_P5Devices[P5Id].m_fy;
00323                 }
00324 
00325                 for (j=0; j<(int)(zflushsize); j++)
00326                 {
00327                     for (i=0; i<(P5MOTION_XYZFILTERSIZE-1); i++)
00328                     {
00329                         fZPos[i] = fZPos[i+1];
00330                     }
00331                     fZPos[P5MOTION_XYZFILTERSIZE-1] = P5device->m_P5Devices[P5Id].m_fz;
00332                 }
00333 
00334             }
00335 
00336             //Average all the values in the filter to smoothen the data
00337             fFilterX = 0.0f;
00338             fFilterY = 0.0f;
00339             fFilterZ = 0.0f;
00340             for (int i=0; i<P5MOTION_XYZFILTERSIZE; i++)
00341             {
00342                 //we are going to divide the values to get rid of some jitter
00343                 fFilterX += fXPos[i]/2.0f;
00344                 fFilterY += fYPos[i]/2.0f;
00345                 fFilterZ += fZPos[i]/2.0f;
00346             }
00347             fFilterX /= P5MOTION_XYZFILTERSIZE;
00348             fFilterY /= P5MOTION_XYZFILTERSIZE;
00349             fFilterZ /= P5MOTION_XYZFILTERSIZE;
00350     }
00351     }
00352 
00353     /***********************************************
00354 Function: P5Motion_FilterYPR()
00355 Use: Internal Function.  Used to filter Orientation Data
00356 Parameter: None
00357     ***********************************************/
00358     float fYaw[P5MOTION_YPRFILTERSIZE], fPitch[P5MOTION_YPRFILTERSIZE], fRoll[P5MOTION_YPRFILTERSIZE];
00359     float fFilterYaw, fFilterPitch, fFilterRoll;
00360 
00361     void P5GloveModule::P5Motion_FilterYPR()
00362     {
00363     static int firsttime = 1;
00364     if (P5device != NULL)
00365     {
00366             if (firsttime==1)
00367             {
00368                 //dont process anything on the first call, just init our filter arrays
00369                 for (int i=0; i<P5MOTION_YPRFILTERSIZE; i++)
00370                 {
00371                     fYaw[i] = P5device->m_P5Devices[P5Id].m_fyaw;
00372                     fPitch[i] = P5device->m_P5Devices[P5Id].m_fpitch;
00373                     fRoll[i] = P5device->m_P5Devices[P5Id].m_froll;
00374                 }
00375 
00376                 firsttime = 0;
00377             }
00378             else
00379             {
00380                 for (int i=0; i<(P5MOTION_YPRFILTERSIZE-1); i++)
00381                 {
00382                     fYaw[i] = fYaw[i+1];
00383                     fPitch[i] = fPitch[i+1];
00384                     fRoll[i] = fRoll[i+1];
00385                 }
00386                 fYaw[P5MOTION_YPRFILTERSIZE-1] = P5device->m_P5Devices[P5Id].m_fyaw;
00387                 fPitch[P5MOTION_YPRFILTERSIZE-1] = P5device->m_P5Devices[P5Id].m_fpitch;
00388                 fRoll[P5MOTION_YPRFILTERSIZE-1] = P5device->m_P5Devices[P5Id].m_froll;
00389 
00390             }
00391 
00392             //Average all the values in the filter to smoothen the data
00393             fFilterYaw = 0.0f;
00394             fFilterPitch = 0.0f;
00395             fFilterRoll = 0.0f;
00396             for (int i=0; i<P5MOTION_YPRFILTERSIZE; i++)
00397             {
00398                 fFilterYaw += fYaw[i];
00399                 fFilterPitch += fPitch[i];
00400                 fFilterRoll += fRoll[i];
00401             }
00402             fFilterYaw /= P5MOTION_YPRFILTERSIZE;
00403             fFilterPitch /= P5MOTION_YPRFILTERSIZE;
00404             fFilterRoll /= P5MOTION_YPRFILTERSIZE;
00405 
00406     }
00407     }
00408 
00409     /***********************************************
00410 Function: P5Motion_Process()
00411 Use: Processes the XYZ motion information every frame.  Call this function
00412      to calculate a pointer's position and oreintation with filtering and acceleration
00413 Parameter: None
00414     ***********************************************/
00415     void P5GloveModule::P5Motion_Process()
00416     {
00417     //XYZ information first
00418     static float fLastXpos = 0.0f, fLastYpos = 0.0f, fLastZpos = 0.0f;
00419 
00420     P5Motion_FilterXYZ();
00421     //apply axis invertion if required and calculate the delta from last frame
00422     fXMickey = (fLastXpos - fFilterX) * nxinvert;
00423     fYMickey = (fLastYpos - fFilterY) * nyinvert;
00424     fZMickey = (fLastZpos - fFilterZ) * nzinvert;
00425 
00426 #define COEFF1  0.0042f
00427 #define COEFF2  1.2403f
00428 
00429     //this is a third degree polynomial acceleration function.. seems to give the best results
00430     //by trial and error
00431     fXMickey = COEFF1*fXMickey*fXMickey*fXMickey + COEFF2*fXMickey;
00432     fYMickey = COEFF1*fYMickey*fYMickey*fYMickey + COEFF2*fYMickey;
00433     fZMickey = COEFF1*fZMickey*fZMickey*fZMickey + COEFF2*fZMickey;
00434 
00435     //A step function gain to speed up the high end motion for faster travel across the screen
00436     if ( fabs(fXMickey) > 3.0f)
00437             fXMickey *= 4.0f;
00438     else
00439             fXMickey *= 2.0f;
00440 
00441         if ( fabs(fYMickey) > 3.0f)
00442             fYMickey *= 4.0f;
00443     else
00444             fYMickey *= 2.0f;
00445 
00446     if ( fabs(fZMickey) > 3.0f)
00447             fZMickey *= 4.0f;
00448     else
00449             fZMickey *= 2.0f;
00450 
00451         nXPos += (int)(fXMickey);
00452     nYPos += (int)(fYMickey);
00453     nZPos += (int)(fZMickey);
00454 
00455     //Clip the data to fit the clipping region
00456     if (nXPos > nxclipend)
00457             nXPos = nxclipend;
00458     else if (nXPos < nxclipstart)
00459             nXPos = nxclipstart;
00460 
00461         if (nYPos > nyclipend)
00462             nYPos = nyclipend;
00463     else if (nYPos < nyclipstart)
00464             nYPos = nyclipstart;
00465 
00466     if (nZPos > nzclipend)
00467             nZPos = nzclipend;
00468     else if (nZPos < nzclipstart)
00469             nZPos = nzclipstart;
00470 
00471     fLastXpos = fFilterX;
00472     fLastYpos = fFilterY;
00473     fLastZpos = fFilterZ;
00474 
00475     //Lets process Yaw, Pitch, Roll information now
00476     P5Motion_FilterYPR();
00477 
00478     //Only prcess YPR information if a fist is made.. like grabbing the object
00479     //This is just an example gesture, and can be changed to be anything at all
00480 
00481     static float fZeroYawPos, fZeroPitchPos, fZeroRollPos;
00482 
00483         fZeroYawPos = fFilterYaw;
00484     fZeroPitchPos = fFilterPitch;
00485     fZeroRollPos = fFilterRoll;
00486 
00487     fAbsYawPos = fFilterYaw;
00488     fAbsPitchPos = fFilterPitch;
00489     fAbsRollPos = fFilterRoll;
00490 
00491     //In relative mode, YPR acts as a rate controller, providing continuous motion when a certain
00492     //limit is hit
00493 
00494 #define YPR_ROTSPEED    0.5f
00495 
00496     if ( fFilterRoll > (fZeroRollPos+30.0f) )
00497             fRelRollPos += YPR_ROTSPEED;
00498     else if ( fFilterRoll<(fZeroRollPos-30.0f) )
00499             fRelRollPos -= YPR_ROTSPEED;
00500     if (fRelRollPos > 180.0f)
00501             fRelRollPos = -180.0f;
00502     else if (fRelRollPos < -180.0f)
00503             fRelRollPos = 180.0f;
00504 
00505     if (fFilterYaw > (fZeroYawPos+25.0f))
00506             fRelYawPos += YPR_ROTSPEED;
00507     else if (fFilterYaw < (fZeroYawPos-25.0f))
00508             fRelYawPos -= YPR_ROTSPEED;
00509     if (fRelYawPos > 180.0f)
00510             fRelYawPos = -180.0f;
00511     else if (fRelYawPos < -180.0f)
00512             fRelYawPos = 180.0f;
00513 
00514         if (fFilterPitch > (fZeroPitchPos+25.0f))
00515             fRelPitchPos += YPR_ROTSPEED;
00516     else if (fFilterPitch < (fZeroPitchPos-25.0f))
00517             fRelPitchPos -= YPR_ROTSPEED;
00518     if (fRelPitchPos > 180.0f)
00519             fRelPitchPos = -180.0f;
00520     else if (fRelPitchPos < -180.0f)
00521             fRelPitchPos = 180.0f;
00522     }
00523 
00524 } // namespace ot
00525 
00526 #else
00527 #pragma message(">>> no P5 glove support")
00528 #endif
00529 
00530 /* 
00531  * ------------------------------------------------------------
00532  *   End of P5GloveModule.cxx
00533  * ------------------------------------------------------------
00534  *   Automatic Emacs configuration follows.
00535  *   Local Variables:
00536  *   mode:c++
00537  *   c-basic-offset: 4
00538  *   eval: (c-set-offset 'substatement-open 0)
00539  *   eval: (c-set-offset 'case-label '+)
00540  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00541  *   eval: (setq indent-tabs-mode nil)
00542  *   End:
00543  * ------------------------------------------------------------ 
00544  */

copyright (c) 2006 Graz University of Technology