lk.cpp

// Copyright 2011 Zdenek Kalal
//
// This file is part of TLD.
// 
// TLD is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// TLD is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// 
// You should have received a copy of the GNU General Public License
// along with TLD.  If not, see <http://www.gnu.org/licenses/>.

#include "cv.h"
#include "highgui.h"
#include "math.h"
#include <limits>
#ifdef _CHAR16T
#define CHAR16_T
#endif
#include "mex.h" 


const int MAX_COUNT = 500;
const int MAX_IMG   = 2;
int win_size = 4;
CvPoint2D32f* points[3] = {0,0,0};

void loadImageFromMatlab(const mxArray *mxImage, IplImage *image) {

	unsigned char *values =  (unsigned char *) mxGetPr(mxImage);
	int widthStep = image->widthStep;
	int N = mxGetN(mxImage); // width
	int M = mxGetM(mxImage); // height

	if (N == 0 || M == 0) {
		printf("Input image error\n");
		return;
	}

	for(int i=0;i<N;i++)
		for(int j=0;j<M;j++) 
			image->imageData[j*widthStep+i] = values[j+i*M];
}

void euclideanDistance (CvPoint2D32f *point1, CvPoint2D32f *point2, float *match, int nPts) {

	for (int i = 0; i < nPts; i++) {

		match[i] = sqrt((point1[i].x - point2[i].x)*(point1[i].x - point2[i].x) + 
		(point1[i].y - point2[i].y)*(point1[i].y - point2[i].y) );

	}
}

void normCrossCorrelation(IplImage *imgI, IplImage *imgJ, CvPoint2D32f *points0, CvPoint2D32f *points1, int nPts, char *status, float *match,int winsize, int method) {


	IplImage *rec0 = cvCreateImage( cvSize(winsize, winsize), 8, 1 );
	IplImage *rec1 = cvCreateImage( cvSize(winsize, winsize), 8, 1 );
	IplImage *res  = cvCreateImage( cvSize( 1, 1 ), IPL_DEPTH_32F, 1 );

	for (int i = 0; i < nPts; i++) {
		if (status[i] == 1) {
			cvGetRectSubPix( imgI, rec0, points0[i] );
			cvGetRectSubPix( imgJ, rec1, points1[i] );
			cvMatchTemplate( rec0,rec1, res, method );
			match[i] = ((float *)(res->imageData))[0]; 

		} else {
			match[i] = 0.0;
		}
	}
	cvReleaseImage( &rec0 );
	cvReleaseImage( &rec1 );
	cvReleaseImage( &res );

}


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;
	}

	IplImage **IMG = 0;
	IplImage **PYR = 0;

	IMG = (IplImage**) calloc(MAX_IMG,sizeof(IplImage*));
	PYR = (IplImage**) calloc(MAX_IMG,sizeof(IplImage*));

	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
	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]);

	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, status, 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];
		}
    else
    {
			output[M*i]   = nan;
			output[M*i+1] = nan;
			output[M*i+2] = nan;
			output[M*i+3] = nan;
		}
	}

  // clean up
  for (int i = 0; i < MAX_IMG; i++) 
  {
		cvReleaseImage(&(IMG[i]));
		cvReleaseImage(&(PYR[i]));
	}
	free(IMG);
	free(PYR);
}