| void Impala::Core::Tracking::SearcherAroundLastPos::SearchObject | ( | Array::Array2dVec3Real64 & | image | ) | [inline, virtual] |
Reimplemented from Impala::Core::Tracking::SearcherBase.
Definition at line 29 of file SearcherAroundLastPos.h.
References Impala::Core::Array::Array2dTem< StorT, elemSize, ArithT >::CH(), Impala::Core::Array::Array2dTem< StorT, elemSize, ArithT >::CW(), Impala::Core::Tracking::SearcherBase::mLastPos, Impala::Core::Tracking::SearcherBase::mObject, mPosSigma, mSampleSize, mSpeed, Impala::Core::Array::Pattern::PatSet(), Impala::Core::Tracking::ObjectRepresentation::Score(), Impala::Core::Tracking::Position::size, Impala::Core::Tracking::Position::translation, Impala::Core::Tracking::Point::x, and Impala::Core::Tracking::Point::y.
00030 { 00031 int i; 00032 double score; 00033 double bestScore = 1e100; //high, todo: get high value from standard library 00034 Point bestPos; 00035 Array::Array2dVec3Real64* temp = 00036 new Array::Array2dVec3Real64(mLastPos.size.x, mLastPos.size.y, 0, 0); 00037 for(i=0 ; i<mSampleSize ; i++) 00038 { 00039 double x,y,sqr; 00040 // take 2 random numbers with normal distribution 00041 do 00042 { 00043 do 00044 { 00045 x = ((double)rand() / (double)RAND_MAX) * 2 - 1; 00046 y = ((double)rand() / (double)RAND_MAX) * 2 - 1; 00047 sqr = x*x + y*y; 00048 } 00049 while(sqr >=1.0 || sqr == 0); 00050 double fac = sqrt(-2.0*log(sqr)/sqr); 00051 00052 x = (fac * x * mPosSigma) + mLastPos.translation.x; 00053 y = (fac * y * mPosSigma) + mLastPos.translation.y; 00054 } 00055 while(x<0 || y<0 || x+mLastPos.size.x >= image.CW() || 00056 y+mLastPos.size.y >= image.CH()); 00057 00058 //Sample search area at pos. If scale is used in the searcher, we 00059 //should just send an image to the object representation. Since the 00060 //representation might not use pixel data the representation is 00061 //responsible for interpreting the sample we provide it. 00062 Array::Pattern::PatSet(temp,&image, (int)x, (int)y, mLastPos.size.x, 00063 mLastPos.size.y, 0, 0); 00064 //get score 00065 score = mObject->Score(*temp,mLastPos); 00066 //if best 00067 if(score < bestScore) 00068 { 00069 bestPos.x = x; 00070 bestPos.y = y; 00071 bestScore = score; 00072 } 00073 } 00074 00075 //now search around the found position for the best fit, because not all 00076 //neighbours are sampled 00077 int x,y; 00078 for(x=bestPos.x-3 ; x<=bestPos.x+3 ; x++) 00079 { 00080 for(y=bestPos.y-3 ; y<=bestPos.y+3 ; y++) 00081 { 00082 if(x>=0 && y>=0 && x+mLastPos.size.x < image.CW() && 00083 y+mLastPos.size.y < image.CH()) 00084 { 00085 Array::Pattern::PatSet(temp,&image, (int)x, (int)y, 00086 mLastPos.size.x, mLastPos.size.y, 0, 0); 00087 score = mObject->Score(*temp,mLastPos); 00088 if(score < bestScore) 00089 { 00090 bestPos.x = x; 00091 bestPos.y = y; 00092 bestScore = score; 00093 } 00094 } 00095 } 00096 } 00097 delete temp; 00098 00099 00100 mSpeed.translation.x = bestPos.x - mLastPos.translation.x; 00101 mSpeed.translation.y = bestPos.y - mLastPos.translation.y; 00102 mLastPos.translation = bestPos; 00103 }
Here is the call graph for this function:
