float polynomial(int d, F2D* a, F2D* b, int dim)
{
float ret;
F2D *bt, *bt1;
int i,j,r,c;
r = b->height;
c = b->width;
bt = fMallocHandle(c, r);
for(i=0; i<r; i++)
{
for(j=0; j<c; j++)
{
subsref(bt,j,i) = subsref(b,i,j);
}
}
bt1 = fMtimes(a, bt);
fFreeHandle(bt);
if(bt1->height == 1 && bt1->width ==1)
ret = pow(asubsref(bt1,0),d)/dim;
else
{
fFreeHandle(bt1);
return -1;
}
fFreeHandle(bt1);
return ret;
}
F2D* resizeArray(F2D* array, int omin)
{
F2D* prev = NULL;
F2D* current = array;
int o;
if(omin<0)
{
for(o=1; o>=-omin; o--)
{
prev = current;
current = doubleSize(current);
fFreeHandle(prev);
}
}
if(omin>0)
{
for(o=1; o<= omin; o++)
{
prev = current;
current = halveSize(current);
fFreeHandle(prev);
}
}
return current;
}
I2D* getDisparity(I2D* Ileft, I2D* Iright, int win_sz, int max_shift)
{
I2D* retDisp;
int nr, nc, k;
I2D *halfWin;
int half_win_sz, rows, cols;
F2D *retSAD, *minSAD, *SAD, *integralImg;
I2D* IrightPadded, *IleftPadded, *Iright_moved;
nr = Ileft->height;
nc = Ileft->width;
half_win_sz=win_sz/2;
minSAD = fSetArray(nr, nc, 255.0*255.0);
retDisp = iSetArray(nr, nc,max_shift);
halfWin = iSetArray(1,2,half_win_sz);
if(win_sz > 1)
{
IleftPadded = padarray2(Ileft, halfWin);
IrightPadded = padarray2(Iright, halfWin);
}
else
{
IleftPadded = Ileft;
IrightPadded = Iright;
}
rows = IleftPadded->height;
cols = IleftPadded->width;
SAD = fSetArray(rows, cols,255);
integralImg = fSetArray(rows, cols,0);
retSAD = fMallocHandle(rows-win_sz, cols-win_sz);
Iright_moved = iSetArray(rows, cols, 0);
for( k=0; k<max_shift; k++)
{
correlateSAD_2D(IleftPadded, IrightPadded, Iright_moved, win_sz, k, SAD, integralImg, retSAD);
findDisparity(retSAD, minSAD, retDisp, k, nr, nc);
}
fFreeHandle(retSAD);
fFreeHandle(minSAD);
fFreeHandle(SAD);
fFreeHandle(integralImg);
iFreeHandle(halfWin);
iFreeHandle(IrightPadded);
iFreeHandle(IleftPadded);
iFreeHandle(Iright_moved);
return retDisp;
}
F2D* quatRot(F2D* vec, F2D* rQuat)
{
F2D *ret;
int nr, i, j, k, rows, cols;
F2D *tv, *vQuat, *temp, *temp1;
F2D *retVec;
nr = vec->height;
tv = fSetArray(nr, 1, 0);
vQuat = fHorzcat(tv, vec);
temp = quatMul(rQuat, vQuat);
temp1 = quatConj(rQuat);
retVec = quatMul(temp, temp1);
rows = retVec->height;
cols = retVec->width;
ret = fSetArray(rows, 3, 0);
for(i=0; i<rows; i++)
{
k = 0;
for(j=1; j<4; j++)
{
subsref(ret,i,k) = subsref(retVec,i,j);
k++;
}
}
fFreeHandle(tv);
fFreeHandle(vQuat);
fFreeHandle(temp);
fFreeHandle(temp1);
fFreeHandle(retVec);
return ret;
}
alphaRet* getAlphaFromTrainSet(int N, F2D* trn1, F2D* trn2, int iterations)
{
float tolerance, C, eps, *b;
F2D *a_result, *b_result;
int NumChanged, r, ExamineAll, cnt, d, dim, ret, iter, i;
F2D *X, *Y;
F2D *a, *e;
b = malloc(sizeof(float));
alphaRet* alpha;
alpha = (alphaRet*)malloc(sizeof(alphaRet));
tolerance = 0.001;
C = 0.05;
d = -1;
dim = 256;
eps = 0.001;
a_result = fSetArray(iterations, N, 0);
b_result = fSetArray(iterations, 1, 0);
ret = 0;
X = usps_read_partial( trn1, trn2, 0, 1, (N/iterations), iterations);
for(iter=0; iter<iterations; iter++)
{
Y = usps_read_partial( trn1, trn2, iter, 0, N/iterations, iterations);
a = fSetArray(N, 1, 0);
arrayref(b,0) = 0; /** check if ptr **/
e = fSetArray(N, 1, 0);
ExamineAll = 1;
cnt = 0;
NumChanged = 0;
while(NumChanged>0 || ExamineAll == 1)
{
cnt = cnt + 1;
NumChanged = 0;
if(ExamineAll == 1)
{
for(i=0; i<N; i++)
{
ret = examineExample(i, a, b, C, e, X, Y, tolerance, N, eps, dim);
NumChanged = NumChanged + ret;
}
}
else
{
for(i=0; i<N; i++)
{
if( asubsref(a,i) > 0 && asubsref(a,i) <C )
{
ret = examineExample(i, a, b, C, e, X, Y, tolerance, N, eps, dim);
NumChanged = NumChanged + ret;
}
}
}
if(ExamineAll == 1)
ExamineAll = 0;
else if(NumChanged == 0)
ExamineAll = 1;
}
for(r=0; r<N; r++)
subsref(a_result,iter,r) = asubsref(a,r); /** a_result has size iteration,N .. Check **/
asubsref(b_result,iter) = arrayref(b,0);
fFreeHandle(Y);
fFreeHandle(e);
fFreeHandle(a);
}
alpha->C = C;
alpha->d = d;
alpha->dim = dim;
alpha->eps = eps;
alpha->a_result = a_result;
alpha->b_result = b_result;
alpha->a = a;
alpha->b = arrayref(b,0);
alpha->X = X;
alpha->tolerance = tolerance;
alpha->ret;
free(b);
return alpha;
}
F2D* usps_read_partial(F2D* dcell1, F2D* dcell2, int idx, int opt, int dim, int iterations)
{
F2D *ret, *X, *Y;
F2D *ADD;
int i, j, k, m, n;
F2D *temp, *temp1;
if(opt == 1)
{
for(i=0; i<iterations; i++)
{
if(i==0)
{
X = fMallocHandle(dim, dcell1->width);
for(m=0; m<dim; m++)
{
for(n=0; n<dcell1->width; n++)
{
subsref(X,m,n) = subsref(dcell1,m,n);
}
}
}
else
{
temp = fDeepCopy(X);
fFreeHandle(X);
temp1 = fMallocHandle(dim, dcell2->width);
for(m=0; m<dim; m++)
{
for(n=0; n<dcell2->width; n++)
{
subsref(temp1,m,n) = subsref(dcell2,m,n);
}
}
X = ffVertcat(temp, temp1);
fFreeHandle(temp);
fFreeHandle(temp1);
}
}
ret = fDeepCopy(X);
fFreeHandle(X);
}
else
{
for(i=0; i<iterations; i++)
{
if(idx == -1)
ADD = fSetArray(dim, 1, i+1);
else
{
if( i!= idx)
ADD = fSetArray(dim, 1, -1);
else
ADD = fSetArray(dim, 1, 1);
}
if(i==0)
{
Y = fDeepCopy(ADD);
}
else
{
F2D* t = fDeepCopy(Y);
fFreeHandle(Y);
Y = ffVertcat(t, ADD);
fFreeHandle(t);
}
fFreeHandle(ADD);
}
ret = fDeepCopy(Y);
fFreeHandle(Y);
}
return ret;
}
F2D* harris(I2D* im)
{
F2D *img1;
F2D *g1, *g2, *g;
F2D *Ix, *Iy;
F2D *Ix2, *Iy2, *IxIy;
F2D *v, *R, *Rmax, *Rnm;
float eps;
F2D *sobel, *sob, *temp, *temp1;
I2D *win, *x, *y;
int i;
g1 = fSetArray(5,5,0);
g2 = fSetArray(3,3,0);
asubsref(g1,0) = 1;
asubsref(g1,1) = 4;
asubsref(g1,2) = 6;
asubsref(g1,3) = 4;
asubsref(g1,4) = 1;
asubsref(g1,5) = 4;
asubsref(g1,6) = 16;
asubsref(g1,7) = 24;
asubsref(g1,8) = 16;
asubsref(g1,9) = 4;
asubsref(g1,10) = 6;
asubsref(g1,11) = 24;
asubsref(g1,12) = 36;
asubsref(g1,13) = 24;
asubsref(g1,14) = 6;
asubsref(g1,15) = 4;
asubsref(g1,16) = 16;
asubsref(g1,17) = 24;
asubsref(g1,18) = 16;
asubsref(g1,19) = 4;
asubsref(g1,20) = 1;
asubsref(g1,21) = 4;
asubsref(g1,22) = 6;
asubsref(g1,23) = 4;
asubsref(g1,24) = 1;
asubsref(g2,0) = 1;
asubsref(g2,1) = 2;
asubsref(g2,2) = 1;
asubsref(g2,3) = 2;
asubsref(g2,4) = 4;
asubsref(g2,5) = 2;
asubsref(g2,6) = 1;
asubsref(g2,7) = 2;
asubsref(g2,8) = 1;
g = fDivide(g1, 256);
sob = fMallocHandle(1,3);
asubsref(sob,0) = -0.5;
asubsref(sob,1) = 0;
asubsref(sob,2) = 0.5;
{
F2D* imf;
imf = fiDeepCopy(im);
img1 = ffConv2(imf, g);
fFreeHandle(imf);
}
Ix = ffConv2(img1, sob);
fFreeHandle(sob);
sob = fMallocHandle(3,1);
asubsref(sob,0) = -0.5;
asubsref(sob,1) = 0;
asubsref(sob,2) = 0.5;
Iy = ffConv2(img1, sob);
fFreeHandle(g);
g = fDivide(g2, 16);
eps = 2.2204e-16;
sobel = fTimes(Ix, Ix);
Ix2 = ffConv2(sobel, g);
fFreeHandle(sobel);
sobel = fTimes(Iy, Iy);
Iy2 = ffConv2(sobel, g);
fFreeHandle(sobel);
sobel = fTimes(Ix, Iy);
IxIy = ffConv2(sobel, g);
fFreeHandle(sobel);
temp = fTimes(Ix2, Iy2);
temp1 = fTimes(IxIy, IxIy);
sobel = fMinus(temp, temp1);
fFreeHandle(temp);
temp = fPlus(Ix2, Iy2);
for(i=0; i<(temp->height*temp->width); i++)
asubsref(temp,i) += eps;
R = ffDivide(sobel, temp);
win = iSetArray(1,2,3);
Rmax = maxWindow(R, win);
Rnm = supress(R, Rmax);
v = fFind3(Rnm);
iFreeHandle(win);
fFreeHandle(Rmax);
fFreeHandle(Rnm);
fFreeHandle(R);
fFreeHandle(img1);
fFreeHandle(g1);
fFreeHandle(g2);
fFreeHandle(g);
fFreeHandle(Ix);
fFreeHandle(Iy);
fFreeHandle(Ix2);
fFreeHandle(Iy2);
fFreeHandle(IxIy);
fFreeHandle(sobel);
fFreeHandle(sob);
fFreeHandle(temp);
fFreeHandle(temp1);
// iFreeHandle(x);
// iFreeHandle(y);
return v;
}
F2D* imageResize(F2D* imageIn)
{
int m, k, rows, cols;
F2D *imageOut;
I2D *kernel;
float tempVal;
int kernelSize, startCol, endCol, halfKernel, startRow, endRow, i, j, kernelSum;
int outputRows, outputCols;
F2D *temp;
rows = imageIn->height;
cols = imageIn->width;
// level 1 is the base image.
outputRows = floor((rows+1)/2);
outputCols = floor((cols+1)/2);
temp = fSetArray(rows, outputCols, 0);
imageOut = fSetArray(outputRows, outputCols, 0);
kernel = iMallocHandle(1, 5);
asubsref(kernel,0) = 1;
asubsref(kernel,1) = 4;
asubsref(kernel,2) = 6;
asubsref(kernel,3) = 4;
asubsref(kernel,4) = 1;
kernelSize = 5;
kernelSum = 16;
startCol = 2;
endCol = cols - 2;
halfKernel = 2;
startRow = 2;
endRow = rows - 2;
for(i=startRow; i<endRow; i++)
{
m = 0;
for(j=startCol; j<endCol; j+=2)
{
tempVal = 0;
for(k=-halfKernel; k<=halfKernel; k++)
{
tempVal += subsref(imageIn,i,j+k) * asubsref(kernel,k+halfKernel);
}
subsref(temp,i,m) = tempVal/kernelSum;
m = m+1;
}
}
m = 0;
for(i=startRow; i<endRow; i+=2)
{
for(j=0; j<outputCols; j++)
{
tempVal = 0;
for(k=-halfKernel; k<=halfKernel; k++)
{
tempVal += subsref(temp,(i+k),j) * asubsref(kernel,k+halfKernel);
}
subsref(imageOut,m,j) = (tempVal/kernelSum);
}
m = m+1;
}
fFreeHandle(temp);
iFreeHandle(kernel);
return imageOut;
}
F2D* calcSobel_dX(F2D* imageIn)
{
int rows, cols;
F2D *kernel_1, *kernel_2;
APPROX float temp;
int kernelSize, startCol, endCol, halfKernel, startRow, endRow, i, j, kernelSum;
int k, kernelSum_1, kernelSum_2;
F2D *imageOut, *tempOut;
rows = imageIn->height;
cols = imageIn->width;
imageOut = fSetArray(rows, cols, 0);
tempOut = fSetArray(rows, cols, 0);
kernel_1 = fMallocHandle(1, 3);
kernel_2 = fMallocHandle(1, 3);
asubsref(kernel_1,0) = 1;
asubsref(kernel_1,1) = 2;
asubsref(kernel_1,2) = 1;
kernelSize = 3;
kernelSum_1 = 4;
asubsref(kernel_2,0) = 1;
asubsref(kernel_2,1) = 0;
asubsref(kernel_2,2) = -1;
kernelSum_2 = 2;
startCol = 1;
endCol = cols - 1;
halfKernel = 1;
startRow = 1;
endRow = rows - 1;
for(i=startRow; i<endRow; i++)
{
for(j=startCol; j<endCol; j++)
{
temp = 0;
for(k=-halfKernel; k<=halfKernel; k++)
{
temp += subsref(imageIn,i,j+k) * asubsref(kernel_2,k+halfKernel);
}
subsref(tempOut,i,j) = temp/kernelSum_2;
}
}
for(i=startRow; i<endRow; i++)
{
for(j=startCol; j<endCol; j++)
{
temp = 0;
for(k=-halfKernel; k<=halfKernel; k++)
{
temp += subsref(tempOut,(i+k),j) * asubsref(kernel_1,k+halfKernel);
}
subsref(imageOut,i,j) = temp/(float)kernelSum_1;
}
}
fFreeHandle(tempOut);
fFreeHandle(kernel_1);
fFreeHandle(kernel_2);
return imageOut;
}
int main(int argc, char* argv[])
{
int iter, N, Ntst, i, j, k, n;
F2D* trn1, *tst1, *trn2, *tst2, *Yoffset;
alphaRet* alpha;
F2D *a_result, *result;
F2D *s;
F2D *b_result;
F2D *Xtst, *Ytst;
unsigned int* start, *stop, *elapsed;
char im1[256];
int dim = 256;
N = 100;
Ntst = 100;
iter = 10;
#ifdef test
N = 4;
Ntst = 4;
iter = 2;
#endif
#ifdef sim_fast
N = 20;
Ntst = 20;
iter = 2;
#endif
#ifdef sim
N = 16;
Ntst = 16;
iter = 8;
#endif
#ifdef sqcif
N = 60;
Ntst = 60;
iter = 6;
#endif
#ifdef qcif
N = 72;
Ntst = 72;
iter = 8;
#endif
#ifdef vga
N = 450;
Ntst = 450;
iter = 15;
#endif
#ifdef wuxga
N = 1000;
Ntst = 1000;
iter = 20;
#endif
printf("Input size\t\t- (%dx%dx%d)\n", N, Ntst, iter);
if(argc < 2)
{
printf("We need input image path\n");
return -1;
}
sprintf(im1, "%s/d16trn_1.txt", argv[1]);
trn1 = readFile(im1);
sprintf(im1, "%s/d16trn_2.txt", argv[1]);
trn2 = readFile(im1);
sprintf(im1, "%s/d16tst_1.txt", argv[1]);
tst1 = readFile(im1);
sprintf(im1, "%s/d16tst_2.txt", argv[1]);
tst2 = readFile(im1);
/** Start timing **/
start = photonStartTiming();
alpha = getAlphaFromTrainSet(N, trn1, trn2, iter);
a_result = alpha->a_result;
b_result = alpha->b_result;
Yoffset = fSetArray(iter, N, 0);
Xtst = usps_read_partial(tst1, tst2, -1, 1, Ntst/iter, iter);
Ytst = usps_read_partial(tst1, tst2, -1, 0, Ntst/iter, iter);
for(i=0; i<iter; i++)
{
F2D *temp;
temp = usps_read_partial(trn1, trn2, i, 0, N/iter, iter);
for(j=0; j<N; j++)
subsref(Yoffset,i,j) = asubsref(temp,j);
fFreeHandle(temp);
}
result = fSetArray(Ntst,1,0);
for( n=0; n<Ntst; n++)
{
float maxs=0;
s=fSetArray(iter,1,0);
for( i=0; i<iter; i++)
{
for (j=0; j<N; j++)
{
if (subsref(a_result,i,j) > 0)
{
F2D *Xtemp, *XtstTemp, *X;
X = alpha->X;
Xtemp = fDeepCopyRange(X,j,1,0,X->width);
XtstTemp = fDeepCopyRange(Xtst, n,1,0,Xtst->width);
asubsref(s,i) = asubsref(s,i) + subsref(a_result,i,j) * subsref(Yoffset,i,j) * polynomial(3,Xtemp,XtstTemp, dim);
fFreeHandle(Xtemp);
fFreeHandle(XtstTemp);
}
}
asubsref(s,i) = asubsref(s,i) - asubsref(b_result,i);
if( asubsref(s,i) > maxs)
maxs = asubsref(s,i);
}
fFreeHandle(s);
asubsref(result,n) = maxs;
}
/** Timing utils */
stop = photonEndTiming();
#ifdef CHECK
/** Self checking - use expected.txt from data directory **/
{
int ret=0;
float tol = 0.5;
#ifdef GENERATE_OUTPUT
fWriteMatrix(result, argv[1]);
#endif
ret = fSelfCheck(result, argv[1], tol);
if (ret == -1)
printf("Error in SVM\n");
}
/** Self checking done **/
#endif
fFreeHandle(trn1);
fFreeHandle(tst1);
fFreeHandle(trn2);
fFreeHandle(tst2);
fFreeHandle(Yoffset);
fFreeHandle(result);
fFreeHandle(alpha->a_result);
fFreeHandle(alpha->b_result);
fFreeHandle(alpha->X);
free(alpha);
fFreeHandle(Xtst);
fFreeHandle(Ytst);
elapsed = photonReportTiming(start, stop);
photonPrintTiming(elapsed);
free(start);
free(stop);
free(elapsed);
return 0;
}
I2D* calcPyrLKTrack(F2D* previousImageBlur_level1, F2D* previousImageBlur_level2, F2D* vertEdge_level1, F2D* vertEdge_level2, F2D* horzEdge_level1, F2D* horzEdge_level2, F2D* currentImageBlur_level1, F2D* currentImageBlur_level2, F2D* previousFrameFeatures, int nFeatures, int winSize, float accuracy, int max_iter, F2D* currentFrameFeatures)
{
int idx, level, pLevel, i, j, k, winSizeSq;
I2D *valid;
F2D *rate, *iPatch, *jPatch, *iDxPatch;
F2D *iDyPatch;
float tr, x, y, dX, dY, c_xx, c_yy, c_xy;
int imgSize_1, imgSize_2;
float mX, mY, dIt, eX, eY, c_det;
I2D *imgDims;
imgDims = iMallocHandle(2, 2);
subsref(imgDims,0,0) = previousImageBlur_level1->height;
subsref(imgDims,0,1) = previousImageBlur_level1->width;
subsref(imgDims,1,0) = previousImageBlur_level2->height;
subsref(imgDims,1,1) = previousImageBlur_level2->width;
pLevel = 2;
rate = fMallocHandle(1, 6);
asubsref(rate,0) = 1;
asubsref(rate,1) = 0.5;
asubsref(rate,2) = 0.25;
asubsref(rate,3) = 0.125;
asubsref(rate,4) = 0.0625;
asubsref(rate,5) = 0.03125;
winSizeSq = 4*winSize*winSize;
valid = iSetArray(1,nFeatures, 1);
/** For each feature passed from previous frame, compute the dx and dy, the displacements **/
for(i=0; i<nFeatures; i++)
{
dX = 0;
dY = 0;
/** Compute the x and y co-ordinate values at "pLevel" **/
x = subsref(previousFrameFeatures,0,i) * asubsref(rate,pLevel);
y = subsref(previousFrameFeatures,1,i) * asubsref(rate,pLevel);
c_det = 0;
/** For each pyramid level, try to find correspondence.
We look for the correspondence in a given window size
, (winSize x winSize) neighborhood **/
for(level = pLevel-1; level>=0; level--)
{
x = x+x;
y = y+y;
dX = dX + dX;
dY = dY + dY;
imgSize_1 = subsref(imgDims,level,0);
imgSize_2 = subsref(imgDims,level,1);
c_xx = 0;
c_xy = 0;
c_yy = 0;
if((x-winSize)<0 || (y-winSize)<0 || (y+winSize+1)>=imgSize_1 || (x+winSize+1)>=imgSize_2)
{
asubsref(valid,i) = 0;
break;
}
/** Perform interpolation. Use co-ord from previous
frame and use the images from current frame **/
if(level ==0)
{
iPatch = getInterpolatePatch(previousImageBlur_level1, imgSize_2, x, y, winSize);
iDxPatch = getInterpolatePatch(vertEdge_level1, imgSize_2, x, y, winSize);
iDyPatch = getInterpolatePatch(horzEdge_level1, imgSize_2, x, y, winSize);
}
if(level ==1)
{
iPatch = getInterpolatePatch(previousImageBlur_level2, imgSize_2, x, y, winSize);
iDxPatch = getInterpolatePatch(vertEdge_level2, imgSize_2, x, y, winSize);
iDyPatch = getInterpolatePatch(horzEdge_level2, imgSize_2, x, y, winSize);
}
/** Compute feature strength in similar way as calcGoodFeature **/
for(idx=0; idx<winSizeSq; idx++)
{
c_xx += asubsref(iDxPatch,idx) * asubsref(iDxPatch,idx);
c_xy += asubsref(iDxPatch,idx) * asubsref(iDyPatch,idx);
c_yy += asubsref(iDyPatch,idx) * asubsref(iDyPatch,idx);
}
c_det = (c_xx * c_yy -c_xy * c_xy);
tr = c_xx + c_yy;
if((c_det/(tr+0.00001)) < accuracy)
{
asubsref(valid,i) = 0;
fFreeHandle(iPatch);
fFreeHandle(iDxPatch);
fFreeHandle(iDyPatch);
break;
}
c_det = 1/c_det;
/** We compute dX and dY using previous frame and current
frame images. For this, the strength is computed at each
pixel in the new image. **/
for(k=0; k<max_iter; k++)
{
if( (x+dX-winSize)<0 || (y+dY-winSize)<0 || (y+dY+winSize+1)>=imgSize_1 || (x+dX+winSize+1)>=imgSize_2)
{
asubsref(valid,i) = 0;
break;
}
if(level == 0)
jPatch = getInterpolatePatch(currentImageBlur_level1, imgSize_2, x+dX, y+dY, winSize);
if(level == 1)
jPatch = getInterpolatePatch(currentImageBlur_level2, imgSize_2, x+dX, y+dY, winSize);
eX = 0;
eY = 0;
for(idx=0; idx<winSizeSq; idx++)
{
dIt = asubsref(iPatch,idx) - asubsref(jPatch,idx);
eX += dIt * asubsref(iDxPatch,idx);
eY += dIt * asubsref(iDyPatch,idx);
}
mX = c_det * (eX * c_yy - eY * c_xy);
mY = c_det * (-eX * c_xy + eY * c_xx);
dX = dX + mX;
dY = dY + mY;
if( (mX*mX+mY*mY) < accuracy)
{
fFreeHandle(jPatch);
break;
}
fFreeHandle(jPatch);
}
fFreeHandle(iPatch);
fFreeHandle(iDxPatch);
fFreeHandle(iDyPatch);
}
subsref(currentFrameFeatures,0,i) = subsref(previousFrameFeatures,0,i) + dX;
subsref(currentFrameFeatures,1,i) = subsref(previousFrameFeatures,1,i) + dY;
}
fFreeHandle(rate);
iFreeHandle(imgDims);
return valid;
}
F2D* calcSobel_dY(F2D* imageIn)
{
int rows, cols;
I2D *kernel_1, *kernel_2;
float temp;
int kernelSize, startCol, endCol, halfKernel, startRow, endRow, i, j, kernelSum;
int k, kernelSum_2, outputRows, outputCols;
F2D *imageOut, *tempOut;
float kernelSum_1;
rows = imageIn->height;
cols = imageIn->width;
// level 1 is the base image.
outputRows = rows;
outputCols = cols;
imageOut = fSetArray(outputRows, outputCols, 0);
tempOut = fSetArray(outputRows, outputCols, 0);
kernel_1 = iMallocHandle(1, 3);
kernel_2 = iMallocHandle(1, 3);
asubsref(kernel_1,0) = 1;
asubsref(kernel_1,1) = 0;
asubsref(kernel_1,2) = -1;
kernelSize = 3;
kernelSum_1 = 2.0;
asubsref(kernel_2,0) = 1;
asubsref(kernel_2,1) = 2;
asubsref(kernel_2,2) = 1;
kernelSum_2 = 4;
startCol = 1;
endCol = cols - 1;
halfKernel = 1;
startRow = 1;
endRow = rows - 1;
for(i=startRow; i<endRow; i++)
{
for(j=startCol; j<endCol; j++)
{
temp = 0;
for(k=-halfKernel; k<=halfKernel; k++)
{
temp += subsref(imageIn,(i+k),j) * asubsref(kernel_1,k+halfKernel);
}
subsref(tempOut,i,j) = temp/kernelSum_1;
}
}
for(i=startRow; i<endRow; i++)
{
for(j=startCol; j<endCol; j++)
{
temp = 0;
for(k=-halfKernel; k<=halfKernel; k++)
{
temp += subsref(tempOut,i,j+k) * asubsref(kernel_2,k+halfKernel);
}
subsref(imageOut,i,j) = temp/(float)kernelSum_2;
}
}
fFreeHandle(tempOut);
iFreeHandle(kernel_1);
iFreeHandle(kernel_2);
return imageOut;
}
int takeStep(int i, int j, F2D* a, float C, F2D* e, F2D* Y, F2D* X, float eps, float* b, int N, int dim)
{
int ret=1;
float s;
int m, n, k;
float Ei, Ej, gamma, L, H;
F2D *a_old;
float k11, k12, k22, eta;
F2D *temp, *temp1, *temp2;
float t, t1, t2;
float bnew, delta_b;
float c1, c2, Lobj, Hobj;
if(i==j)
return 0;
a_old = fDeepCopy(a);
if( asubsref(a_old,i)>0 && asubsref(a_old,i)<C )
Ei = asubsref(e,i);
else
Ei = cal_learned_func(i, a, b, N, Y, X, dim) - asubsref(Y,i);
if( asubsref(a_old,j)>0 && asubsref(a_old,j)<C )
Ej = asubsref(e,j);
else
Ej = cal_learned_func(j, a, b, N, Y, X, dim) - asubsref(Y,j);
s = asubsref(Y,i) * asubsref(Y,j);
if( asubsref(Y,i) == asubsref(Y,j) )
{
gamma = asubsref(a_old,i) + asubsref(a_old,j);
if(gamma > C)
{
L = gamma - C;
H = C;
}
else
{
L = 0;
H = gamma;
}
}
else
{
gamma = asubsref(a_old,i) - asubsref(a_old,j);
if(gamma > 0)
{
L = 0;
H = C - gamma;
}
else
{
L = -gamma;
H = C;
}
}
if(L==H)
{
fFreeHandle(a_old);
return 0;
}
temp = fMallocHandle(1, X->width);
temp1 = fMallocHandle(1, X->width);
for(m=0; m<X->width; m++)
{
asubsref(temp,m) = subsref(X,i,m);
asubsref(temp1,m) = subsref(X,j,m);
}
k11 = polynomial(3, temp, temp, dim);
k12 = polynomial(3, temp, temp1, dim);
k22 = polynomial(3, temp1, temp1, dim);
eta = 2 * k12 - k11 - k22;
fFreeHandle(temp1);
fFreeHandle(temp);
if(eta<0)
{
asubsref(a,j) = asubsref(a_old,j) + asubsref(Y,j) * (Ej-Ei)/eta;
if( asubsref(a,j) < L)
asubsref(a,j) = L;
else if( asubsref(a,j) > H )
asubsref(a,j) = H;
}
else
{
c1 = eta/2;
c2 = asubsref(Y,j) * (Ei-Ej) - eta * asubsref(a_old,j);
Lobj = c1 * L * L + c2 * L;
Hobj = c1 * H * H + c2 * H;
if (Lobj > (Hobj+eps))
asubsref(a,j) = L;
else if (Lobj < (Hobj-eps))
asubsref(a,j) = H;
else
asubsref(a,j) = asubsref(a_old,j);
}
if( fabsf( asubsref(a,j)- asubsref(a_old,j) ) < (eps* (asubsref(a,j) + asubsref(a_old,j) +eps)) )
{
fFreeHandle(a_old);
return 0;
}
asubsref(a,i) = asubsref(a_old,i) - s * ( asubsref(a,j) - asubsref(a_old,j) );
if( asubsref(a,i) < 0)
{
asubsref(a,j) = asubsref(a,j) + s * asubsref(a,i);
asubsref(a,i) = 0;
}
else if (asubsref(a,i) > C)
{
t = asubsref(a,i) - C;
asubsref(a,j) = asubsref(a,j) + s * t;
asubsref(a,i) = C;
}
/** Update threshold to react change in Lagrange multipliers **/
if( asubsref(a,i) > 0 && asubsref(a,i) < C )
bnew = arrayref(b,0) + Ei + asubsref(Y,i) * (asubsref(a,i) - asubsref(a_old,i)) * k11 + asubsref(Y,j) * (asubsref(a,j) - asubsref(a_old,j)) * k12;
else
{
if( asubsref(a,j) > 0 && asubsref(a,j) < C )
bnew = arrayref(b,0) + Ej + asubsref(Y,i) * (asubsref(a,i) - asubsref(a_old,i)) * k12 + asubsref(Y,j) * (asubsref(a,j) - asubsref(a_old,j)) * k22;
else
{
float b1, b2;
b1 = arrayref(b,0) + Ei + asubsref(Y,i) * (asubsref(a,i) - asubsref(a_old,i)) * k11 + asubsref(Y,j) * (asubsref(a,j) - asubsref(a_old,j)) * k12;
b2 = arrayref(b,0) + Ej + asubsref(Y,i) * (asubsref(a,i) - asubsref(a_old,i)) * k12 + asubsref(Y,j) * (asubsref(a,j) - asubsref(a_old,j)) * k22;
bnew = (b1 + b2) / 2;
}
}
delta_b = bnew - arrayref(b,0);
arrayref(b,0) = bnew;
/** Update error cache using new Lagrange multipliers 24ai **/
t1 = asubsref(Y,i) * (asubsref(a,i)-asubsref(a_old,i));
t2 = asubsref(Y,j) * (asubsref(a,j)-asubsref(a_old,j));
temp = fMallocHandle(1, X->width);
temp1 = fMallocHandle(1, X->width);
temp2 = fMallocHandle(1, X->width);
for (k=0; k<N; k++)
{
if (0 < asubsref(a_old,i) && asubsref(a_old,i) < C )
{
for(m=0; m<X->width; m++)
{
asubsref(temp,m) = subsref(X,i,m);
asubsref(temp1,m) = subsref(X,k,m);
asubsref(temp2,m) = subsref(X,j,m);
}
asubsref(e,k) = asubsref(e,k)+t1 * polynomial(3, temp, temp1, dim) + t2 * polynomial(3, temp2, temp1, dim) - delta_b;
asubsref(e,i) = 0;
asubsref(e,j) = 0;
}
}
fFreeHandle(a_old);
fFreeHandle(temp);
fFreeHandle(temp1);
fFreeHandle(temp2);
ret = 1;
return ret;
}
