EventCounterData.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, AASS Research Center, Orebro University.
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
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 #ifndef CEVENT_COUNTER_DATA_H
00036 #define CEVENT_COUNTER_DATA_H
00037 #include<cmath>
00038 #include <Eigen/Dense>
00039 #include <inttypes.h>
00040 
00042 #define EVENTMAP_OCCU  255
00043 #define EVENTMAP_UNKNOWN  127
00044 #define EVENTMAP_FREE  0
00045 #define EVENTMAP_NUMBER_OF_EVENTS_STORED 128
00046 
00047 
00049 #define EVENTMAP_USE_RECENCY_FILTERING
00050 #define EVENTMAP_OBSERVATION_LIMIT 30000.0
00051 #define Wforget (5000.0/5001.0)
00052 
00056 struct TEventData
00057 {
00058     uint8_t     occval;                         
00059     float               a_exit_event; 
00060     float               b_exit_event; 
00061 
00062     float               a_entry_event;
00063     float               b_entry_event; 
00064     uint64_t  events;        
00065 
00066     TEventData():
00067         occval(127),
00068         a_exit_event(1.0),
00069         b_exit_event(1.0),
00070         a_entry_event(1.0),
00071         b_entry_event(1.0),
00072         events(0)
00073     {
00074     };
00075 
00076     ~TEventData()
00077     {
00078     }
00079 
00080 
00081     const TEventData &operator =(const TEventData &copy)
00082     {
00083         occval = copy.occval;
00084         a_exit_event = copy.a_exit_event;
00085         b_exit_event = copy.b_exit_event;
00086         a_entry_event = copy.a_entry_event;
00087         b_entry_event = copy.b_entry_event;
00088         events = copy.events;
00089 
00090         return *this;
00091     }
00092 
00093     TEventData(const TEventData& copy):
00094         occval(copy.occval),
00095         a_exit_event(copy.a_exit_event),
00096         b_exit_event(copy.b_exit_event),
00097         a_entry_event(copy.a_entry_event),
00098         b_entry_event(copy.b_entry_event),
00099         events( copy.events)
00100     {
00101     }
00102 
00103 
00104     void Reset()
00105     {
00106         occval = 127;
00107         a_exit_event = 1;
00108         b_exit_event = 1;
00109         a_entry_event = 1;
00110         b_entry_event = 1;
00111     }
00112 
00113     bool getBit(int ind)
00114     {
00115         if((events & (1<<ind))>0) return true;
00116         return false;
00117     }
00118 
00119 
00120     float computeShortTermOccupancy()
00121     {
00122         static float OCC_LOG_PROP = log(0.6/0.4);
00123         static float EMP_LOG_PROP = log(0.3/0.7);
00124         uint bs=0;
00125 
00126         if( (b_entry_event+b_exit_event-2) > 63) bs=64;
00127         else bs = b_entry_event+b_exit_event-2;
00128         uint cnt=0;
00129         float log_ogg_prob  = 0 ;
00130         for(uint i = 0; i<bs; i++)
00131         {
00132             if(getBit(i))
00133             {
00134                 log_ogg_prob += OCC_LOG_PROP;
00135                 cnt++;
00136             }
00137             else
00138             {
00139                 log_ogg_prob += EMP_LOG_PROP;
00140             }
00141         }
00142         return (1.0- 1.0/(1.0+exp(log_ogg_prob)));
00143     }
00144 
00145     int getObservations()
00146     {
00147         return (b_entry_event+b_exit_event);
00148     }
00149 
00153     void updateSimple(uint8_t meas)
00154     {
00155         events = events<<1;
00156         if(meas > EVENTMAP_UNKNOWN)  
00157         {
00158             events |= 1;
00159         }
00160         if(occval > EVENTMAP_UNKNOWN)  
00161         {
00162             updateExitEvent(meas, 1.0);
00163         }
00164         else if(occval < EVENTMAP_UNKNOWN)
00165         {
00166             updateEntryEvent(meas,1.0);
00167         }
00168         else
00169         {
00170             occval = meas; 
00171         }
00172     }
00173 
00177     void updateSimple(uint8_t meas, float w)
00178     {
00179         events = events<<1;
00180         if(meas > EVENTMAP_UNKNOWN)  
00181         {
00182             events |= 1;
00183         }
00184         if(occval > EVENTMAP_UNKNOWN)  
00185         {
00186             updateExitEvent(meas, w);
00187         }
00188         else if(occval < EVENTMAP_UNKNOWN)
00189         {
00190             updateEntryEvent(meas,w);
00191         }
00192         else
00193         {
00194             occval = meas; 
00195         }
00196 #ifdef EVENTMAP_USE_RECENCY_FILTERING
00197         performRecencyFiltering();
00198 #endif
00199     }
00200 
00201     void performRecencyFiltering()
00202     {
00203         if(occval > EVENTMAP_UNKNOWN)  
00204         {
00205             if(b_exit_event>EVENTMAP_OBSERVATION_LIMIT)
00206             {
00207                 float w = EVENTMAP_OBSERVATION_LIMIT / b_exit_event;
00208                 b_exit_event *= w;
00209                 a_exit_event *= w;
00210             }
00211             a_entry_event = 1.0 + (a_entry_event-1.0) * Wforget;
00212             b_entry_event = 1.0 + (b_entry_event-1.0) * Wforget;
00213         }
00214         else
00215         {
00216             if(b_entry_event>EVENTMAP_OBSERVATION_LIMIT)
00217             {
00218                 float w = EVENTMAP_OBSERVATION_LIMIT / b_entry_event;
00219                 b_entry_event *= w;
00220                 a_entry_event *= w;
00221             }
00222             a_exit_event = 1.0 + (a_exit_event-1.0) * Wforget;
00223             b_exit_event = 1.0 + (b_exit_event-1.0) * Wforget;
00224         }
00225     }
00226 
00227 
00231     void updateExitEvent(uint8_t meas, float w)
00232     {
00233         if(meas>EVENTMAP_UNKNOWN)  
00234         {
00235             b_exit_event+=1.0;
00236         }
00237         else
00238         {
00239             if(w>1.0) fprintf(stderr,"EXIT W larger that one = %f\n ",w);
00240             a_exit_event+=w;
00241             b_exit_event+=1.0;
00242             occval = EVENTMAP_FREE;
00243         }
00244     }
00248     void updateEntryEvent(uint8_t meas, float w)
00249     {
00250         if(meas>EVENTMAP_UNKNOWN)  
00251         {
00252             b_entry_event+=1.0;
00253             if(w>1.0) fprintf(stderr,"W larger that one = %f\n ",w);
00254             a_entry_event+=w;
00255             occval = EVENTMAP_OCCU;
00256         }
00257         else
00258         {
00259             b_entry_event+=1.0;
00260         }
00261     }
00262 
00264     int getEntryN()
00265     {
00266         return (int)((float)b_entry_event/(float)a_entry_event +0.5);
00267     }
00269     int getExitN()
00270     {
00271         return (int)((float)b_exit_event/(float)a_exit_event +0.5);
00272     }
00273 
00274 
00275     double entryL()
00276     {
00277         if(b_entry_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
00278         return ((double)a_entry_event/(double)b_entry_event );
00279     }
00280     double exitL()
00281     {
00282         if(b_exit_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
00283         return ((double)a_exit_event/(double)b_exit_event);
00284     }
00285     double L()
00286     {
00287         if(b_exit_event == 0) fprintf(stderr,"B Zero, which is not possible!!\n");
00288         return ((double)(a_exit_event+a_entry_event)/(double)(b_exit_event+b_entry_event));
00289     }
00290 
00291     double fac(int n)
00292     {
00293         double t=1;
00294         for (int i=n; i>1; i--)
00295             t*=i;
00296         return t;
00297     }
00298 
00299     double Bin(int n,double p,int r)
00300     {
00301         return fac(n)/(fac(n-r)*fac(r))*pow(p,r)*pow(1-p,n-r);
00302     }
00303 
00304     float binaryBayesUpdate(float Pold, float P)
00305     {
00306         return (( Pold*P) / ( Pold*P + (1.0f-Pold)*(1.0f-P)));  
00307     }
00308 
00312     void normalizeProb(float &p)
00313     {
00314         if(p>0.999) p = 0.999;
00315         if(p<0.001) p = 0.001;
00316 
00317     }
00322 
00325     float getOccStaticLikelihood()
00326     {
00327         if( (b_entry_event + b_exit_event)<20.0 )
00328         {
00329             return 0.5f;
00330         }
00331         Eigen::Matrix2f P;
00332         Eigen::Vector2f u1(0.5, 0.5);
00333         Eigen::Vector2f P0;
00334         float Lex = exitL();
00335         float Len = entryL();
00336         P(0,0) = (1.0-Len);
00337         P(0,1) =  Len;
00338         P(1,0)   = Lex;
00339         P(1,1) = (1-Lex);
00340 
00341         P0 = u1.transpose() * P;
00342         return (P0(1));
00343     }
00344 
00348     float getFreeStaticLikelihood()
00349     {
00350         if( (b_entry_event + b_exit_event)<20.0 )
00351         {
00352             return 0.5f;
00353         }
00354         Eigen::Matrix2f P;
00355         Eigen::Vector2f P0;
00356         Eigen::Vector2f u1(0.5, 0.5);
00357         float Lex = exitL();
00358         float Len = entryL();
00359         P(0,0) = (1.0-Len);
00360         P(0,1) =  Len;
00361         P(1,0)   = Lex;
00362         P(1,1) = (1-Lex);
00363 
00364         P0 = u1.transpose() * P;
00365         return (P0(0));
00366     }
00367 
00375     float computeSemiStaticLikelihood(int N)
00376     {
00377         if( (b_entry_event + b_exit_event)<20.0 )
00378         {
00379             return 0.0f;
00380         }
00381 
00382 
00383         Eigen::Matrix2f P;
00384         Eigen::Vector2f u2(0, 1.0);
00385         Eigen::Vector2f u3(1.0, 0);
00386         float Lex = exitL();
00387         float Len = entryL();
00388         Eigen::Vector2f P2,P3;
00389         P(0,0) = (1.0-Len);
00390         P(0,1) =  Len;
00391         P(1,0)   = Lex;
00392         P(1,1) = (1-Lex);
00393         for(int i=0; i<N; i++) P = P*P;
00394         P2 = u2.transpose() * P;
00395         P3 = u3.transpose() * P;
00396 
00397         float Po = P2(1);
00398         float Pu = binaryBayesUpdate(Po, P3(0));
00399         normalizeProb(Pu);
00400 
00401         return Pu;
00402     }
00403 
00404     float getOccupancyNow()
00405     {
00406         if( (b_entry_event + b_exit_event)<50.0)
00407         {
00408             return 0.5f;
00409         }
00410         float Po = 0;
00411 
00412 
00413         Po = computeShortTermOccupancy();
00414         float Lex = exitL();
00415         float Len = entryL();
00416         Eigen::Matrix2f P;
00417         P(0,0) = (1.0-Len);
00418         P(0,1) =  Len;
00419         P(1,0)   = Lex;
00420         P(1,1) = (1-Lex);
00421         Eigen::Vector2f u1(1.0-Po, Po);
00422         Eigen::Vector2f u = u1.transpose() *P;
00423         return u(1);
00424     }
00425 
00431 
00432     float predictOccupancy(int N)
00433     {
00434         if( (b_entry_event + b_exit_event)<20.0)
00435         {
00436             return 0.5f;
00437         }
00439         float ps = getOccStaticLikelihood();
00440         if(ps>0.6) return 0.9;
00442 
00443 
00444         float Po = computeShortTermOccupancy();
00445         float Lex = exitL();
00446         float Len = entryL();
00447         Eigen::Matrix2f P;
00448         P(0,0) = (1.0-Len);
00449         P(0,1) =  Len;
00450         P(1,0)   = Lex;
00451         P(1,1) = (1-Lex);
00452         Eigen::Vector2f u1(1.0-Po, Po);
00453         for(int i=0; i<N; i++) P = P*P;
00454         Eigen::Vector2f u = u1.transpose() *P;
00455 
00456         return u(1);
00457     }
00458 
00459 
00460 };
00461 #endif
00462 


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Wed Aug 26 2015 15:24:40