Impala Documentation

Vec3Real64 Impala::Core::Array::Trait::FuncKalman::doIt	(	const Vec3Real64 &	p1,
		const Vec3Real64 &	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

Todo:: check correctness of outliers during occlusion

Definition at line 126 of file FunctorKalman.h.

References Impala::Core::Array::Array2dTem< StorT, elemSize, ArithT >::CW(), Impala::Core::Array::Element::EuclidDistSquare(), Impala::Core::Array::Array2dTem< StorT, elemSize, ArithT >::mData, mN_omax, mOcclusion, mOutCounter, mOutliers, mOutlierTreshold, mPixelCount, mResidualThisFrame, mSigmaG, and mSigmaI.

00127     {
00128         double r2;
00129         r2 = EuclidDistSquare(p1,p2);
00130 
00131         //(part of)eq.14
00132         mResidualThisFrame += r2;
00133         mPixelCount++;
00134         //eq.12
00135         if(r2 < mOutlierTreshold)
00136         {
00137             mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00138             //eq.10
00139             return (p1*mSigmaI + p2*mSigmaG) / (mSigmaI+mSigmaG);
00140         }
00141         else
00142         {
00143             mOutliers++;
00144             if(mOcclusion) // this extra check to leave the outlier counters intact during occlusion
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     }

Here is the call graph for this function: