//--------------------------------------------------------------
void eyeTracker::update(ofxCvGrayscaleImage & grayImgFromCam, float threshold, float minSize, float maxSize, float minSquareness) {
//threshold?
//threshold = thresh;
grayImgPreWarp.setFromPixels(grayImgFromCam.getPixels(), grayImgFromCam.width, grayImgFromCam.height); // TODO: there's maybe an unnecessary grayscale image (and copy) here...
if( flipX || flipY ) {
grayImgPreWarp.mirror(flipY, flipX);
}
/* // before we were scaling and translating, but this is removed for now
if (fabs(xoffset-1) > 0.1f || fabs(yoffset-1) > 0.1f){
grayImgPreWarp.translate(xoffset, yoffset);
}
if (fabs(scalef-1) > 0.1f){
grayImgPreWarp.scale(scalef, scalef);
}*/
grayImg = grayImgPreWarp;
grayImgPreModification = grayImg;
grayImg.blur(5);
if (bUseContrast == true) {
grayImg.applyBrightnessContrast(brightness,contrast);
}
if (bUseGamma == true) {
grayImg.applyMinMaxGamma(gamma);
}
grayImg += edgeMask;
threshImg = grayImg;
threshImg.contrastStretch();
threshImg.threshold(threshold, true);
// the dilation of a 640 x 480 image is very slow, so let's just do a ROI near the thing we like:
threshImg.setROI(currentEyePoint.x-50, currentEyePoint.y-50, 100,100); // 200 pix ok?
if (bUseDilate == true) {
for (int i = 0; i < nDilations; i++) {
threshImg.dilate();
}
}
threshImg.resetROI();
bFoundOne = false;
int whoFound = -1;
int num = contourFinder.findContours(threshImg, minSize, maxSize, 100, false, true);
if( num ) {
for(int k = 0; k < num; k++) {
float ratio = contourFinder.blobs[k].boundingRect.width < contourFinder.blobs[k].boundingRect.height ?
contourFinder.blobs[k].boundingRect.width / contourFinder.blobs[k].boundingRect.height :
contourFinder.blobs[k].boundingRect.height / contourFinder.blobs[k].boundingRect.width;
float arcl = contourFinder.blobs[k].length;
float area = contourFinder.blobs[k].area;
float compactness = (float)((arcl*arcl/area)/FOUR_PI);
if (bUseCompactnessTest == true && compactness > maxCompactness) {
continue;
}
//printf("compactness %f \n", compactness);
//lets ignore rectangular blobs
if( ratio > minSquareness) {
currentEyePoint = contourFinder.blobs[k].centroid;
currentNormPoint.x = currentEyePoint.x;
currentNormPoint.y = currentEyePoint.y;
currentNormPoint.x /= w;
currentNormPoint.y /= h;
bFoundOne = true;
whoFound = k;
break;
}
}
}
if (bFoundOne && whoFound != -1) {
// do some convex hull stuff:
CvSeq* ptseq = cvCreateSeq( CV_SEQ_KIND_GENERIC|CV_32SC2, sizeof(CvContour),sizeof(CvPoint), storage );
CvSeq* hull;
CvPoint pt0;
for(int i = 0; i < contourFinder.blobs[whoFound].nPts; i++ ) {
pt0.x = contourFinder.blobs[whoFound].pts[i].x;
pt0.y = contourFinder.blobs[whoFound].pts[i].y;
cvSeqPush( ptseq, &pt0 );
}
hull = cvConvexHull2( ptseq, 0, CV_CLOCKWISE, 0 );
int hullcount = hull->total;
// -------------------------------- TRY TO GET A GOOD ELLIPSE HELLS YEAH !!
int MYN = hullcount;
float x[MYN], y[MYN];
double p[6];
double ellipseParam[5];
float theta;
FitEllipse fitter;
for (int i=0; i<MYN; i++) {
CvPoint pt = **CV_GET_SEQ_ELEM( CvPoint*, hull, i);
x[i] = pt.x;
y[i] = pt.y;
}
double xc, yc;
double xa, ya;
double la, lb;
fitter.apply(x,y,MYN);
p[0] = fitter.Axx;
p[1] = fitter.Axy;
p[2] = fitter.Ayy;
p[3] = fitter.Ax;
p[4] = fitter.Ay;
p[5] = fitter.Ao;
bool bOk = solve_ellipse(p,ellipseParam);
ofxCvBlob temp;
if (bOk == true) {
//float *params_ellipse = pupilGeometries[whichEye].params_ellipse;
float axis_a = ellipseParam[0];
float axis_b = ellipseParam[1];
float cx = ellipseParam[2];
float cy = ellipseParam[3];
theta = ellipseParam[4];
float aspect = axis_b/axis_a;
for (int i = 0; i < 5; i++) {
eyeTrackedEllipse.ellipseParam[i] = ellipseParam[i];
}
//theta = ofRandom(0,TWO_PI);
int resolution = 24;
ofxPoint2f ptsForRotation[resolution];
for (int i=0; i<resolution; i++) {
float t = TWO_PI * (float)i/(float)resolution;
float ex = cx + (axis_a * cos(t ));
float ey = cy + (axis_b * sin(t ));
ptsForRotation[i].set(ex,ey);
}
for (int i=0; i<resolution; i++) {
ptsForRotation[i].rotate(theta * RAD_TO_DEG, ofxPoint2f(cx, cy));
}
currentEyePoint.set(cx, cy);
currentNormPoint.x = currentEyePoint.x;
currentNormPoint.y = currentEyePoint.y;
currentNormPoint.x /= w;
currentNormPoint.y /= h;
} else {
bFoundOne = false;
}
cvRelease((void **)&hull);
}
int* pupil_fitting_inliers(int width, int height, int* return_max_inliers_num)
{
int i;
int ep_num = gM_edge_point_N; //ep stands for edge point
tV2d nor_center;
double dis_scale;
int ellipse_point_num = 5; //number of point that needed to fit an ellipse
if (ep_num < ellipse_point_num) {
printf("Error! %d points are not enough to fit ellipse\n", ep_num);
memset(pupil_param, 0, sizeof(pupil_param));
*return_max_inliers_num = 0;
return NULL;
}
//Normalization
tV2d *edge_point_nor = normalize_edge_point(&dis_scale, &nor_center, ep_num);
//Ransac
int *inliers_index = (int*)malloc(sizeof(int)*ep_num);
int *max_inliers_index = (int*)malloc(sizeof(int)*ep_num);
int ninliers = 0;
int max_inliers = 0;
int sample_num = 1000; //number of sample
int ransac_count = 0;
double dis_threshold = sqrt(3.84)*dis_scale;
double dis_error;
memset(inliers_index, 0, sizeof(int)*ep_num);
memset(max_inliers_index, 0, sizeof(int)*ep_num);
int rand_index[5];
double A[6][6];
int M = 6, N = 6; //M is row; N is column
for (i = 0; i < N; i++) {
A[i][5] = 1;
A[5][i] = 0;
}
double **ppa = (double**)malloc(sizeof(double*)*M);
double **ppu = (double**)malloc(sizeof(double*)*M);
double **ppv = (double**)malloc(sizeof(double*)*N);
for (i = 0; i < M; i++) {
ppa[i] = A[i];
ppu[i] = (double*)malloc(sizeof(double)*N);
}
for (i = 0; i < N; i++) {
ppv[i] = (double*)malloc(sizeof(double)*N);
}
double pd[6];
int min_d_index;
double conic_par[6] = {0};
double ellipse_par[5] = {0};
double best_ellipse_par[5] = {0};
double ratio;
while (sample_num > ransac_count) {
get_5_random_num((ep_num-1), rand_index);
//svd decomposition to solve the ellipse parameter
for (i = 0; i < 5; i++) {
A[i][0] = edge_point_nor[rand_index[i]].x * edge_point_nor[rand_index[i]].x;
A[i][1] = edge_point_nor[rand_index[i]].x * edge_point_nor[rand_index[i]].y;
A[i][2] = edge_point_nor[rand_index[i]].y * edge_point_nor[rand_index[i]].y;
A[i][3] = edge_point_nor[rand_index[i]].x;
A[i][4] = edge_point_nor[rand_index[i]].y;
}
svd(M, N, ppa, ppu, pd, ppv);
min_d_index = 0;
for (i = 1; i < N; i++) {
if (pd[i] < pd[min_d_index])
min_d_index = i;
}
for (i = 0; i < N; i++)
conic_par[i] = ppv[i][min_d_index]; //the column of v that corresponds to the smallest singular value,
//which is the solution of the equations
ninliers = 0;
memset(inliers_index, 0, sizeof(int)*ep_num);
for (i = 0; i < ep_num; i++) {
dis_error = conic_par[0]*edge_point_nor[i].x*edge_point_nor[i].x +
conic_par[1]*edge_point_nor[i].x*edge_point_nor[i].y +
conic_par[2]*edge_point_nor[i].y*edge_point_nor[i].y +
conic_par[3]*edge_point_nor[i].x + conic_par[4]*edge_point_nor[i].y + conic_par[5];
if (fabs(dis_error) < dis_threshold) {
inliers_index[ninliers] = i;
ninliers++;
}
}
if (ninliers > max_inliers) {
if (solve_ellipse(conic_par, ellipse_par)) {
denormalize_ellipse_param(ellipse_par, ellipse_par, dis_scale, nor_center);
ratio = ellipse_par[0] / ellipse_par[1];
if (ellipse_par[2] > 0 && ellipse_par[2] <= width-1
&& ellipse_par[3] > 0 && ellipse_par[3] <= height-1
&& ratio > 0.5 && ratio < 2
) {
memcpy(max_inliers_index, inliers_index, sizeof(int)*ep_num);
for (i = 0; i < 5; i++) {
best_ellipse_par[i] = ellipse_par[i];
}
max_inliers = ninliers;
sample_num = (int)(log((double)(1-0.99))/log(1.0-pow(ninliers*1.0/ep_num, 5)));
}
}
}
ransac_count++;
if (ransac_count > 1500) {
printf("Error! ransac_count exceed! ransac break! sample_num=%d, ransac_count=%d\n", sample_num, ransac_count);
break;
}
}
//INFO("ransc end\n");
if (best_ellipse_par[0] > 0 && best_ellipse_par[1] > 0) {
for (i = 0; i < 5; i++) {
pupil_param[i] = best_ellipse_par[i];
}
}else {
memset(pupil_param, 0, sizeof(pupil_param));
max_inliers = 0;
free(max_inliers_index);
max_inliers_index = NULL;
}
for (i = 0; i < M; i++) {
free(ppu[i]);
free(ppv[i]);
}
free(ppu);
free(ppv);
free(ppa);
free(edge_point_nor);
free(inliers_index);
*return_max_inliers_num = max_inliers;
return max_inliers_index;
}
