00001 #ifndef Impala_Core_Array_Trait_FuncKalman_h
00002 #define Impala_Core_Array_Trait_FuncKalman_h
00003
00004 #include <deque>
00005
00024 namespace Impala
00025 {
00026 namespace Core
00027 {
00028 namespace Array
00029 {
00030 namespace Element
00031 {
00032
00033 inline double EuclidDistSquare(const Vec3Real64& p1, const Vec3Real64& p2)
00034 {
00035 double t,d;
00036 t = p1.X() - p2.X();
00037 d = t*t;
00038 t = p1.Y() - p2.Y();
00039 d += t*t;
00040 t = p1.Z() - p2.Z();
00041 d += t*t;
00042 return d;
00043 }
00044
00045 }
00046
00047 namespace Trait
00048 {
00049
00058 class FuncKalman
00059 {
00060 private:
00061 Array2dScalarUInt8* mOutCounter;
00062 std::deque<double> mPastResiduals;
00063 double mOutlierTreshold;
00064 public:
00065 double mSigmaI, mSigmaW, mSigmaG;
00066 double mAlpha, mBeta, mGamma, mN_omax, mK, mPixelCount;
00067 int mOutliers;
00068 double mResidualThisFrame,mResidualAvrg;
00069 bool mInitialised,mOcclusion;
00070
00071 typedef Pattern::TagTransVar TransVarianceCategory;
00072 typedef Pattern::TagCallValue CallCategory;
00073 typedef Vec3Int32 DstArithT;
00074 typedef Vec3Int32 SrcArithT;
00075
00079 FuncKalman(int w, int h)
00080 {
00081
00082 mAlpha = 2.5;
00083 mBeta = 1.1;
00084 mGamma = 0.4;
00085 mN_omax = 20;
00086 mK = 20;
00087
00088 mOutCounter = 0;
00089 Reset(w,h);
00090 }
00091
00101 void Reset(int w, int h)
00102 {
00103 if(mOutCounter)
00104 delete mOutCounter;
00105 mOutCounter = new Array2dScalarUInt8(w,h,0,0);
00106 int i;
00107 for(i=0 ; i<w*h ; i++)
00108 mOutCounter->mData[i] = 0;
00109
00110 mOutlierTreshold = 196608;
00111 mSigmaI = mSigmaW = 0;
00112 mSigmaG = 1;
00113 mPastResiduals.clear();
00114 mResidualThisFrame = 0;
00115 mPixelCount = 0;
00116 mOutliers = 0;
00117 mInitialised = false;
00118 mOcclusion = false;
00119 }
00120
00125 Vec3Real64
00126 doIt(const Vec3Real64& p1, const Vec3Real64& p2, int x, int y)
00127 {
00128 double r2;
00129 r2 = EuclidDistSquare(p1,p2);
00130
00131
00132 mResidualThisFrame += r2;
00133 mPixelCount++;
00134
00135 if(r2 < mOutlierTreshold)
00136 {
00137 mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00138
00139 return (p1*mSigmaI + p2*mSigmaG) / (mSigmaI+mSigmaG);
00140 }
00141 else
00142 {
00143 mOutliers++;
00144 if(mOcclusion)
00145 return p1;
00146 else
00147 {
00148 if(mOutCounter->mData[y*mOutCounter->CW() + x] >= mN_omax)
00149 {
00150 mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00151 return p2;
00152 }
00153 else
00154 {
00155 mOutCounter->mData[y*mOutCounter->CW() + x]++;
00156 return p1;
00157 }
00158 }
00159 }
00160 }
00161
00165 void InitialiseSigma()
00166 {
00167
00168 mSigmaW = 0;
00169 mSigmaI = 0.5*mResidualAvrg;
00170 mSigmaG = 0.5*mResidualAvrg;
00171 mInitialised = true;
00172 }
00173
00176 void UpdateKalman()
00177 {
00178
00179 mResidualThisFrame /= mPixelCount;
00180 mResidualThisFrame *= mBeta;
00181 if(mResidualThisFrame == 0)
00182 mResidualThisFrame = 1;
00183
00184 if(mPixelCount * mGamma > mOutliers)
00185 {
00186 mOcclusion = false;
00187 ComputeResiduals();
00188 if(!mInitialised)
00189 InitialiseSigma();
00190
00191 mSigmaG = (mSigmaG*mSigmaI)/(mSigmaG+mSigmaI);
00192
00193 mSigmaW = mResidualAvrg - mSigmaI - mSigmaG;
00194
00195
00196 if(mSigmaW<0)
00197 mSigmaW = 0;
00198
00199 mSigmaG += mSigmaW;
00200 }
00201 else
00202 {
00203 if(!mOcclusion)
00204 {
00205
00206 mSigmaG -= mSigmaW;
00207 }
00208 mOcclusion = true;
00209 }
00210 mPixelCount = 0;
00211 mOutliers = 0;
00212 }
00213
00214 void ComputeResiduals()
00215 {
00216
00217 mPastResiduals.push_back(mResidualThisFrame);
00218 mResidualThisFrame = 0;
00219 mResidualAvrg = 0;
00220 std::deque<double>::iterator i;
00221 for(i=mPastResiduals.begin() ; i!=mPastResiduals.end() ; i++)
00222 {
00223 mResidualAvrg += *i;
00224 }
00225 mResidualAvrg /= mPastResiduals.size();
00226 mOutlierTreshold = mResidualAvrg * mAlpha;
00227 while(mPastResiduals.size() > mK)
00228 mPastResiduals.pop_front();
00229 }
00230
00231 };
00232
00233
00238 class FuncKalmanColor
00239 {
00240 private:
00241 Array2dScalarUInt8* mOutCounter;
00242 std::deque<double> mPastResiduals[3];
00243 double mOutlierTreshold;
00244 public:
00245 double mSigmaI[3], mSigmaW[3], mSigmaG[3];
00246 double mChi2, mGamma, mN_omax, mK, mPixelCount;
00247 int mOutliers;
00248 double mResidualThisFrame[3], mResidualAvrg[3];
00249 bool mInitialised, mOcclusion;
00250
00251 typedef Pattern::TagTransVar TransVarianceCategory;
00252 typedef Pattern::TagCallPointer CallCategory;
00253 typedef Vec3Int32 DstArithT;
00254 typedef Vec3Int32 SrcArithT;
00255
00259 FuncKalmanColor()
00260 {
00261 CmdOptions& options = CmdOptions::GetInstance();
00262
00263 mChi2 = options.GetDouble("appearance_kalman.kalman.chi^2", 13);
00264 mGamma = options.GetDouble("appearance_kalman.kalman.gamma", 0.3);
00265 mN_omax = options.GetDouble("appearance_kalman.kalman.N_omax", 40);
00266 mK = options.GetDouble("appearance_kalman.kalman.K", 20);
00267
00268 mOutCounter = 0;
00269 Reset(0,0);
00270 }
00271
00281 void Reset(int w, int h)
00282 {
00283 if(mOutCounter)
00284 delete mOutCounter;
00285 mOutCounter = new Array2dScalarUInt8(w,h,0,0);
00286 int i;
00287 for(i=0 ; i<w*h ; i++)
00288 mOutCounter->mData[i] = 0;
00289
00290 mOutlierTreshold = 196608;
00291 for(i=0 ; i<3 ; i++)
00292 {
00293 mSigmaI[i] = 0;
00294 mSigmaW[i] = 0;
00295 mSigmaG[i] = 1;
00296 mPastResiduals[i].clear();
00297 mResidualThisFrame[i] = 0;
00298 }
00299 mPixelCount = 0;
00300 mOutliers = 0;
00301 mInitialised = false;
00302 mOcclusion = false;
00303 }
00304
00308 double MahalanobisDist(double* p1, double* p2)
00309 {
00310 double r, err=0;
00311 for(int i=0 ; i<3 ; i++)
00312 {
00313 r = p1[i] - p2[i];
00314 err += r*r/(mSigmaG[i]+mSigmaI[i]);
00315 mResidualThisFrame[i] += r*r;
00316 }
00317 return err;
00318 }
00319
00324 void
00325 DoIt(double* out, double* p1, double* p2, int x, int y)
00326 {
00327 double err;
00328 err = MahalanobisDist(p1,p2);
00329
00330 mPixelCount++;
00331
00332 if(err < mChi2 || !mInitialised)
00333 {
00334 mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00335
00336 for(int i=0 ; i<3 ; i++)
00337 out[i] = (p1[i] * mSigmaI[i] + p2[i] * mSigmaG[i]) / (mSigmaI[i]+mSigmaG[i]);
00338 return;
00339 }
00340 else
00341 {
00342 mOutliers++;
00343 if(mOcclusion)
00344 {
00345 for(int i=0 ; i<3 ; i++)
00346 out[i] = p1[i];
00347 return;
00348 }
00349 else
00350 {
00351 if(mOutCounter->mData[y*mOutCounter->CW() + x] >= mN_omax)
00352 {
00353 mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00354 for(int i=0 ; i<3 ; i++)
00355 out[i] = p2[i];
00356 return;
00357 }
00358 else
00359 {
00360 mOutCounter->mData[y*mOutCounter->CW() + x]++;
00361 for(int i=0 ; i<3 ; i++)
00362 out[i] = p1[i];
00363 return;
00364 }
00365 }
00366 }
00367 }
00368
00372 void InitialiseSigma()
00373 {
00374 for(int i=0 ; i<3 ; i++)
00375 {
00376
00377 mSigmaW[i] = 0;
00378 mSigmaI[i] = 0.5*mResidualAvrg[i];
00379 mSigmaG[i] = 0.5*mResidualAvrg[i];
00380 }
00381 mInitialised = true;
00382 }
00383
00386 void UpdateKalman()
00387 {
00388 int i;
00389 for(i=0 ; i<3 ; i++)
00390 {
00391
00392 mResidualThisFrame[i] /= mPixelCount;
00393 if(mResidualThisFrame[i] == 0)
00394 mResidualThisFrame[i] = 1;
00395 }
00396
00397 if(mPixelCount * mGamma > mOutliers)
00398 {
00399 mOcclusion = false;
00400 ComputeResiduals();
00401 if(!mInitialised)
00402 InitialiseSigma();
00403 for(i=0 ; i<3 ; i++)
00404 {
00405
00406 mSigmaG[i] = (mSigmaG[i]*mSigmaI[i])/(mSigmaG[i]+mSigmaI[i]);
00407
00408 mSigmaW[i] = mResidualAvrg[i] - mSigmaI[i] - mSigmaG[i];
00409
00410
00411 if(mSigmaW[i]<0)
00412 mSigmaW[i] = 0;
00413
00414 mSigmaG[i] += mSigmaW[i];
00415 }
00416 }
00417 else
00418 {
00419 if(!mOcclusion)
00420 {
00421
00422 for(i=0 ; i<3 ; i++)
00423 mSigmaG[i] -= mSigmaW[i];
00424 }
00425 mOcclusion = true;
00426
00427
00428
00429
00430
00431
00432 for(i=0 ; i<3 ; i++)
00433 mSigmaG[i] *= 1.15;
00434
00435 }
00436 mPixelCount = 0;
00437 mOutliers = 0;
00438 }
00439
00440 void ComputeResiduals()
00441 {
00442
00443 for(int i=0 ; i<3 ; i++)
00444 {
00445 mPastResiduals[i].push_back(mResidualThisFrame[i]);
00446 mResidualThisFrame[i] = 0;
00447 mResidualAvrg[i] = 0;
00448 std::deque<double>::iterator it;
00449 for(it=mPastResiduals[i].begin() ; it!=mPastResiduals[i].end() ; it++)
00450 {
00451 mResidualAvrg[i] += *it;
00452 }
00453 mResidualAvrg[i] /= mPastResiduals[i].size();
00454 mOutlierTreshold = mResidualAvrg[i];
00455 while(mPastResiduals[i].size() > mK)
00456 mPastResiduals[i].pop_front();
00457 }
00458 }
00459 };
00460
00461
00462
00469 class FuncBpoRobustMahalanobis
00470 {
00471 public:
00472 typedef Pattern::TagTransInVar TransVarianceCategory;
00473 typedef Pattern::TagCallPointer CallCategory;
00474
00475 typedef Vec3Int32 DstArithT;
00476 typedef Vec3Int32 Src1ArithT;
00477 typedef Vec3Int32 Src2ArithT;
00478
00479 double mSigma[3];
00480 double mError;
00481 double mChi;
00482
00483 FuncBpoRobustMahalanobis()
00484 {
00485 mSigma[0] = mSigma[1] = mSigma[2] = 1;
00486 mError = 0;
00487 mChi = 3.5;
00488 }
00489
00490 void
00491 DoIt(double* res, double* p1, double* p2)
00492 {
00493 double r, err=0;
00494 for(int i=0 ; i<3 ; i++)
00495 {
00496 r = p1[i] - p2[i];
00497 err += r*r/(mSigma[i]);
00498 }
00499
00500
00501 if(err < mChi)
00502 {
00503 double r = 0.5 * err * err;
00504 res[0] = res[1] = res[2] = r;
00505 mError += r;
00506 }
00507 else
00508 {
00509 double r = mChi*(err - mChi/2);
00510 res[0] = res[1] = res[2] = r;
00511 mError += r;
00512 }
00513 }
00514 };
00515
00516 }
00517 }
00518 }
00519 }
00520
00521 #endif //Impala_Core_Array_Trait_FuncKalman_h