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 325 of file FunctorKalman.h.

00326     {
00327         double err;
00328         err = MahalanobisDist(p1,p2); // mahalanobis dist
00329 
00330         mPixelCount++;
00331         //eq.11
00332         if(err < mChi2 || !mInitialised)
00333         {
00334             mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00335             //eq.9
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     }