explicit
SkipList()
{
initializeHead();
}
int main(int argc, char* argv[])
{
int boardN = 0;
int ret = 0;
int rhoSize = 256;
int thetaSize = 180;
YARPImageOf<YarpPixelMono> hough_img;
char c;
printf("\n[eyeCalib] Setting up the sensor..");
ret = _grabber.initialize(boardN, _sizeX, _sizeY);
if (ret == YARP_FAIL)
{
printf("[eyeCalib] ERROR in _grabber.initialize(). Quitting.\n");
exit (1);
}
initializeHead();
printf("\n[eyeCalib] Allocating images and filter (%d x %d)...", _sizeX, _sizeY);
for (int i=0; i<N_IMAGES; i++)
_imgBuffer[i].Resize (_sizeX, _sizeY);
_susanImg.Resize (_sizeX, _sizeY);
_susanFlt.resize(_sizeX, _sizeY);
printf("\n[eyeCalib] Allocating Hough Filter (theta %d, rho %d)..", thetaSize, rhoSize);
_houghFlt.resize(thetaSize, rhoSize);
char fileName[255];
FILE *outFile = NULL;
double orientation;
double posVector[4];
double vel;
printf("\n[eyeCalib] Do you want to save results to file ? [Y/n] ");
c = getch();
if (c != 'n')
{
printf("\n[eyeCalib] File name ? ");
scanf("%s", fileName);
printf("[eyeCalib] Append data (Y/n)?");
c = getch();
if (c == 'n')
{
outFile = fopen(fileName,"w");
fprintf(outFile, "velocity;J0;J1;J2;J3;orientation\n");
}
else
{
outFile = fopen(fileName,"a");
}
if (!outFile)
{
printf("[eyeCalib] ERROR opening the file %s. Saving aboorted.\n", fileName);
}
}
printf("\n[eyeCalib] Interactive calibration or Load data form file? [I/l] ");
c = getch();
if ( c == 'l')
{
printf("\n[eyeCalib] File to load? ");
scanf("%s", fileName);
FILE *dataFile = NULL;
dataFile = fopen(fileName,"r");
if (dataFile == NULL)
{
printf("[eyeCalib] ERROR opening the file %s. Quitting.\n", fileName);
releaseSensor();
if (outFile != NULL)
fclose(outFile);
exit(1);
}
bool ret = false;
while (readLine(dataFile, &vel, posVector) == true)
{
printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel);
printf("\n[eyeCalib] Hit any key when Position has been reached..");
moveHead(vel,posVector);
c = getch();
orientation = calibrate();
printf("\n[eyeCalib] Main orientation is %.3frad (%.3fdeg) - variance %.2f", orientation, _rad2deg(orientation));
if (outFile != NULL)
fprintf(outFile, "%lf;%lf;%lf;%lf;%lf;%lf\n", vel, posVector[0], posVector[1], posVector[2], posVector[3], orientation);
}
fclose(dataFile);
}
else
{
do
{
printf("\n[eyeCalib] Avaible Joints:\n\t0 - Left Pan\n\t1 - Left Tilt\n\t2 - Right Pan\n\t3 - Left Tilt");
printf("\n[EyeCalib] Move to? [J0;J1;J2;J3] ");
scanf("%lf;%lf;%lf;%lf", &(posVector[0]), &(posVector[1]), &(posVector[2]), &(posVector[3]));
printf("\n[eyeCalib] Move with velocity? ");
scanf("%lf", &vel);
printf("\n[eyeCalib] Moving [%.2lf %.2lf %.2lf %.2lf * %.2lf]..", posVector[0], posVector[1], posVector[2], posVector[3], vel);
printf("\n[eyeCalib] Hit any key when Position has been reached..");
moveHead(vel, posVector);
c = getch();
orientation = calibrate();
printf("\n[eyeCalib] Main orientation is %.3frad (%.3fdeg) - variance %.2f", orientation, _rad2deg(orientation));
if (outFile != NULL)
fprintf(outFile, "%lf;%lf;%lf;%lf;%lf;%lf\n", vel, posVector[0], posVector[1], posVector[2], posVector[3], orientation);
printf("\n[eyeCalib] Another loop ? [Y/n] ");
c = getch();
} while (c != 'n');
}
if (outFile != NULL)
fclose(outFile);
releaseSensor();
releaseHead();
printf("\n[eyeCalib] Bye.\n");
return 0;
}
