void GazeTracker::update(const IplImage *image, const IplImage *eyegrey, vector<vector<int> > vector_h_v_combined) {
if (isActive()) {
// TODO ARCADI 23/12
//output.gazepoint = Point(gpx->getmean(SharedImage(image, &ignore)),
// gpy->getmean(SharedImage(image, &ignore)));
// TODO ONUR COMBINED H AND V
//vector<vector<int> > vector_h_v_combined;
//vector_h_v_combined.insert(vector_h_v_combined.end(), vector_horizontal.begin(), vector_horizontal.end());
//vector_h_v_combined.insert(vector_h_v_combined.end(), vector_vertical.begin(), vector_vertical.end());
//cout << "UPDATE! Vertical Vector size: " << vector_h_v_combined.size() << endl;
//cout.flush();
//output.gazepoint = Point(histx->getmean(vector_horizontal),
// histy->getmean(vector_vertical));
// Use combined only for Y
//output.gazepoint = Point(histx->getmean(vector_horizontal),
// histy->getmean(vector_h_v_combined));
cout << "Size of vector[5]: " << vector_h_v_combined.size() << endl;
// Use combined for both directions
output.gazepoint = Point(histx->getmean(vector_h_v_combined),
histy->getmean(vector_h_v_combined));
/*
vector<int> vector_horizontal_and_vertical;
copy( vector_horizontal.begin(), vector_horizontal.end(), back_inserter(vector_horizontal_and_vertical));
copy( vector_vertical.begin(), vector_vertical.end(), back_inserter(vector_horizontal_and_vertical));
output.gazepoint = Point(histx->getmean(vector_horizontal_and_vertical),
histy->getmean(vector_horizontal_and_vertical));
*/
//double* out = regression.CalculateOutput(vector_horizontal, vector_vertical);
//output.gazepoint = Point(out[0], out[1]);
output.targetid = getTargetId(output.gazepoint);
output.target = getTarget(output.targetid);
// Neural network
// Resize image to 16x8
cvResize(eyegrey, nn_eye);
cvEqualizeHist(nn_eye, nn_eye);
fann_type inputs[nn_eyewidth * nn_eyeheight];
for (int i = 0; i < nn_eyewidth * nn_eyeheight; ++i)
{
inputs[i] = (float)(nn_eye->imageData[i] + 129) / 257.0f;
}
fann_type* outputs = fann_run(ANN, inputs);
mapFromNeuralNetworkToScreenCoordinates(Point(outputs[0], outputs[1]), output.nn_gazepoint);
}
}
void GoalApproach::act()
{
//send message to move towards target
//get coordinates
Vector2d targetCo = getCore()->getCoordsSub()->getComponent(getTargetId())->getCoords();
//set movement
Parameters moveParameters;
moveParameters.push_back("move");
moveParameters.push_back("setLocalDest");
std::stringstream xSS(""), ySS("");
xSS << targetCo.x;
ySS << targetCo.y;
moveParameters.push_back(xSS.str());
moveParameters.push_back(ySS.str());
//send telegram
Message message(getSourceId() , getTargetId(), moveParameters);
Telegram telegram(getTargetId(), getTargetId(), 0.0, message);
getCore()->getMessageCentre()->addTelegram(telegram);
}
bool GoalApproach::met()
{
//get coords of source and target
//TODO: for met and act, DO NOT DO IT LIKE THIS! Will break if target or source is now null! Get comp* first then try for vector2d if not null
Vector2d sourceCo = getCore()->getCoordsSub()->getComponent(getSourceId())->getCoords();
Vector2d targetCo = getCore()->getCoordsSub()->getComponent(getTargetId())->getCoords();
if (sourceCo.getDistance(targetCo) <= minDistToTarget_)
{
return true;
}
return false;
}
void GazeTracker::update(const cv::Mat *image, const cv::Mat *eyeGrey) {
if (isActive()) {
output.gazePoint = Point(_gaussianProcessX->getmean(Utils::SharedImage(image, &ignore)), _gaussianProcessY->getmean(Utils::SharedImage(image, &ignore)));
output.targetId = getTargetId(output.gazePoint);
output.target = getTarget(output.targetId);
// Neural network
// Resize image to 16x8
cv::resize(*eyeGrey, *_nnEye, _nnEye->size());
cv::equalizeHist(*_nnEye, *_nnEye);
fann_type inputs[_nnEyeWidth * _nnEyeHeight];
for (int i = 0; i < _nnEyeWidth * _nnEyeHeight; ++i) {
inputs[i] = (float)(_nnEye->data[i] + 129) / 257.0f;
}
fann_type *outputs = fann_run(_ANN, inputs);
Utils::mapFromNeuralNetworkToScreenCoordinates(Point(outputs[0], outputs[1]), output.nnGazePoint);
}
}
void GazeTracker::calculateTrainingErrors() {
int numMonitors = Gdk::Screen::get_default()->get_n_monitors();
Gdk::Rectangle monitorGeometry;
Glib::RefPtr<Gdk::Screen> screen = Gdk::Display::get_default()->get_default_screen();
// Geometry of main monitor
screen->get_monitor_geometry(numMonitors - 1, monitorGeometry);
std::vector<Point> points = getSubVector(_calTargets, &CalTarget::point);
//std::cout << "Input count: " << _inputCount;
//std::cout << ", Target size: " << _calTargets.size() << std::endl;
for (int i = 0; i < _calTargets.size(); i++) {
double xTotal = 0;
double yTotal = 0;
double sampleCount = 0;
//std::cout << points[i].x << ", " << points[i].y << " x " << allOutputCoords[j][0] << ", " << allOutputCoords[j][1] << std::endl;
int j = 0;
while (j < _inputCount && points[i].x == allOutputCoords[j][0] && points[i].y == allOutputCoords[j][1]) {
double xEstimate = (_gaussianProcessX->getmean(Utils::SharedImage(allImages[j], &ignore)) + _gaussianProcessXLeft->getmean(Utils::SharedImage(allImagesLeft[j], &ignore))) / 2;
double yEstimate = (_gaussianProcessY->getmean(Utils::SharedImage(allImages[j], &ignore)) + _gaussianProcessYLeft->getmean(Utils::SharedImage(allImagesLeft[j], &ignore))) / 2;
//std::cout << "i, j = (" << i << ", " << j << "), est: " << xEstimate << "(" << _gaussianProcessX->getmean(SharedImage(allImages[j], &ignore)) << "," << _gaussianProcessXLeft->getmean(SharedImage(allImagesLeft[j], &ignore)) << ")" << ", " << yEstimate << "(" << _gaussianProcessY->getmean(SharedImage(allImages[j], &ignore)) << "," << _gaussianProcessYLeft->getmean(SharedImage(allImagesLeft[j], &ignore)) << ")" << std::endl;
xTotal += xEstimate;
yTotal += yEstimate;
sampleCount++;
j++;
}
xTotal /= sampleCount;
yTotal /= sampleCount;
*outputFile << "TARGET: (" << _calTargets[i].point.x << "\t, " << _calTargets[i].point.y << "\t),\tESTIMATE: (" << xTotal << "\t, " << yTotal << ")" << std::endl;
//std::cout << "TARGET: (" << _calTargets[i].point.x << "\t, " << _calTargets[i].point.y << "\t),\tESTIMATE: (" << xTotal << "\t, " << yTotal << "),\tDIFF: (" << fabs(_calTargets[i].point.x- x_total) << "\t, " << fabs(_calTargets[i].point.y - y_total) << ")" << std::endl;
// Calibration error removal
_xv[i][0] = xTotal; // Source
_xv[i][1] = yTotal;
// Targets
_fvX[i] = _calTargets[i].point.x;
_fvY[i] = _calTargets[i].point.y;
_sigv[i] = 0;
int targetId = getTargetId(Point(xTotal, yTotal));
if (targetId != i) {
std::cout << "Target id is not the expected one!! (Expected: "<< i << ", Current: " << targetId << ")" << std::endl;
}
}
// Add the corners of the monitor as 4 extra data points. This helps the correction for points that are near the edge of monitor
_xv[_calTargets.size()][0] = monitorGeometry.get_x();
_xv[_calTargets.size()][1] = monitorGeometry.get_y();
_fvX[_calTargets.size()] = monitorGeometry.get_x()-40;
_fvY[_calTargets.size()] = monitorGeometry.get_y()-40;
_xv[_calTargets.size()+1][0] = monitorGeometry.get_x() + monitorGeometry.get_width();
_xv[_calTargets.size()+1][1] = monitorGeometry.get_y();
_fvX[_calTargets.size()+1] = monitorGeometry.get_x() + monitorGeometry.get_width() + 40;
_fvY[_calTargets.size()+1] = monitorGeometry.get_y() - 40;
_xv[_calTargets.size()+2][0] = monitorGeometry.get_x() + monitorGeometry.get_width();
_xv[_calTargets.size()+2][1] = monitorGeometry.get_y() + monitorGeometry.get_height();
_fvX[_calTargets.size()+2] = monitorGeometry.get_x() + monitorGeometry.get_width() + 40;
_fvY[_calTargets.size()+2] = monitorGeometry.get_y() + monitorGeometry.get_height() + 40;
_xv[_calTargets.size()+3][0] = monitorGeometry.get_x();
_xv[_calTargets.size()+3][1] = monitorGeometry.get_y() + monitorGeometry.get_height();
_fvX[_calTargets.size()+3] = monitorGeometry.get_x() - 40;
_fvY[_calTargets.size()+3] = monitorGeometry.get_y() + monitorGeometry.get_height() + 40;
int pointCount = _calTargets.size() + 4;
int N = pointCount;
N = binomialInv(N, 2) - 1;
// Find the best beta and gamma parameters for interpolation
mirBetaGamma(1, 2, pointCount, (double *)_xv, _fvX, _sigv, 0, NULL, NULL, NULL, N, 2, 50.0, &_betaX, &_gammaX);
mirBetaGamma(1, 2, pointCount, (double *)_xv, _fvY, _sigv, 0, NULL, NULL, NULL, N, 2, 50.0, &_betaY, &_gammaY);
*outputFile << std::endl << std::endl;
std::cout << std::endl << std::endl;
outputFile->flush();
std::cout << "ERROR CALCULATION FINISHED. BETA = " << _betaX << ", " << _betaY << ", GAMMA IS " << _gammaX << ", " << _gammaY << std::endl;
for (int i = 0; i < pointCount; i++) {
std::cout << _xv[i][0] << ", " << _xv[i][1] << std::endl;
}
//checkErrorCorrection();
}
void GazeTracker::calculateTrainingErrors() {
int num_of_monitors = Gdk::Screen::get_default()->get_n_monitors();
Gdk::Rectangle monitorgeometry;
Glib::RefPtr<Gdk::Screen> screen = Gdk::Display::get_default()->get_default_screen();
// Geometry of main monitor
screen->get_monitor_geometry(num_of_monitors - 1, monitorgeometry);
vector<Point> points = getsubvector(caltargets, &CalTarget::point);
int j = 0;
//cout << "Input count: " << input_count;
//cout << ", Target size: " << caltargets.size() << endl;
for(int i=0; i<caltargets.size(); i++) {
double x_total = 0;
double y_total = 0;
double sample_count = 0;
//cout << points[i].x << ", " << points[i].y << " x " << all_output_coords[j][0] << ", " << all_output_coords[j][1] << endl;
while(j < input_count && points[i].x == all_output_coords[j][0] && points[i].y == all_output_coords[j][1]) {
double x_estimate = (gpx->getmean(SharedImage(all_images[j], &ignore)) + gpx_left->getmean(SharedImage(all_images_left[j], &ignore))) / 2;
double y_estimate = (gpy->getmean(SharedImage(all_images[j], &ignore)) + gpy_left->getmean(SharedImage(all_images_left[j], &ignore))) / 2;
//cout << "i, j = (" << i << ", " << j << "), est: " << x_estimate << "("<< gpx->getmean(SharedImage(all_images[j], &ignore)) << ","<< gpx_left->getmean(SharedImage(all_images_left[j], &ignore)) << ")" << ", " << y_estimate << "("<< gpy->getmean(SharedImage(all_images[j], &ignore)) <<","<< gpy_left->getmean(SharedImage(all_images_left[j], &ignore)) << ")"<< endl;
x_total += x_estimate;
y_total += y_estimate;
sample_count++;
j++;
}
x_total /= sample_count;
y_total /= sample_count;
*output_file << "TARGET: (" << caltargets[i].point.x << "\t, " << caltargets[i].point.y << "\t),\tESTIMATE: ("<< x_total << "\t, " << y_total <<")" << endl;
//cout << "TARGET: (" << caltargets[i].point.x << "\t, " << caltargets[i].point.y << "\t),\tESTIMATE: ("<< x_total << "\t, " << y_total <<"),\tDIFF: ("<< fabs(caltargets[i].point.x- x_total) << "\t, " << fabs(caltargets[i].point.y - y_total) <<")" << endl;
// Calibration error removal
xv[i][0] = x_total; // Source
xv[i][1] = y_total;
// Targets
fv_x[i] = caltargets[i].point.x;
fv_y[i] = caltargets[i].point.y;
sigv[i] = 0;
int targetId = getTargetId(Point(x_total, y_total));
if(targetId != i) {
cout << "Target id is not the expected one!! (Expected: "<< i<< ", Current: "<< targetId << ")" << endl;
}
}
// Add the corners of the monitor as 4 extra data points. This helps the correction for points that are near the edge of monitor
xv[caltargets.size()][0] = monitorgeometry.get_x();
xv[caltargets.size()][1] = monitorgeometry.get_y();
fv_x[caltargets.size()] = monitorgeometry.get_x()-40;
fv_y[caltargets.size()] = monitorgeometry.get_y()-40;
xv[caltargets.size()+1][0] = monitorgeometry.get_x() + monitorgeometry.get_width();
xv[caltargets.size()+1][1] = monitorgeometry.get_y();
fv_x[caltargets.size()+1] = monitorgeometry.get_x() + monitorgeometry.get_width() + 40;
fv_y[caltargets.size()+1] = monitorgeometry.get_y() - 40;
xv[caltargets.size()+2][0] = monitorgeometry.get_x() + monitorgeometry.get_width();
xv[caltargets.size()+2][1] = monitorgeometry.get_y() + monitorgeometry.get_height();
fv_x[caltargets.size()+2] = monitorgeometry.get_x() + monitorgeometry.get_width() + 40;
fv_y[caltargets.size()+2] = monitorgeometry.get_y() + monitorgeometry.get_height() + 40;
xv[caltargets.size()+3][0] = monitorgeometry.get_x();
xv[caltargets.size()+3][1] = monitorgeometry.get_y() + monitorgeometry.get_height();
fv_x[caltargets.size()+3] = monitorgeometry.get_x() - 40;
fv_y[caltargets.size()+3] = monitorgeometry.get_y() + monitorgeometry.get_height() + 40;
int point_count = caltargets.size() + 4;
int N = point_count;
N = binomialInv(N, 2) - 1;
// Find the best beta and gamma parameters for interpolation
mirBetaGamma(1, 2, point_count, (double*)xv, fv_x, sigv, 0, NULL, NULL, NULL,
N, 2, 50.0, &beta_x, &gamma_x);
mirBetaGamma(1, 2, point_count, (double*)xv, fv_y, sigv, 0, NULL, NULL, NULL,
N, 2, 50.0, &beta_y, &gamma_y);
*output_file << endl << endl;
cout << endl << endl;
output_file->flush();
cout << "ERROR CALCULATION FINISHED. BETA = " << beta_x << ", " << beta_y << ", GAMMA IS " << gamma_x << ", " << gamma_y << endl;
for(int j=0; j<point_count; j++) {
cout << xv[j][0] << ", " << xv[j][1] << endl;
}
//checkErrorCorrection();
}
