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 108 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.

00109     {
00110         double r2;
00111         r2 = EuclidDistSquare(p1,p2);
00112 
00113         //(part of)eq.14
00114         mResidualThisFrame += r2;
00115         mPixelCount++;
00116         //eq.12
00117         if(r2 < mOutlierTreshold)
00118         {
00119             mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00120             //eq.10
00121             return (p1*mSigmaI + p2*mSigmaG) / (mSigmaI+mSigmaG);
00122         }
00123         else
00124         {
00125             mOutliers++;
00126             if(mOcclusion) // this extra check to leave the outlier counters intact during occlusion
00127                 return p1; 
00128             else
00129             {
00130                 if(mOutCounter->mData[y*mOutCounter->CW() + x] >= mN_omax)
00131                 {
00132                     mOutCounter->mData[y*mOutCounter->CW() + x] = 0;
00133                     return p2;
00134                 }
00135                 else
00136                 {
00137                     mOutCounter->mData[y*mOutCounter->CW() + x]++;
00138                     return p1;
00139                 }
00140             }
00141         }
00142     }

Here is the call graph for this function: