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 @@
+#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];