Reimplemented from Impala::Core::Tracking::SearcherBase. Definition at line 22 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. 00023 { 00024 int i; 00025 double score, bestScore = 1e100; //high, todo: get high value from standard library 00026 Point bestPos; 00027 Array::Array2dVec3Real64* temp = new Array::Array2dVec3Real64(mLastPos.size.x, mLastPos.size.y, 0, 0); 00028 for(i=0 ; i<mSampleSize ; i++) 00029 { 00030 double x,y,sqr; 00031 // take 2 random numbers with normal distribution 00032 do 00033 { 00034 do 00035 { 00036 x = ((double)rand() / (double)RAND_MAX) * 2 - 1; 00037 y = ((double)rand() / (double)RAND_MAX) * 2 - 1; 00038 sqr = x*x + y*y; 00039 } 00040 while(sqr >=1.0 || sqr == 0); 00041 double fac = sqrt(-2.0*log(sqr)/sqr); 00042 00043 x = (fac * x * mPosSigma) + mLastPos.translation.x; 00044 y = (fac * y * mPosSigma) + mLastPos.translation.y; 00045 } 00046 while(x<0 || y<0 || x+mLastPos.size.x >= image.CW() || y+mLastPos.size.y >= image.CH()); 00047 00048 //sample search area at pos 00049 //if scale is used in the searcher, we should just send an image to the object representation. 00050 //since the representation might not use pixel data the representation is responsible for 00051 //interpreting the sample we provide it. 00052 Array::Pattern::PatSet(temp,&image, (int)x, (int)y, mLastPos.size.x, mLastPos.size.y, 0, 0); 00053 00054 //get score 00055 score = mObject->Score(*temp,mLastPos); 00056 00057 //if best 00058 if(score < bestScore) 00059 { 00060 bestPos.x = x; 00061 bestPos.y = y; 00062 bestScore = score; 00063 } 00064 } 00065 00066 //now search around the found position for the best fit, because not all neighbours are sampled 00067 int x,y; 00068 for(x=bestPos.x-3 ; x<=bestPos.x+3 ; x++) 00069 { 00070 for(y=bestPos.y-3 ; y<=bestPos.y+3 ; y++) 00071 { 00072 if(x>=0 && y>=0 && x+mLastPos.size.x < image.CW() && y+mLastPos.size.y < image.CH()) 00073 { 00074 Array::Pattern::PatSet(temp,&image, (int)x, (int)y, mLastPos.size.x, mLastPos.size.y, 0, 0); 00075 score = mObject->Score(*temp,mLastPos); 00076 if(score < bestScore) 00077 { 00078 bestPos.x = x; 00079 bestPos.y = y; 00080 bestScore = score; 00081 } 00082 } 00083 } 00084 } 00085 delete temp; 00086 00087 00088 mSpeed.translation.x = bestPos.x - mLastPos.translation.x; 00089 mSpeed.translation.y = bestPos.y - mLastPos.translation.y; 00090 mLastPos.translation = bestPos; 00091 }
Here is the call graph for this function:
|