From f618466c25d43f3bae9e40920273bf77de1e1149 Mon Sep 17 00:00:00 2001
From: leochanj105 <leochanj@live.unc.edu>
Date: Mon, 19 Oct 2020 23:09:30 -0400
Subject: initial sd-vbs

initial sd-vbs

add sd-vbs

sd-vbs
---
 SD-VBS/common/toolbox/lagrcv/lagrcv.cpp | 840 ++++++++++++++++++++++++++++++++
 1 file changed, 840 insertions(+)
 create mode 100755 SD-VBS/common/toolbox/lagrcv/lagrcv.cpp

(limited to 'SD-VBS/common/toolbox/lagrcv/lagrcv.cpp')

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 @@
+#include "lagrcv.h"
+/*
+void calcImagePyr(char *src, int sizeY, int sizeX, int level, char *pyr){
+	pyr=(char*) malloc(sizeof(char)*(int)sizeY*sizeX*4/3); 
+	int i, startPnt, nX, nY;
+
+	memcpy(pyr,src, sizeY*sizeX);
+	nY=sizeY;
+	nX=sizeX;
+	for(i=0; i<level; i++){
+		nY = (nY + 1) >> 1;
+        	nX = (nX + 1) >> 1;
+		calcSubsample(pyr, sizeY, sizeX, pyr+sizeY*sizeX)  
+	}
+	free(pyr);
+}
+*/
+
+void calcSubSampleAvg(double *src, int sizeY, int sizeX, double *dest, int destSizeY, int destSizeX){
+	int i, j, idx,
+	 destI, destJ, idxDest;
+	for(i=0, destI=0; destI<destSizeX; i+=2, destI++){
+		for(j=0, destJ=0; destJ<destSizeY; j+=2, destJ++){
+			idx=i*sizeX+j;
+			idxDest=destI*destSizeY+destJ;
+			dest[idxDest]=(src[idx]+src[idx+1]+src[idx+sizeY]+src[idx+sizeY+1])/4;
+		}
+	}
+} 
+void calcImgBlur(double *src, int sizeY, int sizeX, double *dest){
+	double kernel[]={0.0625, 0.25, 0.375, 0.25, 0.0625};
+	double *temp;
+	temp=(double*) malloc(sizeof(double)*sizeY*sizeX);
+	int i, j, idx, idxCol;
+	for(i=2; i<sizeX-2;i++){
+		idxCol=i*sizeY;
+		for(j=0;j<sizeY; j++){
+			idx=idxCol+j;
+			temp[idx]=src[idx-2*sizeY]/16+src[idx-sizeY]/4
+				+src[idx]*3/8+src[idx+sizeY]/4+src[idx+2*sizeY]/16;
+		}
+	}
+	for(i=0; i<sizeX;i++){
+		idxCol=i*sizeY;
+		for(j=2;j<sizeY-2; j++){
+			idx=idxCol+j;
+			dest[idx]=temp[idx-2]/16+temp[idx-1]/4
+				+temp[idx]*3/8+temp[idx+1]/4+temp[idx+2]/16;
+		}
+	}
+	free(temp);
+}
+void calcImgResize(double *src, int sizeY, int sizeX, double *dest, int dstSizeY, int dstSizeX){
+	double ker[]={ 0.0039,   0.0156,   0.0234,   0.0156,   0.0039,
+    			0.0156,   0.0625,   0.0938,   0.0625,   0.0156,
+    			0.0234,   0.0938,   0.1406,   0.0938,   0.0234,
+    			0.0156,   0.0625,   0.0938,   0.0625,   0.0156,
+    			0.0039,   0.0156,   0.0234,   0.0156,   0.0039};
+	int srcI, srcJ, dstI, dstJ, srcIdx, dstIdx, idxCol_1, idxCol1, idxCol_2, idxCol2;
+	for(srcI=2, dstI=1; srcI<sizeX-2; srcI+=2, dstI++){
+		for(srcJ=2, dstJ=1; srcJ<sizeY-2; srcJ+=2, dstJ++){
+			srcIdx=srcI*sizeY+srcJ;
+			dstIdx=dstI*dstSizeY+dstJ;
+			idxCol_1=srcIdx-sizeY;
+			idxCol_2=srcIdx-2*sizeY;
+			idxCol1=srcIdx+sizeY;
+			idxCol2=srcIdx+2*sizeY;
+			dest[dstIdx]=
+			src[idxCol_2-2]*ker[0]+src[idxCol_1-2]*ker[1]
+		+src[srcIdx-2]*ker[2]+src[idxCol1-2]*ker[3]+src[idxCol2-2]*ker[4]
+			+src[idxCol_2-1]*ker[5]+src[idxCol_1-1]*ker[6]
+		+src[srcIdx-1]*ker[7]+src[idxCol1-1]*ker[8]+src[idxCol2-1]*ker[9]
+			+src[idxCol_2]*ker[10]+src[idxCol_1]*ker[11]
+		+src[srcIdx]*ker[12]+src[idxCol1]*ker[13]+src[idxCol2]*ker[14]
+			+src[idxCol_2+1]*ker[5]+src[idxCol_1+1]*ker[16]
+		+src[srcIdx+1]*ker[17]+src[idxCol1+1]*ker[18]+src[idxCol2+1]*ker[19];
+			+src[idxCol_2+2]*ker[20]+src[idxCol_1+2]*ker[21]
+		+src[srcIdx+2]*ker[22]+src[idxCol1+2]*ker[23]+src[idxCol2+2]*ker[24];
+		}
+	}
+}
+
+void calcGradient(double *src, int sizeY, int sizeX, double *dX, double *dY){
+	int i, j, idx;
+	for(i=1; i<sizeX; i++){
+		for(j=1; j<sizeY; j++){
+			idx=i*sizeY+j;
+			dX[idx]=src[idx]-src[idx-sizeY];
+			dY[idx]=src[idx]-src[idx-1];
+		}
+	}
+}
+void calcGradient(char *src, int sizeY, int sizeX, char *dX, char *dY){
+	int i, j, idx;
+	for(i=1; i<sizeX; i++){
+		for(j=1; j<sizeY; j++){
+			idx=i*sizeY+j;
+			dX[idx]=src[idx]-src[idx-sizeY];
+			dY[idx]=src[idx]-src[idx-1];
+		}
+	}
+}
+void calcSobel(double *src, int sizeY, int sizeX, double *dX, double *dY){
+	//char kernelV[9]={-1, -2, -1, 0, 0, 0, 1, 2, 1};
+	//char kernelH[9]={-1, 0, 1, -2, 0, 2, -1, 0, 1};
+	//const int kerSize=3;
+	int i, j, idx, idxPrevCol, idxNextCol;
+	for(i=1; i<sizeX-1; i++){
+		for(j=1; j<sizeY-1; j++){
+			idx=i*sizeY+j;
+			idxPrevCol=idx-sizeY;
+			idxNextCol=idx+sizeY;
+			dX[idx]=src[idxPrevCol-1]*-0.0938f+src[idxPrevCol]*-0.3125f+src[idxPrevCol+1]*-0.0938f
+				+src[idxNextCol-1]*0.0938f+src[idxNextCol]*0.3125f+src[idxNextCol+1]*0.0938f;
+			dY[idx]=src[idxPrevCol-1]*-0.0938f+src[idx-1]*-0.3125f+src[idxNextCol-1]*-0.0938f
+				+src[idxPrevCol+1]*0.0938f+src[idx+1]*0.3125f+src[idxNextCol+1]*0.0938f;
+/*
+			dX[idx]=(src[idxPrevCol-1]*-1+src[idxPrevCol]*-2+src[idxPrevCol+1]*-1
+				+src[idxNextCol-1]*1+src[idxNextCol]*2+src[idxNextCol+1]*1)*0.0625;
+			dY[idx]=(src[idxPrevCol-1]*-1+src[idx-1]*-2+src[idxNextCol-1]*-1
+				+src[idxPrevCol+1]*1+src[idx+1]*2+src[idxNextCol+1]*1)*0.0625;
+*/
+		}
+	}
+}
+
+void calcGoodFeature(double *dX, double *dY, int sizeY, int sizeX, int winSize, double* lambda, double* tr, double* det,
+	double* c_xx, double* c_xy, double* c_yy){
+	double *xx, *xy, *yy;
+	int i,j,idx;
+	xx=(double*)malloc(sizeof(double)*sizeY*sizeX);
+	xy=(double*)malloc(sizeof(double)*sizeY*sizeX);
+	yy=(double*)malloc(sizeof(double)*sizeY*sizeX);
+	for(idx=0; idx<sizeX*sizeY; idx++){
+		xx[idx]=dX[idx]*dX[idx];
+		xy[idx]=dX[idx]*dY[idx];
+		yy[idx]=dY[idx]*dY[idx];
+	}
+	calcAreaSum(xx, sizeY, sizeX, winSize, c_xx);
+	calcAreaSum(xy, sizeY, sizeX, winSize, c_xy);
+	calcAreaSum(yy, sizeY, sizeX, winSize, c_yy);
+	for(idx=0; idx<sizeX*sizeY; idx++){
+		tr[idx]=c_xx[idx]+c_yy[idx];
+		det[idx]=c_xx[idx]*c_yy[idx]-c_xy[idx]*c_xy[idx];
+		lambda[idx]=det[idx]/(tr[idx] + 0.00001);
+	}
+	free(xx);
+	free(xy);
+	free(yy);
+}
+void calcGoodFeature(char *dX, char *dY, int sizeY, int sizeX, int winSize, float* lambda, float* tr, float* det){
+	int *xx, *xy, *yy;
+	int *c_xx, *c_xy, *c_yy;
+	int i,j,idx;
+	xx=(int*)malloc(sizeof(int)*sizeY*sizeX);
+	xy=(int*)malloc(sizeof(int)*sizeY*sizeX);
+	yy=(int*)malloc(sizeof(int)*sizeY*sizeX);
+	c_xx=(int*)malloc(sizeof(int)*sizeY*sizeX);
+	c_xy=(int*)malloc(sizeof(int)*sizeY*sizeX);
+	c_yy=(int*)malloc(sizeof(int)*sizeY*sizeX);
+	for(i=0; i<sizeX; i++){
+		for(j=0; j<sizeY; j++){
+			idx=i*sizeY+j;
+			xx[idx]=dX[idx]*dX[idx];
+			xy[idx]=dX[idx]*dY[idx];
+			yy[idx]=dY[idx]*dY[idx];
+		}
+	}
+	calcAreaSum(xx, sizeY, sizeX, winSize, c_xx);
+	calcAreaSum(xy, sizeY, sizeX, winSize, c_xy);
+	calcAreaSum(yy, sizeY, sizeX, winSize, c_yy);
+	for(i=0; i<sizeX; i++){
+		for(j=0; j<sizeY; j++){
+			idx=i*sizeY+j;
+			tr[idx]=c_xx[idx]+c_yy[idx];
+			det[idx]=c_xx[idx]*c_yy[idx]-c_xy[idx]*c_xy[idx];
+			lambda[i*sizeY+j]=det[idx]/(tr[idx] + 0.00001);
+		}
+	}
+}
+void calcMinEigenValue(char *dX, char *dY, int sizeY, int sizeX, float* lambda){
+	int xx, xy, yy;
+	int tr;
+	int i,j;
+	for(i=0; i<sizeX; i++){
+		for(j=0; j<sizeY; j++){
+			xx=dX[i*sizeY+j]*dX[i*sizeY+j];
+			xy=dX[i*sizeY+j]*dY[i*sizeY+j];
+			yy=dY[i*sizeY+j]*dY[i*sizeY+j];
+			tr=xx+yy;
+			lambda[i*sizeY+j]=(float)(tr-pow((xx-yy)*(xx-yy)+4*xy*xy,0.5))/2;
+		}
+	}
+}
+
+void calcAreaSum(double *src, int sizeY, int sizeX, int winSize, double *ret){
+/**/
+  const int MAX_COLS = 1024;
+  double *a_array[MAX_COLS], *b_array[MAX_COLS];
+  double a1[MAX_COLS], a1sum;
+
+  int nave = winSize;
+  int nave_half = (int)floor((nave+1)/2)-1;
+
+  double *a_ptr = src;
+
+  int nx = sizeY;
+  int ny = sizeX;
+
+  double *b_ptr = ret;
+
+  // Construct array pointers
+  for (int iy = 0; iy < ny; iy++) {
+    a_array[iy] = a_ptr + iy*nx;
+    b_array[iy] = b_ptr + iy*nx;
+  }
+
+  // Initialize a1 array
+  for (int i = 0; i < nx+nave; i++)
+    a1[i] = 0.0;
+  // Sum over cols:
+  for (int iy = 0; iy < ny; iy++) {
+    // Copy col into temp array:
+    for (int ix = 0; ix < nx; ix++)
+      a1[ix+nave_half] = a_array[iy][ix];
+
+    a1sum = 0.0;
+    for (int i = 0; i < nave; i++)
+      a1sum += a1[i];
+
+    for (int ix = 0; ix < nx; ix++) {
+      b_array[iy][ix] = a1sum;
+      a1sum += a1[ix+nave]-a1[ix];
+    }
+  }
+  // Re-initialize a1 array
+  for (int i = 0; i < ny+nave; i++)
+    a1[i] = 0.0;
+  // Sum over rows:
+  for (int ix = 0; ix < nx; ix++) {
+    // Copy row into temp array:
+    for (int iy = 0; iy < ny; iy++)
+      a1[iy+nave_half] = b_array[iy][ix];
+
+    a1sum = 0.0;
+    for (int i = 0; i < nave; i++)
+      a1sum += a1[i];
+
+    for (int iy = 0; iy < ny; iy++) {
+      b_array[iy][ix] = a1sum;
+      a1sum += a1[iy+nave]-a1[iy];
+    }
+  }
+/**/
+	// 8n^2+2n
+/*
+	// 10n^2
+	int idx, idxCol, idx_1, idx_win, idx_Y, idx_win_Y;
+	double* sum=(double*)malloc(sizeof(double)*sizeX*sizeY);
+	//sum up along Y
+	for(idx=idxCol=0; idx<(sizeX*sizeY); idxCol=idx){
+		sum[idx]=src[idx];
+		idx++;
+		for(idx_1=idx-1;idx<(idxCol+winSize);idx++,
+							idx_1++){
+			sum[idx]=sum[idx_1]+src[idx];
+		}
+		for(idx_win=idx-winSize;idx<(idxCol+sizeY);idx++,
+							idx_1++,
+							idx_win++){
+			sum[idx]=sum[idx_1]+src[idx]-src[idx_win];
+		}
+	}
+	//sum up along X
+	for(idx=0;idx<sizeY;idx++){
+		ret[idx]=sum[idx];
+	}
+	for(idx_Y=idx-sizeY;idx<(winSize*sizeY);idx++,
+						idx_Y++){
+		ret[idx]+=sum[idx]+ret[idx_Y];
+	}
+	for(idx_win_Y=idx-winSize*sizeY;idx<(sizeX*sizeY); idx++,
+							idx_win_Y++,
+							idx_Y++){
+		ret[idx]+=sum[idx]+ret[idx_Y]-sum[idx_win_Y];
+	}
+	free(sum);
+/**/
+/*
+	// 8n^2+2n
+	int i,j;
+	int idx,idxCol;
+	int idxDelta[4];
+	double* sum=(double*)malloc(sizeof(double)*sizeX*sizeY);
+	int halfSize=(int)winSize/2;
+	ret[0]=src[0];
+	for(i=1; i<sizeY; i++){
+		sum[i]=src[i]+sum[i-1];
+	}
+	for(i=1; i<sizeX; i++){
+		idx=i*sizeY;
+		sum[idx]=src[idx]+sum[idx-sizeY];
+	}
+	for(i=1; i<sizeX; i++){
+		for(j=1; j<sizeY; j++){
+			idx=i*sizeY+j;
+			sum[idx]=src[idx] 
+				+ sum[idx-1]
+				+ sum[idx-sizeY]
+				- sum[idx-sizeY-1];
+		}
+	}
+	idxDelta[0]=halfSize*sizeY+halfSize;
+	idxDelta[1]=-halfSize*sizeY+halfSize;
+	idxDelta[2]=halfSize*sizeY-halfSize;
+	idxDelta[3]=-halfSize*sizeY-halfSize;
+	for(i=halfSize; i<sizeX-halfSize; i++){
+		for(j=halfSize; j<sizeY-halfSize; j++){
+			idx=i*sizeY+j;
+			ret[i*sizeY+j]=sum[idx+idxDelta[0]]-sum[idx+idxDelta[1]]-sum[idx+idxDelta[2]]+sum[idx+idxDelta[3]];
+		}
+	}
+	free(sum);
+/**/
+} 
+void calcAreaSum(int *src, int sizeY, int sizeX, int sizeSum, int *ret){
+	// 8n^2+2n
+	int i,j;
+	int idx,idxCol;
+	int idxDelta[4];
+	int *sum;
+	sum=(int*)malloc(sizeof(int)*sizeY*sizeX);
+	int halfSize=(int)sizeSum/2;
+	ret[0]=src[0];
+	for(i=1; i<sizeY; i++){
+		sum[i]=src[i]+sum[i-1];
+	}
+	for(i=0; i<sizeX; i++){
+		idx=i*sizeY;
+		sum[idx]=src[idx]+sum[idx-sizeY];
+	}
+	for(i=1; i<sizeX; i++){
+		for(j=1; j<sizeY; j++){
+			idx=i*sizeY+j;
+			sum[idx]=src[idx] 
+				+ sum[idx-1]
+				+ sum[idx-sizeY]
+				- sum[idx-sizeY-1];
+		}
+	}
+/*
+	for(i=0; i<sizeX; i++){
+		for(j=0; j<sizeY; j++){
+			idx=i*sizeY+j;
+			sum[idx]=src[idx] 
+				+ ((j>0)?sum[idx-1]:0)
+				+ ((i>0)?sum[idx-sizeY]:0)
+				- ((i>0&&j>0)?sum[idx-sizeY-1]:0);
+		}
+	}
+*/
+	idxDelta[0]=halfSize*sizeY+halfSize;
+	idxDelta[1]=-halfSize*sizeY+halfSize;
+	idxDelta[2]=halfSize*sizeY-halfSize;
+	idxDelta[3]=-halfSize*sizeY-halfSize;
+	for(i=halfSize; i<sizeX-halfSize; i++){
+		for(j=halfSize; j<sizeY-halfSize; j++){
+			idx=i*sizeY+j;
+			ret[i*sizeY+j]=sum[idx+idxDelta[0]]-sum[idx+idxDelta[1]]-sum[idx+idxDelta[2]]+sum[idx+idxDelta[3]];
+		}
+	}
+
+	free(sum);
+} 
+
+
+void getInterpolatePatch(double* srcImg, int* srcDims, double centerX, double centerY, int winSize, double* dstImg){
+	double a, b, a11, a12, a21, a22, b1, b2;
+	int srcIdxX, srcIdx;
+	int dstIdxX, dstIdx;
+	a=centerX-(double)((int)centerX);
+	b=centerY-(double)((int)centerY);
+	a11=(1.f-a)*(1.f-b);
+	a12=a*(1.f-b);
+	a21=(1.f-a)*b;
+	a22=a*b;
+	b1=1.f-b;
+	b2=b;
+	
+	for(int i=-winSize; i<winSize; i++){
+		srcIdxX=(int)centerX+i;
+		dstIdxX=i+winSize;
+		for(int j=-winSize; j<winSize; j++){
+			srcIdx=(int)srcIdxX*srcDims[0]+(int)centerY+j;
+			dstIdx=dstIdxX*2*winSize+j+winSize;
+			dstImg[dstIdx]=srcImg[srcIdx]*a11
+				+srcImg[srcIdx+1]*a12
+				+srcImg[srcIdx+srcDims[0]]*a21
+				+srcImg[srcIdx+srcDims[0]+1]*a22;
+		}
+	}
+}
+void calcPyrLKTrack(double** iP, double** iDxP, double** iDyP, double** jP, const int* imgDims, int pLevel, 
+        double* fPnt, int nFeatures, int winSize, 
+	double* newFPnt, char* valid){
+        calcPyrLKTrack( iP, iDxP,  iDyP,  jP, imgDims,  pLevel, 
+         fPnt,  nFeatures,  winSize, 
+	 newFPnt,  valid, LK_ACCURACY_TH,  LK_MAX_ITER);
+}
+void calcPyrLKTrackWInit(double** iP, double** jP, const int* imgDims, int pLevel, 
+        double* fPnt, int nFeatures, int winSize, 
+	double* newFPnt, double* initFPnt,
+	char* valid, double accuracy_th, int max_iter){
+
+	double x, y, eX, eY, dX, dY, mX, mY, prevMX, prevMY, c_xx, c_xy, c_yy, c_det, dIt;
+	double* iPatch, *jPatch, *iDxPatch, *iDyPatch;
+	int level, winSizeSq, winSizeSqWBorder;
+	int i, k, idx;
+	int imgSize[2];
+	
+        static const double rate[]={1, 0.5, 0.25, 0.125, 0.0625, 0.03125};
+	winSizeSq=4*winSize*winSize;
+	winSizeSqWBorder=4*(winSize+1)*(winSize+1);
+
+	iPatch=(double*) malloc(sizeof(double)*winSizeSqWBorder);
+	jPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	iDxPatch=(double*) malloc(sizeof(double)*winSizeSqWBorder);
+	iDyPatch=(double*) malloc(sizeof(double)*winSizeSqWBorder);
+
+	for(i=0; i<nFeatures; i++){
+		dX=(initFPnt[i*2+0]-fPnt[i*2+0])*rate[pLevel];
+		dY=(initFPnt[i*2+1]-fPnt[i*2+1])*rate[pLevel];
+		x=fPnt[i*2+0]*rate[pLevel];//half size of real level
+		y=fPnt[i*2+1]*rate[pLevel];
+		for(level=pLevel-1; level>=0; level--){
+			x+=x; y+=y; dX+=dX; dY+=dY;
+			imgSize[0]=imgDims[level*2+0]; //y,x		
+			imgSize[1]=imgDims[level*2+1]; //y,x		
+			
+			c_xx=c_xy=c_yy=0;
+			//when feature goes out to the boundary.
+			if((x-winSize-1)<0 || (y-winSize-1)<0 
+			|| (y+winSize+1+1)>=imgSize[0] || (x+winSize+1+1)>=imgSize[1]){ 
+				//winSize+1due to interpolation
+				//error or skip the level??
+				valid[i]=0;
+				break;
+			} 
+
+			getInterpolatePatch(iP[level], imgSize, x, y, winSize+1, iPatch); //to calculate iDx, iDy
+			calcSobel(iPatch, (winSize+1)*2, (winSize+1)*2, iDxPatch, iDyPatch);
+ 			for(k=0; k <(winSize*2); k++ ){
+                		memcpy( iPatch + k*winSize*2, iPatch + (k+1)*(winSize+1)*2 + 1, winSize*2 );
+                		memcpy( iDxPatch + k*winSize*2, iDxPatch + (k+1)*(winSize+1)*2 + 1, winSize*2 );
+                		memcpy( iDyPatch + k*winSize*2, iDyPatch + (k+1)*(winSize+1)*2 + 1, winSize*2 );
+			}
+
+			for(idx=0; idx<winSizeSq;idx++){
+				c_xx+=iDxPatch[idx]*iDxPatch[idx];
+				c_xy+=iDxPatch[idx]*iDyPatch[idx];
+				c_yy+=iDyPatch[idx]*iDyPatch[idx];
+			}
+			c_det=c_xx*c_yy-c_xy*c_xy;
+			if(c_det/(c_xx+c_yy+0.00000001)<GOOD_FEATURE_LAMBDA_TH){
+				//just skip?
+				valid[i]=0;
+				break;
+			}
+			c_det=1/c_det;
+			for(k=0; k<max_iter; k++){
+				if((x+dX-winSize)<0 || (y+dY-winSize)<0 
+				|| (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){ 
+					//winSize+1due to interpolation
+					//error or skip the level??
+					valid[i]=0;
+					break;
+				}	 
+				getInterpolatePatch(jP[level], imgSize, x+dX, y+dY, winSize, jPatch);
+				eX=eY=0;
+				for(idx=0;idx<winSizeSq;idx++){
+					dIt=iPatch[idx]-jPatch[idx];
+					eX+=dIt*iDxPatch[idx];
+					eY+=dIt*iDyPatch[idx];
+				}
+				mX=c_det*(eX*c_yy-eY*c_xy);
+				mY=c_det*(-eX*c_xy+eY*c_xx);
+				dX+=mX;
+				dY+=mY;
+				if((mX*mX+mY*mY)<accuracy_th) break;
+ 				if( k>0 && (mX + prevMX) < 0.01 && (mX+prevMX) > -0.01
+				&& (mY + prevMY) < 0.01 && (mY+prevMY) > -0.01)
+               		        {
+                    			dX -= mX*0.5f;
+                    			dY -= mY*0.5f;
+                    			break;
+                		}
+				prevMX=mX;
+				prevMY=mY;
+			}
+			if(k==max_iter){
+				valid[i]=0;
+			}
+		}
+		newFPnt[i*2+0]=fPnt[i*2+0]+dX;
+		newFPnt[i*2+1]=fPnt[i*2+1]+dY;
+/*
+		if(valid[i]
+		|| (x+dX-winSize)<0 || (y+dY-winSize)<0 
+		|| (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){ 
+			newFPnt[i*2+0]=fPnt[i*2+0]+dX;
+			newFPnt[i*2+1]=fPnt[i*2+1]+dY;
+			getInterpolatePatch(jP[0], imgSize, newFPnt[i*2+0], newFPnt[i*2+0], winSize, jPatch);
+			dIt=0;
+			for(idx=0;idx<winSizeSq;idx++){
+				dIt+=(iPatch[idx]-jPatch[idx])*(iPatch[idx]-jPatch[idx]);
+			}
+			if(dIt>winSizeSq*50000){
+				valid[i]=0;
+			}
+		}else{
+			newFPnt[i*2+0]=0;
+			newFPnt[i*2+1]=0;
+		}
+*/
+	}
+	free(iPatch);
+	free(jPatch);
+	free(iDxPatch);
+	free(iDyPatch);
+}
+void calcPyrLKTrackWInit(double** iP, double** iDxP, double** iDyP, double** jP, const int* imgDims, int pLevel, 
+        double* fPnt, int nFeatures, int winSize, 
+	double* newFPnt, double* initFPnt,
+	char* valid, double accuracy_th, int max_iter){
+
+	double x, y, eX, eY, dX, dY, mX, mY, prevMX, prevMY, c_xx, c_xy, c_yy, c_det, dIt;
+	double* iPatch, *jPatch, *iDxPatch, *iDyPatch;
+	int level, winSizeSq;
+	int i, k, idx;
+	int imgSize[2];
+	
+        static const double rate[]={1, 0.5, 0.25, 0.125, 0.0625, 0.03125};
+	winSizeSq=4*winSize*winSize;
+
+	iPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	jPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	iDxPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	iDyPatch=(double*) malloc(sizeof(double)*winSizeSq);
+
+	for(i=0; i<nFeatures; i++){
+		dX=(initFPnt[i*2+0]-fPnt[i*2+0])*rate[pLevel];
+		dY=(initFPnt[i*2+1]-fPnt[i*2+1])*rate[pLevel];
+		x=fPnt[i*2+0]*rate[pLevel];//half size of real level
+		y=fPnt[i*2+1]*rate[pLevel];
+		for(level=pLevel-1; level>=0; level--){
+			x+=x; y+=y; dX+=dX; dY+=dY;
+			imgSize[0]=imgDims[level*2+0]; //y,x		
+			imgSize[1]=imgDims[level*2+1]; //y,x		
+			
+			c_xx=c_xy=c_yy=0;
+			//when feature goes out to the boundary.
+			if((x-winSize)<0 || (y-winSize)<0 
+			|| (y+winSize+1)>=imgSize[0] || (x+winSize+1)>=imgSize[1]){ 
+				//winSize+1due to interpolation
+				//error or skip the level??
+				valid[i]=0;
+				break;
+			} 
+
+			getInterpolatePatch(iP[level], imgSize, x, y, winSize, iPatch);
+			getInterpolatePatch(iDxP[level], imgSize, x, y, winSize, iDxPatch);
+			getInterpolatePatch(iDyP[level], imgSize, x, y, winSize, iDyPatch);
+
+			for(idx=0; idx<winSizeSq;idx++){
+				c_xx+=iDxPatch[idx]*iDxPatch[idx];
+				c_xy+=iDxPatch[idx]*iDyPatch[idx];
+				c_yy+=iDyPatch[idx]*iDyPatch[idx];
+			}
+			c_det=c_xx*c_yy-c_xy*c_xy;
+			if(c_det/(c_xx+c_yy+0.00000001)<GOOD_FEATURE_LAMBDA_TH){
+				//just skip?
+				valid[i]=0;
+				break;
+			}
+			c_det=1/c_det;
+			for(k=0; k<max_iter; k++){
+				if((x+dX-winSize)<0 || (y+dY-winSize)<0 
+				|| (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){ 
+					//winSize+1due to interpolation
+					//error or skip the level??
+					valid[i]=0;
+					break;
+				}	 
+				getInterpolatePatch(jP[level], imgSize, x+dX, y+dY, winSize, jPatch);
+				eX=eY=0;
+				for(idx=0;idx<winSizeSq;idx++){
+					dIt=iPatch[idx]-jPatch[idx];
+					eX+=dIt*iDxPatch[idx];
+					eY+=dIt*iDyPatch[idx];
+				}
+				mX=c_det*(eX*c_yy-eY*c_xy);
+				mY=c_det*(-eX*c_xy+eY*c_xx);
+				dX+=mX;
+				dY+=mY;
+				if((mX*mX+mY*mY)<accuracy_th) break;
+ 				if( k>0 && (mX + prevMX) < 0.01 && (mX+prevMX) > -0.01
+				&& (mY + prevMY) < 0.01 && (mY+prevMY) > -0.01)
+               		        {
+                    			dX -= mX*0.5f;
+                    			dY -= mY*0.5f;
+                    			break;
+                		}
+				prevMX=mX;
+				prevMY=mY;
+			}
+			if(k==max_iter){
+				valid[i]=0;
+			}
+		}
+		newFPnt[i*2+0]=fPnt[i*2+0]+dX;
+		newFPnt[i*2+1]=fPnt[i*2+1]+dY;
+/*
+		if(valid[i]
+		|| (x+dX-winSize)<0 || (y+dY-winSize)<0 
+		|| (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){ 
+			newFPnt[i*2+0]=fPnt[i*2+0]+dX;
+			newFPnt[i*2+1]=fPnt[i*2+1]+dY;
+			getInterpolatePatch(jP[0], imgSize, newFPnt[i*2+0], newFPnt[i*2+0], winSize, jPatch);
+			dIt=0;
+			for(idx=0;idx<winSizeSq;idx++){
+				dIt+=(iPatch[idx]-jPatch[idx])*(iPatch[idx]-jPatch[idx]);
+			}
+			if(dIt>winSizeSq*50000){
+				valid[i]=0;
+			}
+		}else{
+			newFPnt[i*2+0]=0;
+			newFPnt[i*2+1]=0;
+		}
+*/
+	}
+	free(iPatch);
+	free(jPatch);
+	free(iDxPatch);
+	free(iDyPatch);
+}
+void calcPyrLKTrack(double** iP, double** iDxP, double** iDyP, double** jP, const int* imgDims, int pLevel, 
+        double* fPnt, int nFeatures, int winSize, 
+	double* newFPnt, char* valid, double accuracy_th, int max_iter){
+
+	double x, y, eX, eY, dX, dY, mX, mY, c_xx, c_xy, c_yy, c_det, dIt;
+	double* iPatch, *jPatch, *iDxPatch, *iDyPatch;
+	int level, winSizeSq;
+	int i, k, idx;
+	int imgSize[2];
+	
+        static const double rate[]={1, 0.5, 0.25, 0.125, 0.0625, 0.03125};
+	winSizeSq=4*winSize*winSize;
+
+	iPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	jPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	iDxPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	iDyPatch=(double*) malloc(sizeof(double)*winSizeSq);
+
+	for(i=0; i<nFeatures; i++){
+		dX=0;
+		dY=0;
+		x=fPnt[i*2+0]*rate[pLevel];//half size of real level
+		y=fPnt[i*2+1]*rate[pLevel];
+		for(level=pLevel-1; level>=0; level--){
+			x+=x; y+=y; dX+=dX; dY+=dY;
+			imgSize[0]=imgDims[level*2+0]; //y,x		
+			imgSize[1]=imgDims[level*2+1]; //y,x		
+			
+			c_xx=c_xy=c_yy=0;
+			//when feature goes out to the boundary.
+			if((x-winSize)<0 || (y-winSize)<0 
+			|| (y+winSize+1)>=imgSize[0] || (x+winSize+1)>=imgSize[1]){ 
+				//winSize+1due to interpolation
+				//error or skip the level??
+				valid[i]=0;
+				break;
+			} 
+
+			getInterpolatePatch(iP[level], imgSize, x, y, winSize, iPatch);
+			getInterpolatePatch(iDxP[level], imgSize, x, y, winSize, iDxPatch);
+			getInterpolatePatch(iDyP[level], imgSize, x, y, winSize, iDyPatch);
+
+			for(idx=0; idx<winSizeSq;idx++){
+				c_xx+=iDxPatch[idx]*iDxPatch[idx];
+				c_xy+=iDxPatch[idx]*iDyPatch[idx];
+				c_yy+=iDyPatch[idx]*iDyPatch[idx];
+			}
+			c_det=c_xx*c_yy-c_xy*c_xy;
+			if(c_det/(c_xx+c_yy+0.00000001)<GOOD_FEATURE_LAMBDA_TH){
+				valid[i]=0;
+				break;
+			}
+			c_det=1/c_det;
+			for(k=0; k<max_iter; k++){
+				if((x+dX-winSize)<0 || (y+dY-winSize)<0 
+				|| (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){ 
+					//winSize+1due to interpolation
+					//error or skip the level??
+					valid[i]=0;
+					break;
+				}	 
+				getInterpolatePatch(jP[level], imgSize, x+dX, y+dY, winSize, jPatch);
+				eX=eY=0;
+				for(idx=0;idx<winSizeSq;idx++){
+					dIt=iPatch[idx]-jPatch[idx];
+					eX+=dIt*iDxPatch[idx];
+					eY+=dIt*iDyPatch[idx];
+				}
+				mX=c_det*(eX*c_yy-eY*c_xy);
+				mY=c_det*(-eX*c_xy+eY*c_xx);
+				dX+=mX;
+				dY+=mY;
+				if((mX*mX+mY*mY)<accuracy_th) break;
+			}
+		}
+		newFPnt[i*2+0]=fPnt[i*2+0]+dX;
+		newFPnt[i*2+1]=fPnt[i*2+1]+dY;
+/*
+		if(valid[i]
+		|| (x+dX-winSize)<0 || (y+dY-winSize)<0 
+		|| (y+dY+winSize+1)>=imgSize[0] || (x+dX+winSize+1)>=imgSize[1]){ 
+			newFPnt[i*2+0]=fPnt[i*2+0]+dX;
+			newFPnt[i*2+1]=fPnt[i*2+1]+dY;
+			getInterpolatePatch(jP[0], imgSize, newFPnt[i*2+0], newFPnt[i*2+0], winSize, jPatch);
+			dIt=0;
+			for(idx=0;idx<winSizeSq;idx++){
+				dIt+=(iPatch[idx]-jPatch[idx])*(iPatch[idx]-jPatch[idx]);
+			}
+			if(dIt>winSizeSq*50000){
+				valid[i]=0;
+			}
+		}else{
+			newFPnt[i*2+0]=0;
+			newFPnt[i*2+1]=0;
+		}
+*/
+	}
+	free(iPatch);
+	free(jPatch);
+	free(iDxPatch);
+	free(iDyPatch);
+}
+void getPatch(double* srcImg, const int* srcDims, double centerX, double centerY, int winSize, double** dstImg){
+	int srcIdxX, srcIdxY, dstIdxX;
+	srcIdxY=(int)centerY-winSize;
+	for(int i=-winSize; i<winSize; i++){
+		srcIdxX=(int)centerX+i;
+		dstIdxX=i+winSize;
+		dstImg[dstIdxX]=srcImg+srcIdxX*winSize*2+srcIdxY;
+	}
+}
+void calcLKTrack(double* imgI, double* iDx, double* iDy, double* imgJ, const int* imdims,
+	double* c_xx, double* c_xy, double* c_yy,
+        double* fPnt, double* initPnt, int nFeatures, int winSize,
+        double* newFPnt, char* valid){
+	calcLKTrack( imgI, iDx, iDy, imgJ, imdims,
+		c_xx, c_xy, c_yy,
+		fPnt, initPnt, nFeatures, winSize, 
+        	newFPnt, valid, LK_ACCURACY_TH, LK_MAX_ITER);
+}
+void calcLKTrack(double* imgI, double* iDx, double* iDy, double* imgJ, const int* imdims, 
+	double* c_xx, double* c_xy, double* c_yy,
+        double* fPnt, double* initPnt, int nFeatures, int winSize, 
+        double* newFPnt, char* valid, 
+        double accuracy_th, int max_iter){
+	double x, y, eX, eY, dX, dY, mX, mY, c_det, dIt;
+	double* iPatch[LK_MAX_WIN], *iDxPatch[LK_MAX_WIN], *iDyPatch[LK_MAX_WIN];
+	double*  jPatch;
+	int level, winSizeSq;
+	int i, k, idxCol, idxRow, idx;
+	int imgSize[2];
+
+	jPatch=(double*) malloc(sizeof(double)*winSizeSq);
+	winSizeSq=4*winSize*winSize;
+	imgSize[0]=imdims[0];
+	imgSize[1]=imdims[1];
+	for(i=0; i<nFeatures; i++){
+		x=fPnt[i*2+0];//half size of real level
+		y=fPnt[i*2+1];
+		dX=initPnt[i*2+0]-x;
+		dY=initPnt[i*2+1]-y;
+//printf("input dx dy %.2f %.2f:", dX, dY);
+			
+		//when feature goes out to the boundary.
+		if((x-winSize)<0 || (y-winSize)<0 
+		|| (y+winSize)>=imdims[0] || (x+winSize)>=imdims[1]){ 
+			//error or skip the level??
+			valid[i]=0;
+			continue;
+		} 
+
+		getPatch(imgI, imdims, x, y, winSize, iPatch);
+		getPatch(iDx, imdims, x, y, winSize, iDxPatch);
+		getPatch(iDy, imdims, x, y, winSize, iDyPatch);
+
+		idx=(int)x*imdims[0]+(int)y;
+		c_det=c_xx[i]*c_yy[i]-c_xy[i]*c_xy[i];
+		if(c_det/(c_xx[i]+c_yy[i]+0.00000001)<GOOD_FEATURE_LAMBDA_TH*100){
+			valid[i]=0;
+			continue;
+		}
+		c_det=1/c_det;
+		for(k=0; k<max_iter; k++){
+			if((x+dX-winSize)<0 || (y+dY-winSize)<0 
+			|| (y+dY+winSize+1)>=imdims[0] || (x+dX+winSize+1)>=imdims[1]){ 
+				//winSize+1due to interpolation
+				//error or skip the level??
+				valid[i]=0;
+				break;
+			}	 
+			getInterpolatePatch(imgJ, imgSize, x+dX, y+dY, winSize, jPatch);
+			eX=eY=0;
+			for(idxCol=0;idxCol<2*winSize;idxCol++){
+				for(idxRow=0;idxRow<2*winSize;idxRow++){
+					dIt=iPatch[idxCol][idxRow]-jPatch[idxCol*winSize*2+idxRow];
+					eX+=dIt*iDxPatch[idxCol][idxRow];
+					eY+=dIt*iDyPatch[idxCol][idxRow];
+				}
+			}
+			mX=c_det*(eX*c_yy[i]-eY*c_xy[i]);
+			mY=c_det*(-eX*c_xy[i]+eY*c_xx[i]);
+			dX+=mX;
+			dY+=mY;
+			if((mX*mX+mY*mY)<accuracy_th) break;
+		}
+		if(k==max_iter){
+			valid[i]=0;
+		}
+		newFPnt[i*2+0]=x+dX;
+		newFPnt[i*2+1]=y+dY;
+	}
+	free(jPatch);
+}
+
-- 
cgit v1.2.2