Impala Documentation

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

this function does reestimation of the sigmas using the residual

Definition at line 386 of file FunctorKalman.h.

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

00387     {
00388         int i;
00389         for(i=0 ; i<3 ; i++)
00390         {
00391             //eq.12
00392             mResidualThisFrame[i] /= mPixelCount;
00393             if(mResidualThisFrame[i] == 0)
00394                 mResidualThisFrame[i] = 1; 
00395         }
00396 
00397         if(mPixelCount * mGamma > mOutliers) // if no occlusion
00398         {
00399             mOcclusion = false;
00400             ComputeResiduals();
00401             if(!mInitialised)
00402                 InitialiseSigma();
00403             for(i=0 ; i<3 ; i++)
00404             {
00405                 //eq.10
00406                 mSigmaG[i] = (mSigmaG[i]*mSigmaI[i])/(mSigmaG[i]+mSigmaI[i]);
00407                 //eq.14
00408                 mSigmaW[i] = mResidualAvrg[i] - mSigmaI[i] - mSigmaG[i];
00409                 // following found in Hieus code, makes some sense, but only because the
00410                 // assumption about residual and sigmas are incorrect (I can't define them, sorry)
00411                 if(mSigmaW[i]<0)
00412                     mSigmaW[i] = 0;
00413                 //eq.8 (prediction of sigmaG for the next time step)
00414                 mSigmaG[i] += mSigmaW[i];
00415             }
00416         }
00417         else
00418         {
00419             if(!mOcclusion)
00420             {
00421                 //undo prediction of sigmaG (eq.8) (only if the last time sigmaG was predicted)
00422                 for(i=0 ; i<3 ; i++)
00423                     mSigmaG[i] -= mSigmaW[i];
00424             }
00425             mOcclusion = true;
00426 
00427             /* *
00428                 \note augmentation to hieus algorithm: increment sigma G so that the algorithm
00429                 leaves occlusion mode with increasing probability 
00430                 */
00431             
00432             for(i=0 ; i<3 ; i++)
00433                 mSigmaG[i] *= 1.15;
00434             
00435         }
00436         mPixelCount = 0;
00437         mOutliers = 0;
00438     }