00001 #ifndef Impala_Core_Tracking_SearcherAroundLastpos_h
00002 #define Impala_Core_Tracking_SearcherAroundLastpos_h
00003
00004 #include "Core/Tracking/SearcherBase.h"
00005
00006 namespace Impala
00007 {
00008 namespace Core
00009 {
00010 namespace Tracking
00011 {
00012
00019 class SearcherAroundLastPos : public SearcherBase
00020 {
00021 public:
00022 SearcherAroundLastPos()
00023 {
00024 mSampleSize = 300;
00025 mPosSigma = 20;
00026 }
00027
00028 void
00029 SearchObject(Array::Array2dVec3Real64& image)
00030 {
00031 int i;
00032 double score;
00033 double bestScore = 1e100;
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
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
00059
00060
00061
00062 Array::Pattern::PatSet(temp,&image, (int)x, (int)y, mLastPos.size.x,
00063 mLastPos.size.y, 0, 0);
00064
00065 score = mObject->Score(*temp,mLastPos);
00066
00067 if(score < bestScore)
00068 {
00069 bestPos.x = x;
00070 bestPos.y = y;
00071 bestScore = score;
00072 }
00073 }
00074
00075
00076
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 }
00104
00105 void
00106 Reset(Array::Array2dVec3Real64& image, ObjectRepresentation& object,
00107 const Position& pos)
00108 {
00109 SearcherBase::Reset(image, object, pos);
00110 }
00111
00112 private:
00113 int mSampleSize;
00114 double mPosSigma;
00115 Position mSpeed;
00116 };
00117
00118 }
00119 }
00120 }
00121
00122 #endif //Impala_Core_Tracking_SearcherAroundLastpos_h