summaryrefslogtreecommitdiffstats
path: root/SD-VBS/common/toolbox/lagrcv/lagrcv.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'SD-VBS/common/toolbox/lagrcv/lagrcv.cpp')
-rwxr-xr-xSD-VBS/common/toolbox/lagrcv/lagrcv.cpp840
1 files changed, 840 insertions, 0 deletions
diff --git a/SD-VBS/common/toolbox/lagrcv/lagrcv.cpp b/SD-VBS/common/toolbox/lagrcv/lagrcv.cpp
new file mode 100755
index 0000000..4f24b5d
--- /dev/null
+++ b/SD-VBS/common/toolbox/lagrcv/lagrcv.cpp
@@ -0,0 +1,840 @@
1#include "lagrcv.h"
2/*
3void calcImagePyr(char *src, int sizeY, int sizeX, int level, char *pyr){
4 pyr=(char*) malloc(sizeof(char)*(int)sizeY*sizeX*4/3);
5 int i, startPnt, nX, nY;
6
7 memcpy(pyr,src, sizeY*sizeX);
8 nY=sizeY;
9 nX=sizeX;
10 for(i=0; i<level; i++){
11 nY = (nY + 1) >> 1;
12 nX = (nX + 1) >> 1;
13 calcSubsample(pyr, sizeY, sizeX, pyr+sizeY*sizeX)
14 }
15 free(pyr);
16}
17*/
18
19void calcSubSampleAvg(double *src, int sizeY, int sizeX, double *dest, int destSizeY, int destSizeX){
20 int i, j, idx,
21 destI, destJ, idxDest;
22 for(i=0, destI=0; destI<destSizeX; i+=2, destI++){
23 for(j=0, destJ=0; destJ<destSizeY; j+=2, destJ++){
24 idx=i*sizeX+j;
25 idxDest=destI*destSizeY+destJ;
26 dest[idxDest]=(src[idx]+src[idx+1]+src[idx+sizeY]+src[idx+sizeY+1])/4;
27 }
28 }
29}
30void calcImgBlur(double *src, int sizeY, int sizeX, double *dest){
31 double kernel[]={0.0625, 0.25, 0.375, 0.25, 0.0625};
32 double *temp;
33 temp=(double*) malloc(sizeof(double)*sizeY*sizeX);
34 int i, j, idx, idxCol;
35 for(i=2; i<sizeX-2;i++){
36 idxCol=i*sizeY;
37 for(j=0;j<sizeY; j++){
38 idx=idxCol+j;
39 temp[idx]=src[idx-2*sizeY]/16+src[idx-sizeY]/4
40 +src[idx]*3/8+src[idx+sizeY]/4+src[idx+2*sizeY]/16;
41 }
42 }
43 for(i=0; i<sizeX;i++){
44 idxCol=i*sizeY;
45 for(j=2;j<sizeY-2; j++){
46 idx=idxCol+j;
47 dest[idx]=temp[idx-2]/16+temp[idx-1]/4
48 +temp[idx]*3/8+temp[idx+1]/4+temp[idx+2]/16;
49 }
50 }
51 free(temp);
52}
53void calcImgResize(double *src, int sizeY, int sizeX, double *dest, int dstSizeY, int dstSizeX){
54 double ker[]={ 0.0039, 0.0156, 0.0234, 0.0156, 0.0039,
55 0.0156, 0.0625, 0.0938, 0.0625, 0.0156,
56 0.0234, 0.0938, 0.1406, 0.0938, 0.0234,
57 0.0156, 0.0625, 0.0938, 0.0625, 0.0156,
58 0.0039, 0.0156, 0.0234, 0.0156, 0.0039};
59 int srcI, srcJ, dstI, dstJ, srcIdx, dstIdx, idxCol_1, idxCol1, idxCol_2, idxCol2;
60 for(srcI=2, dstI=1; srcI<sizeX-2; srcI+=2, dstI++){
61 for(srcJ=2, dstJ=1; srcJ<sizeY-2; srcJ+=2, dstJ++){
62 srcIdx=srcI*sizeY+srcJ;
63 dstIdx=dstI*dstSizeY+dstJ;
64 idxCol_1=srcIdx-sizeY;
65 idxCol_2=srcIdx-2*sizeY;
66 idxCol1=srcIdx+sizeY;
67 idxCol2=srcIdx+2*sizeY;
68 dest[dstIdx]=
69 src[idxCol_2-2]*ker[0]+src[idxCol_1-2]*ker[1]
70 +src[srcIdx-2]*ker[2]+src[idxCol1-2]*ker[3]+src[idxCol2-2]*ker[4]
71 +src[idxCol_2-1]*ker[5]+src[idxCol_1-1]*ker[6]
72 +src[srcIdx-1]*ker[7]+src[idxCol1-1]*ker[8]+src[idxCol2-1]*ker[9]
73 +src[idxCol_2]*ker[10]+src[idxCol_1]*ker[11]
74 +src[srcIdx]*ker[12]+src[idxCol1]*ker[13]+src[idxCol2]*ker[14]
75 +src[idxCol_2+1]*ker[5]+src[idxCol_1+1]*ker[16]
76 +src[srcIdx+1]*ker[17]+src[idxCol1+1]*ker[18]+src[idxCol2+1]*ker[19];
77 +src[idxCol_2+2]*ker[20]+src[idxCol_1+2]*ker[21]
78 +src[srcIdx+2]*ker[22]+src[idxCol1+2]*ker[23]+src[idxCol2+2]*ker[24];
79 }
80 }
81}
82
83void calcGradient(double *src, int sizeY, int sizeX, double *dX, double *dY){
84 int i, j, idx;
85 for(i=1; i<sizeX; i++){
86 for(j=1; j<sizeY; j++){
87 idx=i*sizeY+j;
88 dX[idx]=src[idx]-src[idx-sizeY];
89 dY[idx]=src[idx]-src[idx-1];
90 }
91 }
92}
93void calcGradient(char *src, int sizeY, int sizeX, char *dX, char *dY){
94 int i, j, idx;
95 for(i=1; i<sizeX; i++){
96 for(j=1; j<sizeY; j++){
97 idx=i*sizeY+j;
98 dX[idx]=src[idx]-src[idx-sizeY];
99 dY[idx]=src[idx]-src[idx-1];
100 }
101 }
102}
103void calcSobel(double *src, int sizeY, int sizeX, double *dX, double *dY){
104 //char kernelV[9]={-1, -2, -1, 0, 0, 0, 1, 2, 1};
105 //char kernelH[9]={-1, 0, 1, -2, 0, 2, -1, 0, 1};
106 //const int kerSize=3;
107 int i, j, idx, idxPrevCol, idxNextCol;
108 for(i=1; i<sizeX-1; i++){
109 for(j=1; j<sizeY-1; j++){
110 idx=i*sizeY+j;
111 idxPrevCol=idx-sizeY;
112 idxNextCol=idx+sizeY;
113 dX[idx]=src[idxPrevCol-1]*-0.0938f+src[idxPrevCol]*-0.3125f+src[idxPrevCol+1]*-0.0938f
114 +src[idxNextCol-1]*0.0938f+src[idxNextCol]*0.3125f+src[idxNextCol+1]*0.0938f;
115 dY[idx]=src[idxPrevCol-1]*-0.0938f+src[idx-1]*-0.3125f+src[idxNextCol-1]*-0.0938f
116 +src[idxPrevCol+1]*0.0938f+src[idx+1]*0.3125f+src[idxNextCol+1]*0.0938f;
117/*
118 dX[idx]=(src[idxPrevCol-1]*-1+src[idxPrevCol]*-2+src[idxPrevCol+1]*-1
119 +src[idxNextCol-1]*1+src[idxNextCol]*2+src[idxNextCol+1]*1)*0.0625;
120 dY[idx]=(src[idxPrevCol-1]*-1+src[idx-1]*-2+src[idxNextCol-1]*-1
121 +src[idxPrevCol+1]*1+src[idx+1]*2+src[idxNextCol+1]*1)*0.0625;
122*/
123 }
124 }
125}
126
127void calcGoodFeature(double *dX, double *dY, int sizeY, int sizeX, int winSize, double* lambda, double* tr, double* det,
128 double* c_xx, double* c_xy, double* c_yy){
129 double *xx, *xy, *yy;
130 int i,j,idx;
131 xx=(double*)malloc(sizeof(double)*sizeY*sizeX);
132 xy=(double*)malloc(sizeof(double)*sizeY*sizeX);
133 yy=(double*)malloc(sizeof(double)*sizeY*sizeX);
134 for(idx=0; idx<sizeX*sizeY; idx++){
135 xx[idx]=dX[idx]*dX[idx];
136 xy[idx]=dX[idx]*dY[idx];
137 yy[idx]=dY[idx]*dY[idx];
138 }
139 calcAreaSum(xx, sizeY, sizeX, winSize, c_xx);
140 calcAreaSum(xy, sizeY, sizeX, winSize, c_xy);
141 calcAreaSum(yy, sizeY, sizeX, winSize, c_yy);
142 for(idx=0; idx<sizeX*sizeY; idx++){
143 tr[idx]=c_xx[idx]+c_yy[idx];
144 det[idx]=c_xx[idx]*c_yy[idx]-c_xy[idx]*c_xy[idx];
145 lambda[idx]=det[idx]/(tr[idx] + 0.00001);
146 }
147 free(xx);
148 free(xy);
149 free(yy);
150}
151void calcGoodFeature(char *dX, char *dY, int sizeY, int sizeX, int winSize, float* lambda, float* tr, float* det){
152 int *xx, *xy, *yy;
153 int *c_xx, *c_xy, *c_yy;
154 int i,j,idx;
155 xx=(int*)malloc(sizeof(int)*sizeY*sizeX);
156 xy=(int*)malloc(sizeof(int)*sizeY*sizeX);
157 yy=(int*)malloc(sizeof(int)*sizeY*sizeX);
158 c_xx=(int*)malloc(sizeof(int)*sizeY*sizeX);
159 c_xy=(int*)malloc(sizeof(int)*sizeY*sizeX);
160 c_yy=(int*)malloc(sizeof(int)*sizeY*sizeX);
161 for(i=0; i<sizeX; i++){
162 for(j=0; j<sizeY; j++){
163 idx=i*sizeY+j;
164 xx[idx]=dX[idx]*dX[idx];
165 xy[idx]=dX[idx]*dY[idx];
166 yy[idx]=dY[idx]*dY[idx];
167 }
168 }
169 calcAreaSum(xx, sizeY, sizeX, winSize, c_xx);
170 calcAreaSum(xy, sizeY, sizeX, winSize, c_xy);
171 calcAreaSum(yy, sizeY, sizeX, winSize, c_yy);
172 for(i=0; i<sizeX; i++){
173 for(j=0; j<sizeY; j++){
174 idx=i*sizeY+j;
175 tr[idx]=c_xx[idx]+c_yy[idx];
176 det[idx]=c_xx[idx]*c_yy[idx]-c_xy[idx]*c_xy[idx];
177 lambda[i*sizeY+j]=det[idx]/(tr[idx] + 0.00001);
178 }
179 }
180}
181void calcMinEigenValue(char *dX, char *dY, int sizeY, int sizeX, float* lambda){
182 int xx, xy, yy;
183 int tr;
184 int i,j;
185 for(i=0; i<sizeX; i++){
186 for(j=0; j<sizeY; j++){
187 xx=dX[i*sizeY+j]*dX[i*sizeY+j];
188 xy=dX[i*sizeY+j]*dY[i*sizeY+j];
189 yy=dY[i*sizeY+j]*dY[i*sizeY+j];
190 tr=xx+yy;
191 lambda[i*sizeY+j]=(float)(tr-pow((xx-yy)*(xx-yy)+4*xy*xy,0.5))/2;
192 }
193 }
194}
195
196void calcAreaSum(double *src, int sizeY, int sizeX, int winSize, double *ret){
197/**/
198 const int MAX_COLS = 1024;
199 double *a_array[MAX_COLS], *b_array[MAX_COLS];
200 double a1[MAX_COLS], a1sum;
201
202 int nave = winSize;
203 int nave_half = (int)floor((nave+1)/2)-1;
204
205 double *a_ptr = src;
206
207 int nx = sizeY;
208 int ny = sizeX;
209
210 double *b_ptr = ret;
211
212 // Construct array pointers
213 for (int iy = 0; iy < ny; iy++) {
214 a_array[iy] = a_ptr + iy*nx;
215 b_array[iy] = b_ptr + iy*nx;
216 }
217
218 // Initialize a1 array
219 for (int i = 0; i < nx+nave; i++)
220 a1[i] = 0.0;
221 // Sum over cols:
222 for (int iy = 0; iy < ny; iy++) {
223 // Copy col into temp array:
224 for (int ix = 0; ix < nx; ix++)
225 a1[ix+nave_half] = a_array[iy][ix];
226
227 a1sum = 0.0;
228 for (int i = 0; i < nave; i++)
229 a1sum += a1[i];
230
231 for (int ix = 0; ix < nx; ix++) {
232 b_array[iy][ix] = a1sum;
233 a1sum += a1[ix+nave]-a1[ix];
234 }
235 }
236 // Re-initialize a1 array
237 for (int i = 0; i < ny+nave; i++)
238 a1[i] = 0.0;
239 // Sum over rows:
240 for (int ix = 0; ix < nx; ix++) {
241 // Copy row into temp array:
242 for (int iy = 0; iy < ny; iy++)
243 a1[iy+nave_half] = b_array[iy][ix];
244
245 a1sum = 0.0;
246 for (int i = 0; i < nave; i++)
247 a1sum += a1[i];
248
249 for (int iy = 0; iy < ny; iy++) {
250 b_array[iy][ix] = a1sum;
251 a1sum += a1[iy+nave]-a1[iy];
252 }
253 }
254/**/
255 // 8n^2+2n
256/*
257 // 10n^2
258 int idx, idxCol, idx_1, idx_win, idx_Y, idx_win_Y;
259 double* sum=(double*)malloc(sizeof(double)*sizeX*sizeY);
260 //sum up along Y
261 for(idx=idxCol=0; idx<(sizeX*sizeY); idxCol=idx){
262 sum[idx]=src[idx];
263 idx++;
264 for(idx_1=idx-1;idx<(idxCol+winSize);idx++,
265 idx_1++){
266 sum[idx]=sum[idx_1]+src[idx];
267 }
268 for(idx_win=idx-winSize;idx<(idxCol+sizeY);idx++,
269 idx_1++,
270 idx_win++){
271 sum[idx]=sum[idx_1]+src[idx]-src[idx_win];
272 }
273 }
274 //sum up along X
275 for(idx=0;idx<sizeY;idx++){
276 ret[idx]=sum[idx];
277 }
278 for(idx_Y=idx-sizeY;idx<(winSize*sizeY);idx++,
279 idx_Y++){
280 ret[idx]+=sum[idx]+ret[idx_Y];
281 }
282 for(idx_win_Y=idx-winSize*sizeY;idx<(sizeX*sizeY); idx++,
283 idx_win_Y++,
284 idx_Y++){
285 ret[idx]+=sum[idx]+ret[idx_Y]-sum[idx_win_Y];
286 }
287 free(sum);
288/**/
289/*
290 // 8n^2+2n
291 int i,j;
292 int idx,idxCol;
293 int idxDelta[4];
294 double* sum=(double*)malloc(sizeof(double)*sizeX*sizeY);
295 int halfSize=(int)winSize/2;
296 ret[0]=src[0];
297 for(i=1; i<sizeY; i++){
298 sum[i]=src[i]+sum[i-1];
299 }
300 for(i=1; i<sizeX; i++){
301 idx=i*sizeY;
302 sum[idx]=src[idx]+sum[idx-sizeY];
303 }
304 for(i=1; i<sizeX; i++){
305 for(j=1; j<sizeY; j++){
306 idx=i*sizeY+j;
307 sum[idx]=src[idx]
308 + sum[idx-1]
309 + sum[idx-sizeY]
310 - sum[idx-sizeY-1];
311 }
312 }
313 idxDelta[0]=halfSize*sizeY+halfSize;
314 idxDelta[1]=-halfSize*sizeY+halfSize;
315 idxDelta[2]=halfSize*sizeY-halfSize;
316 idxDelta[3]=-halfSize*sizeY-halfSize;
317 for(i=halfSize; i<sizeX-halfSize; i++){
318 for(j=halfSize; j<sizeY-halfSize; j++){
319 idx=i*sizeY+j;
320 ret[i*sizeY+j]=sum[idx+idxDelta[0]]-sum[idx+idxDelta[1]]-sum[idx+idxDelta[2]]+sum[idx+idxDelta[3]];
321 }
322 }
323 free(sum);
324/**/
325}
326void calcAreaSum(int *src, int sizeY, int sizeX, int sizeSum, int *ret){
327 // 8n^2+2n
328 int i,j;
329 int idx,idxCol;
330 int idxDelta[4];
331 int *sum;
332 sum=(int*)malloc(sizeof(int)*sizeY*sizeX);
333 int halfSize=(int)sizeSum/2;
334 ret[0]=src[0];
335 for(i=1; i<sizeY; i++){
336 sum[i]=src[i]+sum[i-1];
337 }
338 for(i=0; i<sizeX; i++){
339 idx=i*sizeY;
340 sum[idx]=src[idx]+sum[idx-sizeY];
341 }
342 for(i=1; i<sizeX; i++){
343 for(j=1; j<sizeY; j++){
344 idx=i*sizeY+j;
345 sum[idx]=src[idx]
346 + sum[idx-1]
347 + sum[idx-sizeY]
348 - sum[idx-sizeY-1];
349 }
350 }
351/*
352 for(i=0; i<sizeX; i++){
353 for(j=0; j<sizeY; j++){
354 idx=i*sizeY+j;
355 sum[idx]=src[idx]
356 + ((j>0)?sum[idx-1]:0)
357 + ((i>0)?sum[idx-sizeY]:0)
358 - ((i>0&&j>0)?sum[idx-sizeY-1]:0);
359 }
360 }
361*/
362 idxDelta[0]=halfSize*sizeY+halfSize;
363 idxDelta[1]=-halfSize*sizeY+halfSize;
364 idxDelta[2]=halfSize*sizeY-halfSize;
365 idxDelta[3]=-halfSize*sizeY-halfSize;
366 for(i=halfSize; i<sizeX-halfSize; i++){
367 for(j=halfSize; j<sizeY-halfSize; j++){
368 idx=i*sizeY+j;
369 ret[i*sizeY+j]=sum[idx+idxDelta[0]]-sum[idx+idxDelta[1]]-sum[idx+idxDelta[2]]+sum[idx+idxDelta[3]];
370 }
371 }
372
373 free(sum);
374}
375
376
377void getInterpolatePatch(double* srcImg, int* srcDims, double centerX, double centerY, int winSize, double* dstImg){
378 double a, b, a11, a12, a21, a22, b1, b2;
379 int srcIdxX, srcIdx;
380 int dstIdxX, dstIdx;
381 a=centerX-(double)((int)centerX);
382 b=centerY-(double)((int)centerY);
383 a11=(1.f-a)*(1.f-b);
384 a12=a*(1.f-b);
385 a21=(1.f-a)*b;
386 a22=a*b;
387 b1=1.f-b;
388 b2=b;
389
390 for(int i=-winSize; i<winSize; i++){
391 srcIdxX=(int)centerX+i;
392 dstIdxX=i+winSize;
393 for(int j=-winSize; j<winSize; j++){
394 srcIdx=(int)srcIdxX*srcDims[0]+(int)centerY+j;
395 dstIdx=dstIdxX*2*winSize+j+winSize;
396 dstImg[dstIdx]=srcImg[srcIdx]*a11
397 +srcImg[srcIdx+1]*a12
398 +srcImg[srcIdx+srcDims[0]]*a21
399 +srcImg[srcIdx+srcDims[0]+1]*a22;
400 }
401 }
402}
403void calcPyrLKTrack(double** iP, double** iDxP, double** iDyP, double** jP, const int* imgDims, int pLevel,
404 double* fPnt, int nFeatures, int winSize,
405 double* newFPnt, char* valid){
406 calcPyrLKTrack( iP, iDxP, iDyP, jP, imgDims, pLevel,
407 fPnt, nFeatures, winSize,
408 newFPnt, valid, LK_ACCURACY_TH, LK_MAX_ITER);
409}
410void calcPyrLKTrackWInit(double** iP, double** jP, const int* imgDims, int pLevel,
411 double* fPnt, int nFeatures, int winSize,
412 double* newFPnt, double* initFPnt,
413 char* valid, double accuracy_th, int max_iter){
414
415 double x, y, eX, eY, dX, dY, mX, mY, prevMX, prevMY, c_xx, c_xy, c_yy, c_det, dIt;
416 double* iPatch, *jPatch, *iDxPatch, *iDyPatch;
417 int level, winSizeSq, winSizeSqWBorder;
418 int i, k, idx;
419 int imgSize[2];
420
421 static const double rate[]={1, 0.5, 0.25, 0.125, 0.0625, 0.03125};
422 winSizeSq=4*winSize*winSize;
423 winSizeSqWBorder=4*(winSize+1)*(winSize+1);
424
425 iPatch=(double*) malloc(sizeof(double)*winSizeSqWBorder);
426 jPatch=(double*) malloc(sizeof(double)*winSizeSq);
427 iDxPatch=(double*) malloc(sizeof(double)*winSizeSqWBorder);
428 iDyPatch=(double*) malloc(sizeof(double)*winSizeSqWBorder);
429
430 for(i=0; i<nFeatures; i++){
431 dX=(initFPnt[i*2+0]-fPnt[i*2+0])*rate[pLevel];
432 dY=(initFPnt[i*2+1]-fPnt[i*2+1])*rate[pLevel];
433 x=fPnt[i*2+0]*rate[pLevel];//half size of real level
434 y=fPnt[i*2+1]*rate[pLevel];
435 for(level=pLevel-1; level>=0; level--){
436 x+=x; y+=y; dX+=dX; dY+=dY;
437 imgSize[0]=imgDims[level*2+0]; //y,x
438 imgSize[1]=imgDims[level*2+1]; //y,x
439
440 c_xx=c_xy=c_yy=0;
441 //when feature goes out to the boundary.
442 if((x-winSize-1)<0 || (y-winSize-1)<0
443 || (y+winSize+1+1)>=imgSize[0] || (x+winSize+1+1)>=imgSize[1]){
444 //winSize+1due to interpolation
445 //error or skip the level??
446 valid[i]=0;
447 break;
448 }
449
450 getInterpolatePatch(iP[level], imgSize, x, y, winSize+1, iPatch); //to calculate iDx, iDy
451 calcSobel(iPatch, (winSize+1)*2, (winSize+1)*2, iDxPatch, iDyPatch);
452 for(k=0; k <(winSize*2); k++ ){
453 memcpy( iPatch + k*winSize*2, iPatch + (k+1)*(winSize+1)*2 + 1, winSize*2 );
454 memcpy( iDxPatch + k*winSize*2, iDxPatch + (k+1)*(winSize+1)*2 + 1, winSize*2 );
455 memcpy( iDyPatch + k*winSize*2, iDyPatch + (k+1)*(winSize+1)*2 + 1, winSize*2 );
456 }
457
458 for(idx=0; idx<winSizeSq;idx++){
459 c_xx+=iDxPatch[idx]*iDxPatch[idx];
460 c_xy+=iDxPatch[idx]*iDyPatch[idx];
461 c_yy+=iDyPatch[idx]*iDyPatch[idx];
462 }
463 c_det=c_xx*c_yy-c_xy*c_xy;
464 if(c_det/(c_xx+c_yy+0.00000001)<GOOD_FEATURE_LAMBDA_TH){
465 //just skip?
466 valid[i]=0;
467 break;
468 }
469 c_det=1/c_det;
470 for(k=0; k<max_iter; k++){
471 if((x+dX-winSize)<0 || (y+dY-winSize)<0
472 || (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){
473 //winSize+1due to interpolation
474 //error or skip the level??
475 valid[i]=0;
476 break;
477 }
478 getInterpolatePatch(jP[level], imgSize, x+dX, y+dY, winSize, jPatch);
479 eX=eY=0;
480 for(idx=0;idx<winSizeSq;idx++){
481 dIt=iPatch[idx]-jPatch[idx];
482 eX+=dIt*iDxPatch[idx];
483 eY+=dIt*iDyPatch[idx];
484 }
485 mX=c_det*(eX*c_yy-eY*c_xy);
486 mY=c_det*(-eX*c_xy+eY*c_xx);
487 dX+=mX;
488 dY+=mY;
489 if((mX*mX+mY*mY)<accuracy_th) break;
490 if( k>0 && (mX + prevMX) < 0.01 && (mX+prevMX) > -0.01
491 && (mY + prevMY) < 0.01 && (mY+prevMY) > -0.01)
492 {
493 dX -= mX*0.5f;
494 dY -= mY*0.5f;
495 break;
496 }
497 prevMX=mX;
498 prevMY=mY;
499 }
500 if(k==max_iter){
501 valid[i]=0;
502 }
503 }
504 newFPnt[i*2+0]=fPnt[i*2+0]+dX;
505 newFPnt[i*2+1]=fPnt[i*2+1]+dY;
506/*
507 if(valid[i]
508 || (x+dX-winSize)<0 || (y+dY-winSize)<0
509 || (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){
510 newFPnt[i*2+0]=fPnt[i*2+0]+dX;
511 newFPnt[i*2+1]=fPnt[i*2+1]+dY;
512 getInterpolatePatch(jP[0], imgSize, newFPnt[i*2+0], newFPnt[i*2+0], winSize, jPatch);
513 dIt=0;
514 for(idx=0;idx<winSizeSq;idx++){
515 dIt+=(iPatch[idx]-jPatch[idx])*(iPatch[idx]-jPatch[idx]);
516 }
517 if(dIt>winSizeSq*50000){
518 valid[i]=0;
519 }
520 }else{
521 newFPnt[i*2+0]=0;
522 newFPnt[i*2+1]=0;
523 }
524*/
525 }
526 free(iPatch);
527 free(jPatch);
528 free(iDxPatch);
529 free(iDyPatch);
530}
531void calcPyrLKTrackWInit(double** iP, double** iDxP, double** iDyP, double** jP, const int* imgDims, int pLevel,
532 double* fPnt, int nFeatures, int winSize,
533 double* newFPnt, double* initFPnt,
534 char* valid, double accuracy_th, int max_iter){
535
536 double x, y, eX, eY, dX, dY, mX, mY, prevMX, prevMY, c_xx, c_xy, c_yy, c_det, dIt;
537 double* iPatch, *jPatch, *iDxPatch, *iDyPatch;
538 int level, winSizeSq;
539 int i, k, idx;
540 int imgSize[2];
541
542 static const double rate[]={1, 0.5, 0.25, 0.125, 0.0625, 0.03125};
543 winSizeSq=4*winSize*winSize;
544
545 iPatch=(double*) malloc(sizeof(double)*winSizeSq);
546 jPatch=(double*) malloc(sizeof(double)*winSizeSq);
547 iDxPatch=(double*) malloc(sizeof(double)*winSizeSq);
548 iDyPatch=(double*) malloc(sizeof(double)*winSizeSq);
549
550 for(i=0; i<nFeatures; i++){
551 dX=(initFPnt[i*2+0]-fPnt[i*2+0])*rate[pLevel];
552 dY=(initFPnt[i*2+1]-fPnt[i*2+1])*rate[pLevel];
553 x=fPnt[i*2+0]*rate[pLevel];//half size of real level
554 y=fPnt[i*2+1]*rate[pLevel];
555 for(level=pLevel-1; level>=0; level--){
556 x+=x; y+=y; dX+=dX; dY+=dY;
557 imgSize[0]=imgDims[level*2+0]; //y,x
558 imgSize[1]=imgDims[level*2+1]; //y,x
559
560 c_xx=c_xy=c_yy=0;
561 //when feature goes out to the boundary.
562 if((x-winSize)<0 || (y-winSize)<0
563 || (y+winSize+1)>=imgSize[0] || (x+winSize+1)>=imgSize[1]){
564 //winSize+1due to interpolation
565 //error or skip the level??
566 valid[i]=0;
567 break;
568 }
569
570 getInterpolatePatch(iP[level], imgSize, x, y, winSize, iPatch);
571 getInterpolatePatch(iDxP[level], imgSize, x, y, winSize, iDxPatch);
572 getInterpolatePatch(iDyP[level], imgSize, x, y, winSize, iDyPatch);
573
574 for(idx=0; idx<winSizeSq;idx++){
575 c_xx+=iDxPatch[idx]*iDxPatch[idx];
576 c_xy+=iDxPatch[idx]*iDyPatch[idx];
577 c_yy+=iDyPatch[idx]*iDyPatch[idx];
578 }
579 c_det=c_xx*c_yy-c_xy*c_xy;
580 if(c_det/(c_xx+c_yy+0.00000001)<GOOD_FEATURE_LAMBDA_TH){
581 //just skip?
582 valid[i]=0;
583 break;
584 }
585 c_det=1/c_det;
586 for(k=0; k<max_iter; k++){
587 if((x+dX-winSize)<0 || (y+dY-winSize)<0
588 || (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){
589 //winSize+1due to interpolation
590 //error or skip the level??
591 valid[i]=0;
592 break;
593 }
594 getInterpolatePatch(jP[level], imgSize, x+dX, y+dY, winSize, jPatch);
595 eX=eY=0;
596 for(idx=0;idx<winSizeSq;idx++){
597 dIt=iPatch[idx]-jPatch[idx];
598 eX+=dIt*iDxPatch[idx];
599 eY+=dIt*iDyPatch[idx];
600 }
601 mX=c_det*(eX*c_yy-eY*c_xy);
602 mY=c_det*(-eX*c_xy+eY*c_xx);
603 dX+=mX;
604 dY+=mY;
605 if((mX*mX+mY*mY)<accuracy_th) break;
606 if( k>0 && (mX + prevMX) < 0.01 && (mX+prevMX) > -0.01
607 && (mY + prevMY) < 0.01 && (mY+prevMY) > -0.01)
608 {
609 dX -= mX*0.5f;
610 dY -= mY*0.5f;
611 break;
612 }
613 prevMX=mX;
614 prevMY=mY;
615 }
616 if(k==max_iter){
617 valid[i]=0;
618 }
619 }
620 newFPnt[i*2+0]=fPnt[i*2+0]+dX;
621 newFPnt[i*2+1]=fPnt[i*2+1]+dY;
622/*
623 if(valid[i]
624 || (x+dX-winSize)<0 || (y+dY-winSize)<0
625 || (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){
626 newFPnt[i*2+0]=fPnt[i*2+0]+dX;
627 newFPnt[i*2+1]=fPnt[i*2+1]+dY;
628 getInterpolatePatch(jP[0], imgSize, newFPnt[i*2+0], newFPnt[i*2+0], winSize, jPatch);
629 dIt=0;
630 for(idx=0;idx<winSizeSq;idx++){
631 dIt+=(iPatch[idx]-jPatch[idx])*(iPatch[idx]-jPatch[idx]);
632 }
633 if(dIt>winSizeSq*50000){
634 valid[i]=0;
635 }
636 }else{
637 newFPnt[i*2+0]=0;
638 newFPnt[i*2+1]=0;
639 }
640*/
641 }
642 free(iPatch);
643 free(jPatch);
644 free(iDxPatch);
645 free(iDyPatch);
646}
647void calcPyrLKTrack(double** iP, double** iDxP, double** iDyP, double** jP, const int* imgDims, int pLevel,
648 double* fPnt, int nFeatures, int winSize,
649 double* newFPnt, char* valid, double accuracy_th, int max_iter){
650
651 double x, y, eX, eY, dX, dY, mX, mY, c_xx, c_xy, c_yy, c_det, dIt;
652 double* iPatch, *jPatch, *iDxPatch, *iDyPatch;
653 int level, winSizeSq;
654 int i, k, idx;
655 int imgSize[2];
656
657 static const double rate[]={1, 0.5, 0.25, 0.125, 0.0625, 0.03125};
658 winSizeSq=4*winSize*winSize;
659
660 iPatch=(double*) malloc(sizeof(double)*winSizeSq);
661 jPatch=(double*) malloc(sizeof(double)*winSizeSq);
662 iDxPatch=(double*) malloc(sizeof(double)*winSizeSq);
663 iDyPatch=(double*) malloc(sizeof(double)*winSizeSq);
664
665 for(i=0; i<nFeatures; i++){
666 dX=0;
667 dY=0;
668 x=fPnt[i*2+0]*rate[pLevel];//half size of real level
669 y=fPnt[i*2+1]*rate[pLevel];
670 for(level=pLevel-1; level>=0; level--){
671 x+=x; y+=y; dX+=dX; dY+=dY;
672 imgSize[0]=imgDims[level*2+0]; //y,x
673 imgSize[1]=imgDims[level*2+1]; //y,x
674
675 c_xx=c_xy=c_yy=0;
676 //when feature goes out to the boundary.
677 if((x-winSize)<0 || (y-winSize)<0
678 || (y+winSize+1)>=imgSize[0] || (x+winSize+1)>=imgSize[1]){
679 //winSize+1due to interpolation
680 //error or skip the level??
681 valid[i]=0;
682 break;
683 }
684
685 getInterpolatePatch(iP[level], imgSize, x, y, winSize, iPatch);
686 getInterpolatePatch(iDxP[level], imgSize, x, y, winSize, iDxPatch);
687 getInterpolatePatch(iDyP[level], imgSize, x, y, winSize, iDyPatch);
688
689 for(idx=0; idx<winSizeSq;idx++){
690 c_xx+=iDxPatch[idx]*iDxPatch[idx];
691 c_xy+=iDxPatch[idx]*iDyPatch[idx];
692 c_yy+=iDyPatch[idx]*iDyPatch[idx];
693 }
694 c_det=c_xx*c_yy-c_xy*c_xy;
695 if(c_det/(c_xx+c_yy+0.00000001)<GOOD_FEATURE_LAMBDA_TH){
696 valid[i]=0;
697 break;
698 }
699 c_det=1/c_det;
700 for(k=0; k<max_iter; k++){
701 if((x+dX-winSize)<0 || (y+dY-winSize)<0
702 || (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){
703 //winSize+1due to interpolation
704 //error or skip the level??
705 valid[i]=0;
706 break;
707 }
708 getInterpolatePatch(jP[level], imgSize, x+dX, y+dY, winSize, jPatch);
709 eX=eY=0;
710 for(idx=0;idx<winSizeSq;idx++){
711 dIt=iPatch[idx]-jPatch[idx];
712 eX+=dIt*iDxPatch[idx];
713 eY+=dIt*iDyPatch[idx];
714 }
715 mX=c_det*(eX*c_yy-eY*c_xy);
716 mY=c_det*(-eX*c_xy+eY*c_xx);
717 dX+=mX;
718 dY+=mY;
719 if((mX*mX+mY*mY)<accuracy_th) break;
720 }
721 }
722 newFPnt[i*2+0]=fPnt[i*2+0]+dX;
723 newFPnt[i*2+1]=fPnt[i*2+1]+dY;
724/*
725 if(valid[i]
726 || (x+dX-winSize)<0 || (y+dY-winSize)<0
727 || (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){
728 newFPnt[i*2+0]=fPnt[i*2+0]+dX;
729 newFPnt[i*2+1]=fPnt[i*2+1]+dY;
730 getInterpolatePatch(jP[0], imgSize, newFPnt[i*2+0], newFPnt[i*2+0], winSize, jPatch);
731 dIt=0;
732 for(idx=0;idx<winSizeSq;idx++){
733 dIt+=(iPatch[idx]-jPatch[idx])*(iPatch[idx]-jPatch[idx]);
734 }
735 if(dIt>winSizeSq*50000){
736 valid[i]=0;
737 }
738 }else{
739 newFPnt[i*2+0]=0;
740 newFPnt[i*2+1]=0;
741 }
742*/
743 }
744 free(iPatch);
745 free(jPatch);
746 free(iDxPatch);
747 free(iDyPatch);
748}
749void getPatch(double* srcImg, const int* srcDims, double centerX, double centerY, int winSize, double** dstImg){
750 int srcIdxX, srcIdxY, dstIdxX;
751 srcIdxY=(int)centerY-winSize;
752 for(int i=-winSize; i<winSize; i++){
753 srcIdxX=(int)centerX+i;
754 dstIdxX=i+winSize;
755 dstImg[dstIdxX]=srcImg+srcIdxX*winSize*2+srcIdxY;
756 }
757}
758void calcLKTrack(double* imgI, double* iDx, double* iDy, double* imgJ, const int* imdims,
759 double* c_xx, double* c_xy, double* c_yy,
760 double* fPnt, double* initPnt, int nFeatures, int winSize,
761 double* newFPnt, char* valid){
762 calcLKTrack( imgI, iDx, iDy, imgJ, imdims,
763 c_xx, c_xy, c_yy,
764 fPnt, initPnt, nFeatures, winSize,
765 newFPnt, valid, LK_ACCURACY_TH, LK_MAX_ITER);
766}
767void calcLKTrack(double* imgI, double* iDx, double* iDy, double* imgJ, const int* imdims,
768 double* c_xx, double* c_xy, double* c_yy,
769 double* fPnt, double* initPnt, int nFeatures, int winSize,
770 double* newFPnt, char* valid,
771 double accuracy_th, int max_iter){
772 double x, y, eX, eY, dX, dY, mX, mY, c_det, dIt;
773 double* iPatch[LK_MAX_WIN], *iDxPatch[LK_MAX_WIN], *iDyPatch[LK_MAX_WIN];
774 double* jPatch;
775 int level, winSizeSq;
776 int i, k, idxCol, idxRow, idx;
777 int imgSize[2];
778
779 jPatch=(double*) malloc(sizeof(double)*winSizeSq);
780 winSizeSq=4*winSize*winSize;
781 imgSize[0]=imdims[0];
782 imgSize[1]=imdims[1];
783 for(i=0; i<nFeatures; i++){
784 x=fPnt[i*2+0];//half size of real level
785 y=fPnt[i*2+1];
786 dX=initPnt[i*2+0]-x;
787 dY=initPnt[i*2+1]-y;
788//printf("input dx dy %.2f %.2f:", dX, dY);
789
790 //when feature goes out to the boundary.
791 if((x-winSize)<0 || (y-winSize)<0
792 || (y+winSize)>=imdims[0] || (x+winSize)>=imdims[1]){
793 //error or skip the level??
794 valid[i]=0;
795 continue;
796 }
797
798 getPatch(imgI, imdims, x, y, winSize, iPatch);
799 getPatch(iDx, imdims, x, y, winSize, iDxPatch);
800 getPatch(iDy, imdims, x, y, winSize, iDyPatch);
801
802 idx=(int)x*imdims[0]+(int)y;
803 c_det=c_xx[i]*c_yy[i]-c_xy[i]*c_xy[i];
804 if(c_det/(c_xx[i]+c_yy[i]+0.00000001)<GOOD_FEATURE_LAMBDA_TH*100){
805 valid[i]=0;
806 continue;
807 }
808 c_det=1/c_det;
809 for(k=0; k<max_iter; k++){
810 if((x+dX-winSize)<0 || (y+dY-winSize)<0
811 || (y+dY+winSize+1)>=imdims[0] || (x+dX+winSize+1)>=imdims[1]){
812 //winSize+1due to interpolation
813 //error or skip the level??
814 valid[i]=0;
815 break;
816 }
817 getInterpolatePatch(imgJ, imgSize, x+dX, y+dY, winSize, jPatch);
818 eX=eY=0;
819 for(idxCol=0;idxCol<2*winSize;idxCol++){
820 for(idxRow=0;idxRow<2*winSize;idxRow++){
821 dIt=iPatch[idxCol][idxRow]-jPatch[idxCol*winSize*2+idxRow];
822 eX+=dIt*iDxPatch[idxCol][idxRow];
823 eY+=dIt*iDyPatch[idxCol][idxRow];
824 }
825 }
826 mX=c_det*(eX*c_yy[i]-eY*c_xy[i]);
827 mY=c_det*(-eX*c_xy[i]+eY*c_xx[i]);
828 dX+=mX;
829 dY+=mY;
830 if((mX*mX+mY*mY)<accuracy_th) break;
831 }
832 if(k==max_iter){
833 valid[i]=0;
834 }
835 newFPnt[i*2+0]=x+dX;
836 newFPnt[i*2+1]=y+dY;
837 }
838 free(jPatch);
839}
840
generated by cgit v1.2.2 (git 2.25.0) at 2025-04-28 00:11:21 -0400