25 #include "TrackerBaseWeighted1D.hh"
30 #define SMALL_DET 1e-8
32 template <
class StorageType>
40 CalcGaussianWeightMatrix_();
41 BIASCDOUT(D_TRACKERB_KLT,
"p1: " << p1[0] <<
", " << p1[1]
42 <<
"\tp2: " <<p2[0] <<
", " << p2[1]<<endl);
47 register KLT_TYPE dI, ex, ey, dx, det;
49 const int hws=_HalfWinSize;
50 const int winsize=_WinSize;
51 const int maxiter=_MaxIterations;
52 const KLT_TYPE maxerr=_MaxError;
54 if (p1[0]<_minx1 || p1[0]>=_maxx1 ||
55 p1[1]<_miny1 || p1[1]>=_maxy1 ||
56 p2[0]<_minx2 || p2[0]>=_maxx2 ||
57 p2[1]<_miny2 || p2[1]>=_maxy2 ){
58 BIASCDOUT(D_TRACKERB_KLT,
"p1 ("<<p1[0]<<
", "<<p1[1]<<
") p2 ("<<p2[0]
59 <<
", "<<p2[1]<<
")\n");
60 BIASCDOUT(D_TRACKERB_KLT,
"tracking roi im1 : ( "<<_minx1<<
", "<<_miny1
61 <<
") <--> ( "<<_maxx1<<
", "<<_maxy1<<
")\n");
62 BIASCDOUT(D_TRACKERB_KLT,
"tracking roi im2 : ( "<<_minx2<<
", "<<_miny2
63 <<
") <--> ( "<<_maxx2<<
", "<<_maxy2<<
")\n");
64 BIASCDOUT(D_TRACKERB_KLT,
"points do not lie in ROI, returning -4\n");
67 BIASCDOUT(D_TRACKERB_KLT,
"tracking roi im1 : ( "<<_minx1<<
", "<<_miny1
68 <<
") <--> ( "<<_maxx1<<
", "<<_maxy1<<
")\n");
69 BIASCDOUT(D_TRACKERB_KLT,
"tracking roi im2 : ( "<<_minx2<<
", "<<_miny2
70 <<
") <--> ( "<<_maxx2<<
", "<<_maxy2<<
")\n");
78 BilinearRegion1_(p1[0],p1[1],hws);
80 _gxx=_gxy=_gyy=ex=ey=0.0;
83 BilinearRegion2_(result[0],result[1],hws);
85 for (rh=0; rh<winsize; rh++){
86 for (ch=0; ch<winsize; ch++){
88 _weight[rh][ch] * (_gx1[rh][ch] + _gx2[rh][ch]);
89 _gxx += _gsx[rh][ch]*_gsx[rh][ch];
90 dI = _weight[rh][ch] * (_bl1[rh][ch] - _bl2[rh][ch]);
91 ex += _gsx[rh][ch]*dI;
94 BIASCDOUT(D_TRACKERB_KLT,
"_gxx: "<<_gxx<<
" _gxy: "<<_gxy
95 <<
" _gyy: "<<_gyy<<
" ex: "<<ex<<
" ey: "<<ey<<endl);
98 if (this->_AffineBrightnessInvariance) {
103 if (fabs(det)<SMALL_DET) {
104 BIASCDOUT(D_TRACKERB_KLT,
105 "unable to invert G, det: "<<det<<endl);
113 BIASCDOUT(D_TRACKERB_KLT,
"det: "<<det<<
" _idet: "<<_idet
114 <<
" dx: "<<dx<<endl);
132 BIASCDOUT(D_TRACKERB_KLT,
"disp after "<<iter<<
" iteration: "
133 <<result[0]-p2[0]<<
", "<<result[1]-p2[1]<<
" error: "<< error
134 <<
" ("<<result[0]<<
", "<<result[1]<<
")\n");
138 if (result[0]<_minx2 || result[0]>=_maxx2 ||
139 result[1]<_miny2 || result[1]>=_maxy2){
140 BIASCDOUT(D_TRACKERB_KLT,
141 "feature slid out of image, returning -2\n");
145 }
while (error>maxerr && iter<maxiter);
147 if (res==0 && error>maxerr) {
149 BIASCDOUT(D_TRACKERB_KLT,
"error > maxerror : returning -3\n");
151 BilinearRegion2_(result[0],result[1],hws);
156 cerr <<
"i "<<iter<<
" res "<<res<<
" ";
157 cerr <<
"TrackWeighted_ took "<<timer.
GetRealTime()<<
" us = "
164 template <
class StorageType>
168 _weight.newsize(_WinSize, _WinSize);
170 double sigma = ((double)_WinSize)/6.0;
171 double fac = 1.0 / (2.0 * sigma * sigma);
173 register int i, j, x, y, xp, xm, yp, ym;
174 const int hws=_HalfWinSize;
175 for (i= hws; i >= 0; i--){
176 for (j= hws; j >= 0; j--){
177 tmp=exp(-(
double)(i*i+j*j)*fac);
182 _weight[ym][xm] =_weight[yp][xm] = _weight[ym][xp] = _weight[yp][xp] =
187 for (y=0; y<_WinSize; y++){
188 for (x=0; x<_WinSize; x++){
192 BIASCDOUT(D_TRACKERB_INIT,
"_weight: "<<_weight<<endl);
193 BIASCDOUT(D_TRACKERB_INIT,
"_weight sum before normalization: "<<sum<<endl);
197 for (i= hws; i >= 0; i--){
198 for (j= hws; j >= 0; j--){
204 _weight[ym][xm] = _weight[yp][xm] = _weight[ym][xp] =
205 _weight[yp][xp] = tmp * sum;
208 BIASCDOUT(D_TRACKERB_INIT,
"_weight: "<<_weight<<endl);
211 for (y=0; y<_WinSize; y++){
212 for (x=0; x<_WinSize; x++){
216 BIASCDOUT(D_TRACKERB_INIT,
"_weight sum after normalization: "<<sum<<endl);
229 #ifdef BUILD_IMAGE_INT
232 #ifdef BUILD_IMAGE_CHAR
235 #ifdef BUILD_IMAGE_SHORT
238 #ifdef BUILD_IMAGE_USHORT
241 #ifdef BUILD_IMAGE_UINT
244 #ifdef BUILD_IMAGE_DOUBLE
virtual int Track_(Vector2< KLT_TYPE > &p1, Vector2< KLT_TYPE > &p2, Vector2< KLT_TYPE > &result, KLT_TYPE &error, int &iter, const Matrix2x2< KLT_TYPE > &AffinePred, Matrix2x2< KLT_TYPE > &AffineResult)
Interface of all tracking algorithms implemented in derived classes.
success (error < maxerror)
no spatial structure is present
double GetCycleCount() const
return number of cycles between all subsequent calls to Start() and Stop() since last call to Reset()...
double GetRealTime() const
return real time (=wall time clock) in usec JW For Win32: real-time is measured differently from user...
void CalcGaussianWeightMatrix_()
a point lies outside of valid region in images
maxiter is reached and error is above maxerror