void mexFunction(int plhs_size, mxArray *plhs[], int prhs_size, const mxArray *prhs[])
{
// Load images
if (prhs_size ==4) {
win_size = *mxGetPr(prhs[3]);
}
int N = mxGetN(prhs[0]);
int M = mxGetM(prhs[0]);
grey0 = cvCreateImage( cvSize(N, M), 8, 1 );
grey1 = cvCreateImage( cvSize(N, M), 8, 1 );
loadImageFromMatlab(prhs[0],grey0);
loadImageFromMatlab(prhs[1],grey1);
// Load feature points
double *fp = mxGetPr(prhs[2]);
int num_pts = mxGetN(prhs[2]);
points[0] = (CvPoint2D32f*)cvAlloc(num_pts*sizeof(points[0][0]));
points[1] = (CvPoint2D32f*)cvAlloc(num_pts*sizeof(points[0][0]));
char *status = (char*)cvAlloc(num_pts);
float *error = (float*) cvAlloc(num_pts*sizeof(float));
for (int i = 0; i < num_pts; i++) {
points[0][i].x = fp[2*i];
points[0][i].y = fp[2*i+1];
}
// neni treba, urychleni z fpt 40 -> fps 200
//cvFindCornerSubPix( grey0, points[0], num_pts, cvSize(win_size,win_size), cvSize(-1,-1), cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03));
pyramid1 = cvCreateImage( cvGetSize(grey1), 8, 1 );
pyramid0 = cvCreateImage( cvGetSize(grey1), 8, 1 );
cvCalcOpticalFlowPyrLK( grey0, grey1, pyramid0, pyramid1, points[0], points[1], num_pts, cvSize(win_size,win_size), 6, status, error, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), 0 );
// Output
plhs[0] = mxCreateDoubleMatrix(6, num_pts, mxREAL);
double *output = mxGetPr(plhs[0]);
for (int i = 0; i < num_pts; i++) {
output[6*i] = (double) points[0][i].x;
output[6*i+1] = (double) points[0][i].y;
output[6*i+2] = (double) points[1][i].x;
output[6*i+3] = (double) points[1][i].y;
output[6*i+4] = (double) error[i];
output[6*i+5] = (double) status[i];
//output[5*i+5] = (double) error[i];
}
// Tidy up
cvReleaseImage( &pyramid0 );
cvReleaseImage( &pyramid1 );
cvReleaseImage( &grey0 );
cvReleaseImage( &grey1 );
return;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
double nan = std::numeric_limits<double>::quiet_NaN();
double inf = std::numeric_limits<double>::infinity();
if (nrhs == 0) {
mexPrintf("Lucas-Kanade\n");
return;
}
switch ((int) *mxGetPr(prhs[0])) {
// initialize or clean up
case 0: {
if (IMG!=0 && PYR!=0) {
for (int i = 0; i < MAX_IMG; i++) {
cvReleaseImage(&(IMG[i])); IMG[i] = 0;
cvReleaseImage(&(PYR[i])); PYR[i] = 0;
}
free(IMG); IMG = 0;
free(PYR); PYR = 0;
//mexPrintf("LK: deallocated\n");
}
IMG = (IplImage**) calloc(MAX_IMG,sizeof(IplImage*));
PYR = (IplImage**) calloc(MAX_IMG,sizeof(IplImage*));
//mexPrintf("LK: initialized\n");
return;
}
// tracking
case 2: {
if (IMG == 0 || (nrhs != 5 && nrhs != 6)) {
mexPrintf("lk(2,imgI,imgJ,ptsI,ptsJ,Level)\n");
// 0 1 2 3 4
return;
}
int Level;
if (nrhs == 6) {
Level = (int) *mxGetPr(prhs[5]);
} else {
Level = 5;
}
int I = 0;
int J = 1;
int Winsize = 10;
// Images
if (IMG[I] != 0) {
loadImageFromMatlab(prhs[1],IMG[I]);
} else {
CvSize imageSize = cvSize(mxGetN(prhs[1]),mxGetM(prhs[1]));
IMG[I] = cvCreateImage( imageSize, 8, 1 );
PYR[I] = cvCreateImage( imageSize, 8, 1 );
loadImageFromMatlab(prhs[1],IMG[I]);
}
if (IMG[J] != 0) {
loadImageFromMatlab(prhs[2],IMG[J]);
} else {
CvSize imageSize = cvSize(mxGetN(prhs[2]),mxGetM(prhs[2]));
IMG[J] = cvCreateImage( imageSize, 8, 1 );
PYR[J] = cvCreateImage( imageSize, 8, 1 );
loadImageFromMatlab(prhs[2],IMG[J]);
}
// Points
double *ptsI = mxGetPr(prhs[3]); int nPts = mxGetN(prhs[3]);
double *ptsJ = mxGetPr(prhs[4]);
if (nPts != mxGetN(prhs[4])) {
mexPrintf("Inconsistent input!\n");
return;
}
points[0] = (CvPoint2D32f*)cvAlloc(nPts*sizeof(CvPoint2D32f)); // template
points[1] = (CvPoint2D32f*)cvAlloc(nPts*sizeof(CvPoint2D32f)); // target
points[2] = (CvPoint2D32f*)cvAlloc(nPts*sizeof(CvPoint2D32f)); // forward-backward
for (int i = 0; i < nPts; i++) {
points[0][i].x = ptsI[2*i]; points[0][i].y = ptsI[2*i+1];
points[1][i].x = ptsJ[2*i]; points[1][i].y = ptsJ[2*i+1];
points[2][i].x = ptsI[2*i]; points[2][i].y = ptsI[2*i+1];
}
float *ncc = (float*) cvAlloc(nPts*sizeof(float));
float *ssd = (float*) cvAlloc(nPts*sizeof(float));
float *fb = (float*) cvAlloc(nPts*sizeof(float));
char *status = (char*) cvAlloc(nPts);
cvCalcOpticalFlowPyrLK( IMG[I], IMG[J], PYR[I], PYR[J], points[0], points[1], nPts, cvSize(win_size,win_size), Level, status, 0, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), CV_LKFLOW_INITIAL_GUESSES);
cvCalcOpticalFlowPyrLK( IMG[J], IMG[I], PYR[J], PYR[I], points[1], points[2], nPts, cvSize(win_size,win_size), Level, 0 , 0, cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), CV_LKFLOW_INITIAL_GUESSES | CV_LKFLOW_PYR_A_READY | CV_LKFLOW_PYR_B_READY );
normCrossCorrelation(IMG[I],IMG[J],points[0],points[1],nPts, status, ncc, Winsize,CV_TM_CCOEFF_NORMED);
//normCrossCorrelation(IMG[I],IMG[J],points[0],points[1],nPts, status, ssd, Winsize,CV_TM_SQDIFF);
euclideanDistance( points[0],points[2],fb,nPts);
// Output
int M = 4;
plhs[0] = mxCreateDoubleMatrix(M, nPts, mxREAL);
double *output = mxGetPr(plhs[0]);
for (int i = 0; i < nPts; i++) {
if (status[i] == 1) {
output[M*i] = (double) points[1][i].x;
output[M*i+1] = (double) points[1][i].y;
output[M*i+2] = (double) fb[i];
output[M*i+3] = (double) ncc[i];
//output[M*i+4] = (double) ssd[i];
} else {
output[M*i] = nan;
output[M*i+1] = nan;
output[M*i+2] = nan;
output[M*i+3] = nan;
//output[M*i+4] = nan;
}
}
return;
}
}
}