OpenTracker

An Open Architecture for Reconfigurable Tracking based on XML | Contact

FilterNode.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 "FilterNode.h"
00048 #include <math.h>
00049 
00050 //using namespace std;
00051 
00052 // constructor method.
00053 
00054 namespace ot {
00055 
00056     FilterNode::FilterNode( const std::vector<float> & weights_, const Type & type_ )
00057         : Node(), weights( weights_ ), type( type_ )
00058     {
00059     }
00060 
00061     int FilterNode::isEventGenerator()
00062     {
00063         return 1;
00064     }
00065 
00066     // this method is called by the EventGenerator to update it's observers.
00067 
00068     void FilterNode::onEventGenerated( Event& event, Node& generator)
00069     {
00070         Node * queue = getChild( 0 );
00071         if( queue != NULL && queue->getSize() == weights.size() )
00072         {
00073             double w,sum = 0;
00074             double pos[3] = {0, 0, 0 }, orient[3] = { 0, 0, 0 };
00075             double conf = 0;
00076 
00077             // referencerot for quaternion interpolation
00078             //float * referencerot= queue->getEvent( 0 ).orientation;
00079             std::vector<float> &referencerot = queue->getEvent(0).getOrientation();
00080 
00081             std::vector<float>::iterator it;
00082             for( it = weights.begin(); it != weights.end(); it++ )
00083             {
00084                 Event & event = queue->getEvent( it - weights.begin());
00085                 w = (*it);
00086 
00087                 if( type != ORIENTATION )
00088                 {
00089                     /*  The position is computed as the weighted average of the
00090                         positions in the queue. The average is not normalized, to
00091                         yield a more general filter.*/
00092                     pos[0] += event.getPosition()[0] * w;
00093                     pos[1] += event.getPosition()[1] * w;
00094                     pos[2] += event.getPosition()[2] * w;
00095                 }
00096 
00097                 if( type != POSITION )
00098                 {
00099                     /* The orientation is computed as the normalized weighted average of the
00100                        orientations in the queue in log space. That is, they are transformed
00101                        to 3D vectors inside the unit hemisphere and averaged in this linear space.
00102                        The resulting vector is transformed back to a unit quaternion. */
00103 
00104                     if( MathUtils::dot(referencerot, event.getOrientation(), 4) < 0 )
00105                     {
00106                         event.getOrientation()[0] = -event.getOrientation()[0];
00107                         event.getOrientation()[1] = -event.getOrientation()[1];
00108                         event.getOrientation()[2] = -event.getOrientation()[2];
00109                         event.getOrientation()[3] = -event.getOrientation()[3];
00110                     }
00111 
00112                     double angle = acos( event.getOrientation()[3] );
00113                     double as = sin( angle );
00114                     if( as != 0 )
00115                         as = angle * w / as;
00116                     else
00117                         as = 0;                 // lim x/(sin(x/2)) = 2 for x -> 0 ???
00118                     orient[0] += event.getOrientation()[0] * as;
00119                     orient[1] += event.getOrientation()[1] * as;
00120                     orient[2] += event.getOrientation()[2] * as;
00121                 }
00122 
00123                 /* confidence is also averaged */
00124                 conf += event.getConfidence() * w;
00125 
00126                 sum += w;
00127 
00128             }
00129             if( type != POSITION )
00130             {
00131                 // calculate the length of the summed vector in log space
00132                 // this is the angle of the result quaternion
00133                 w = sqrt((orient[0]*orient[0] + orient[1]*orient[1] + orient[2]*orient[2])/(sum*sum));
00134                 double as = 0;
00135                 if( w != 0)
00136                     as = sin( w ) / w;
00137                 localEvent.getOrientation()[0] = (float)(orient[0] * as);
00138                 localEvent.getOrientation()[1] = (float)(orient[1] * as);
00139                 localEvent.getOrientation()[2] = (float)(orient[2] * as);
00140                 localEvent.getOrientation()[3] = (float)cos( w );
00141                 MathUtils::normalizeQuaternion( localEvent.getOrientation() );
00142             }
00143             else 
00144             {
00145                 localEvent.getOrientation()[0] = event.getOrientation()[0];
00146                 localEvent.getOrientation()[1] = event.getOrientation()[1];
00147                 localEvent.getOrientation()[2] = event.getOrientation()[2];
00148                 localEvent.getOrientation()[3] = event.getOrientation()[3];
00149             }
00150         
00151             if( type != ORIENTATION )
00152             {
00153                 // copy pos to event.position
00154                 localEvent.getPosition()[0] = (float)pos[0];
00155                 localEvent.getPosition()[1] = (float)pos[1];
00156                 localEvent.getPosition()[2] = (float)pos[2];
00157             }
00158             else
00159             {
00160                 localEvent.getPosition()[0] = event.getPosition()[0];
00161                 localEvent.getPosition()[1] = event.getPosition()[1];
00162                 localEvent.getPosition()[2] = event.getPosition()[2];           
00163             }
00164         
00165             localEvent.getConfidence() = (float)conf;
00166 
00167             localEvent.time = event.time;
00168             localEvent.getButton() = event.getButton();
00169             updateObservers( localEvent );
00170         }
00171         else
00172         {
00173             updateObservers( event );
00174         }
00175     }
00176 
00177 } // namespace ot
00178 
00179 
00180 /* 
00181  * ------------------------------------------------------------
00182  *   End of FilterNode.h
00183  * ------------------------------------------------------------
00184  *   Automatic Emacs configuration follows.
00185  *   Local Variables:
00186  *   mode:c++
00187  *   c-basic-offset: 4
00188  *   eval: (c-set-offset 'substatement-open 0)
00189  *   eval: (c-set-offset 'case-label '+)
00190  *   eval: (c-set-offset 'statement 'c-lineup-runin-statements)
00191  *   eval: (setq indent-tabs-mode nil)
00192  *   End:
00193  * ------------------------------------------------------------ 
00194  */

copyright (c) 2006 Graz University of Technology