00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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 ©)
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