bool LKTracker::trackf2f(const GpuMat& gImg1, const GpuMat& gImg2, GpuMat &gPoints1, GpuMat &gPoints2) {
//计算正反光流
gFlow->sparse(gImg1, gImg2, gPoints1, gPoints2, gStatus); // compute gPoints2
gFlow->sparse(gImg2, gImg1, gPoints2, gPointsFB, gFBStatus); //compute gPointsFB
vector<Point2f> points1, points2;
download(gPoints1, points1);
download(gPoints2, points2);
//Compute the real FB-error
FB_error.clear();
for (int i = 0; i < points1.size(); ++i)
{
FB_error.push_back(norm(pointsFB[i] - points1[i]));
}
//Filter out points with FB_error[i] > mean(FB_error) && points with sim_error[i] > mean(sim_error)
normCrossCorrelation(gImg1, gImg2, gPoints1, gPoints2, points1, points2);
bool retVal = filterPts(points1, points2);
//更新gpu数据
upload(points1, gPoints1);
upload(points2, gPoints2);
return retVal;
}
/**
* Tracks Points from 1.Image to 2.Image.
* Need initImgs before start and at the end of the program for cleanup.
*
* @param imgI previous Image source. (isn't changed)
* @param imgJ actual Image target. (isn't changed)
* @param ptsI points to track from first Image.
* Format [0] = x1, [1] = y1, [2] = x2 ...
* @param nPtsI number of Points to track from first Image
* @param ptsJ container for calculated points of second Image.
* Must have the length of nPtsI.
* @param nPtsJ number of Points
* @param level Pyramidlevel, default 5
* @param fb forward-backward confidence value.
* (corresponds to euclidean distance between).
* Must have the length of nPtsI: nPtsI * sizeof(float).
* @param ncc normCrossCorrelation values. needs as inputlength nPtsI * sizeof(float)
* @param status Indicates positive tracks. 1 = PosTrack 0 = NegTrack
* needs as inputlength nPtsI * sizeof(char)
*
*
* Based Matlab function:
* lk(2,imgI,imgJ,ptsI,ptsJ,Level) (Level is optional)
*/
int trackLK(IplImage *imgI, IplImage *imgJ, float ptsI[], int nPtsI,
float ptsJ[], int nPtsJ, int level, float * fb, float*ncc, char*status)
{
//TODO: watch NaN cases
//double nan = std::numeric_limits<double>::quiet_NaN();
//double inf = std::numeric_limits<double>::infinity();
// tracking
int I, J, winsize_ncc;
CvSize pyr_sz;
int i;
//if unused std 5
if (level == -1)
{
level = 5;
}
I = 0;
J = 1;
winsize_ncc = 10;
//NOTE: initImgs() must be used correctly or memleak will follow.
pyr_sz = cvSize(imgI->width + 8, imgI->height / 3);
PYR[I] = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
PYR[J] = cvCreateImage(pyr_sz, IPL_DEPTH_32F, 1);
// Points
if (nPtsJ != nPtsI)
{
printf("Inconsistent input!\n");
return 0;
}
points[0] = (CvPoint2D32f*) malloc(nPtsI * sizeof(CvPoint2D32f)); // template
points[1] = (CvPoint2D32f*) malloc(nPtsI * sizeof(CvPoint2D32f)); // target
points[2] = (CvPoint2D32f*) malloc(nPtsI * sizeof(CvPoint2D32f)); // forward-backward
//TODO:Free
char* statusBacktrack = (char*) malloc(nPtsI);
for (i = 0; i < nPtsI; 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];
}
//lucas kanade track
cvCalcOpticalFlowPyrLK(imgI, imgJ, PYR[I], PYR[J], points[0], points[1],
nPtsI, cvSize(win_size_lk, win_size_lk), level, status, 0, cvTermCriteria(
CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03),
CV_LKFLOW_INITIAL_GUESSES);
//backtrack
cvCalcOpticalFlowPyrLK(imgJ, imgI, PYR[J], PYR[I], points[1], points[2],
nPtsI, cvSize(win_size_lk, win_size_lk), level, statusBacktrack, 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);
for (i = 0; i < nPtsI; i++)
{
if (status[i] && statusBacktrack[i])
{
status[i] = 1;
}else{
status[i] = 0;
}
}
normCrossCorrelation(imgI, imgJ, points[0], points[1], nPtsI, status, ncc,
winsize_ncc, CV_TM_CCOEFF_NORMED);
euclideanDistance(points[0], points[2], fb, nPtsI);
for (i = 0; i < nPtsI; i++)
{
if (status[i] == 1)
{
ptsJ[2 * i] = points[1][i].x;
ptsJ[2 * i + 1] = points[1][i].y;
}
else //flow for the corresponding feature hasn't been found
{
//Todo: shell realy write N_A_N in it?
ptsJ[2 * i] = N_A_N;
ptsJ[2 * i + 1] = N_A_N;
fb[i] = N_A_N;
ncc[i] = N_A_N;
}
}
for (i = 0; i < 3; i++)
{
free(points[i]);
points[i] = 0;
}
free(statusBacktrack);
return 1;
}
bool LKTracker::trackf2f(const Mat& img1, const Mat& img2, vector<Point2f> &points1, vector<cv::Point2f> &points2)
{
//TODO!:implement c function cvCalcOpticalFlowPyrLK() or Faster tracking function
#ifdef MEASURE_TIME
clock_t startTime = clock();
#endif
#ifndef USE_CUDA
//Forward-Backward tracking
calcOpticalFlowPyrLK(img1, img2, points1, points2, status, similarity, window_size, level, term_criteria, lambda, 0/**/);
//TomHeaven注释：这里的过滤可能漏掉一些运动速度快的目标，故而去掉过滤
calcOpticalFlowPyrLK(img2, img1, points2, pointsFB, FB_status, FB_error, window_size, level, term_criteria, lambda, 0);
#else
/////////// GPU 加速
/*Mat p1(1, points1.size(), CV_32FC2), p2(1, points2.size(), CV_32FC2);
// copy data
for (int i = 0; i < points1.size(); ++i) {
Vec2f& t1 = p1.at<Vec2f>(0, i);
t1[0] = points1[i].x;
t1[1] = points1[i].y;
}
for (int i = 0; i < points2.size(); ++i) {
Vec2f& t2 = p2.at<Vec2f>(0, i);
t2[0] = points2[i].x;
t2[1] = points2[i].y;
}
gPoints1.upload(p1);
gPoints2.upload(p2);*/
// 上传数据
gImg1.upload(img1);
gImg2.upload(img2);
upload(points1, gPoints1);
upload(points2, gPoints2);
//计算正反光流
gFlow->sparse(gImg1, gImg2, gPoints1, gPoints2, gStatus); // compute gPoints2
gFlow->sparse(gImg2, gImg1, gPoints2, gPointsFB, gFBStatus); //compute gPointsFB
download(gPoints2, points2);
download(gStatus, status);
download(gPointsFB, pointsFB);
#endif
#ifdef MEASURE_TIME
clock_t endTime = clock();
printf("OF time = %.3f\n", double(endTime - startTime) / CLOCKS_PER_SEC);
#endif
//printf("p1: rows = %d, cols = %d, type = %d\n", p1.rows, p1.cols, p1.type());
//printf("gPoints1: rows = %d, cols = %d, type = %d\n", gPoints1.rows, gPoints1.cols, gPoints1.type());
//printf("pointsFB: size = %d, point[0] = %f, %f\n", pointsFB.size(), pointsFB[0].x, pointsFB[0].y);
//Compute the real FB-error
FB_error.clear();
for (int i = 0; i < points1.size(); ++i)
{
FB_error.push_back(norm(pointsFB[i] - points1[i]));
}
//Filter out points with FB_error[i] > mean(FB_error) && points with sim_error[i] > mean(sim_error)
normCrossCorrelation(img1, img2, points1, points2);
return filterPts(points1, points2);
//return true;
}
// Lucas-Kanade
Eigen::Matrix<double, 4, 150> lk2(IplImage* imgI, IplImage* imgJ, Eigen::Matrix<double, 2,
150> const & pointsI, Eigen::Matrix<double, 2, 150> const & pointsJ,
unsigned int sizeI, unsigned int sizeJ, unsigned int level) {
double nan = std::numeric_limits<double>::quiet_NaN();
int Level;
if (level != 0) {
Level = (int) level;
} else {
Level = 5;
}
int I = 0;
int J = 1;
int Winsize = 10;
// Images
if (IMG[I] != 0) {
IMG[I] = imgI;
} else {
CvSize imageSize = cvGetSize(imgI);
IMG[I] = cvCreateImage(imageSize, 8, 1);
PYR[I] = cvCreateImage(imageSize, 8, 1);
IMG[I] = imgI;
}
if (IMG[J] != 0) {
IMG[J] = imgJ;
} else {
CvSize imageSize = cvGetSize(imgJ);
IMG[J] = cvCreateImage(imageSize, 8, 1);
PYR[J] = cvCreateImage(imageSize, 8, 1);
IMG[J] = imgJ;
}
// Points
int nPts = sizeI;
if (nPts != sizeJ) {
std::cout << "Inconsistent input!" << std::endl;
return Eigen::MatrixXd::Zero(1, 1);
}
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 = pointsI(0, i);
points[0][i].y = pointsI(1, i);
points[1][i].x = pointsJ(0, i);
points[1][i].y = pointsJ(1, i);
points[2][i].x = pointsI(0, i);
points[2][i].y = pointsI(1, i);
}
float *ncc = (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);
euclideanDistance(points[0], points[2], fb, nPts);
// Output
int M = 4;
Eigen::MatrixXd output(M, 150);
for (int i = 0; i < nPts; i++) {
if (status[i] == 1) {
output(0, i) = (double) points[1][i].x;
output(1, i) = (double) points[1][i].y;
output(2, i) = (double) fb[i];
output(3, i) = (double) ncc[i];
} else {
output(0, i) = nan;
output(1, i) = nan;
output(2, i) = nan;
output(3, i) = nan;
}
}
return output;
}
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;
}
}
}
