Impala Documentation

void Impala::Core::Array::Trait::FuncKalmanColor::UpdateKalman ( ) [inline]

this function does reestimation of the sigmas using the residual

Definition at line 368 of file FunctorKalman.h.

Referenced by Impala::Core::Tracking::KalmanTemplate::Update().

00369     {
00370         int i;
00371         for(i=0 ; i<3 ; i++)
00372         {
00373             //eq.12
00374             mResidualThisFrame[i] /= mPixelCount;
00375             if(mResidualThisFrame[i] == 0)
00376                 mResidualThisFrame[i] = 1; 
00377         }
00378 
00379         if(mPixelCount * mGamma > mOutliers) // if no occlusion
00380         {
00381             mOcclusion = false;
00382             ComputeResiduals();
00383             if(!mInitialised)
00384                 InitialiseSigma();
00385             for(i=0 ; i<3 ; i++)
00386             {
00387                 //eq.10
00388                 mSigmaG[i] = (mSigmaG[i]*mSigmaI[i])/(mSigmaG[i]+mSigmaI[i]);
00389                 //eq.14
00390                 mSigmaW[i] = mResidualAvrg[i] - mSigmaI[i] - mSigmaG[i];
00391                 // following found in Hieus code, makes some sense, but only because the
00392                 // assumption about residual and sigmas are incorrect (I can't define them, sorry)
00393                 if(mSigmaW[i]<0)
00394                     mSigmaW[i] = 0;
00395                 //eq.8 (prediction of sigmaG for the next time step)
00396                 mSigmaG[i] += mSigmaW[i];
00397             }
00398         }
00399         else
00400         {
00401             if(!mOcclusion)
00402             {
00403                 //undo prediction of sigmaG (eq.8) (only if the last time sigmaG was predicted)
00404                 for(i=0 ; i<3 ; i++)
00405                     mSigmaG[i] -= mSigmaW[i];
00406             }
00407             mOcclusion = true;
00408 
00409             /* *
00410                 \note augmentation to hieus algorithm: increment sigma G so that the algorithm
00411                 leaves occlusion mode with increasing probability 
00412                 */
00413             
00414             for(i=0 ; i<3 ; i++)
00415                 mSigmaG[i] *= 1.15;
00416             
00417         }
00418         mPixelCount = 0;
00419         mOutliers = 0;
00420     }