Impala Documentation

void Impala::Core::Array::Trait::FuncKalmanColor::DoIt	(	double *	out,
		double *	p1,
		double *	p2,
		int	x,
		int	y
	)			`[inline]`

this function blends the old template with a new measurement using eqation 12 from the paper.

this is also where we decide whether a pixel is an outlier p1 is the old template, p2 is the measurement

Definition at line 307 of file FunctorKalman.h.

00308     {
00309         double err;
00310         err = MahalanobisDist(p1,p2); // mahalanobis dist
00311 
00312         mPixelCount++;
00313         //eq.11
00314         if(err < mChi2 || !mInitialised)
00315         {
00316             mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00317             //eq.9
00318             for(int i=0 ; i<3 ; i++)
00319                 out[i] = (p1[i] * mSigmaI[i] + p2[i] * mSigmaG[i]) / (mSigmaI[i]+mSigmaG[i]);
00320             return;
00321         }
00322         else
00323         {
00324             mOutliers++;
00325             if(mOcclusion)
00326             {
00327                 for(int i=0 ; i<3 ; i++)
00328                     out[i] = p1[i];
00329                 return;
00330             }
00331             else
00332             {
00333                 if(mOutCounter->mData[y*mOutCounter->CW() + x] >= mN_omax)
00334                 {
00335                     mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00336                     for(int i=0 ; i<3 ; i++)
00337                         out[i] = p2[i];
00338                     return;
00339                 }
00340                 else
00341                 {
00342                     mOutCounter->mData[y*mOutCounter->CW() + x]++;
00343                     for(int i=0 ; i<3 ; i++)
00344                         out[i] = p1[i];
00345                     return;
00346                 }
00347             }
00348         }
00349     }