void AxesLib::goToRads(float rx, float ry){
float degsH = (360.0 - _rad2deg(rx));
if(degsH >= 360.0)
degsH = degsH - 360.0;
_moveTo((float) degsH*_pgrad_x, (float) _rad2deg(ry)*_pgrad_y);
}
static double _rts_sun_altitude(double latitude, double delta_prime, double h_prime)
{
double latitude_rad = _deg2rad(latitude);
double delta_prime_rad = _deg2rad(delta_prime);
return _rad2deg (asin (sin(latitude_rad)*sin(delta_prime_rad) +
cos(latitude_rad)*cos(delta_prime_rad)*cos(_deg2rad(h_prime))));
}
static double _top_azimuth_angle_astro(double h_prime, double latitude, double delta_prime)
{
double h_prime_rad = _deg2rad(h_prime);
double lat_rad = _deg2rad(latitude);
return _wrap (_rad2deg (atan2(sin(h_prime_rad),
cos(h_prime_rad)*sin(lat_rad) - tan(_deg2rad(delta_prime))*cos(lat_rad))), 0, 360);
}
static double _top_elevation_angle (double latitude, double delta_prime, double h_prime)
{
double lat_rad = _deg2rad (latitude);
double delta_prime_rad = _deg2rad (delta_prime);
return _rad2deg (asin(sin(lat_rad)*sin(delta_prime_rad) +
cos(lat_rad)*cos(delta_prime_rad) * cos(_deg2rad(h_prime))));
}
/*!
* \brief
* Calculate geocentric declination
* \return [deg]
*/
static double _geo_declination(double beta, double epsilon, double lamda)
{
double beta_rad = _deg2rad (beta);
double epsilon_rad = _deg2rad (epsilon);
return _rad2deg (asin (sin(beta_rad)*cos(epsilon_rad) +
cos(beta_rad)*sin(epsilon_rad)*sin(_deg2rad(lamda))));
}
static double _incidence_angle (spa_output_t so, spa_location_t sl)
{
double zenith_rad = _deg2rad (so.zenith);
double slope_rad = _deg2rad (sl.slope);
return _rad2deg(acos(cos(zenith_rad)*cos(slope_rad) +
sin(slope_rad )*sin(zenith_rad) *
cos(_deg2rad(so.azimuth_astro - sl.azm_rotation))));
}
/*
* \brief
* Calculate topocentric declination and right ascension parallax at once
* \return [dec]
*/
static void _delta_alpha_prime(spa_location_t sl, double xi, double h, double delta, double *delta_alpha, double *delta_prime)
{
double delta_alpha_rad;
double lat_rad = _deg2rad (sl.latitude);
double xi_rad = _deg2rad (xi);
double h_rad = _deg2rad (h);
double delta_rad = _deg2rad (delta);
double u = atan(0.99664719 * tan(lat_rad));
double y = 0.99664719 * sin(u) + sl.elevation*sin(lat_rad)/6378140.0;
double x = cos(u) + sl.elevation*cos(lat_rad)/6378140.0;
delta_alpha_rad = atan2( - x*sin(xi_rad) *sin(h_rad),
cos(delta_rad) - x*sin(xi_rad) *cos(h_rad));
*delta_prime = _rad2deg (atan2((sin(delta_rad) - y*sin(xi_rad))*cos(delta_alpha_rad),
cos(delta_rad) - x*sin(xi_rad) *cos(h_rad)));
*delta_alpha = _rad2deg (delta_alpha_rad);
}
/*!
* \brief
* Calculate geocentric right ascension
* \return [deg]
*/
static double _geo_right_ascension(double lamda, double epsilon, double beta)
{
double lamda_rad = _deg2rad (lamda);
double epsilon_rad = _deg2rad (epsilon);
return _wrap (_rad2deg (
atan2( sin (lamda_rad)*cos (epsilon_rad) - tan (_deg2rad (beta))*sin (epsilon_rad),
cos (lamda_rad))),
0, 360);
}
/*!
* \brief
* Calculate Earth's heliocentric latitude
* \return [deg]
*/
static double _earth_hel_latitude (double jme)
{
double sum[B_COUNT];
int i;
for (i = 0; i < B_COUNT; i++)
sum[i] = _periodic_term_sum (B_TERMS[i], b_subcount[i], jme);
return _rad2deg (_earth_values (sum, B_COUNT, jme));
}
/*!
* \brief
* Calculate Earth's Heliocentric longitude
* \return [deg]
*/
static double _earth_hel_longitude (double jme)
{
double sum[L_COUNT];
int i;
for (i = 0; i < L_COUNT; i++)
sum[i] = _periodic_term_sum (L_TERMS[i], l_subcount[i], jme);
return _wrap (_rad2deg (_earth_values (sum, L_COUNT, jme)), 0, 360);
}
static double _rts_hour_angle_at_rise_set (double latitude, double delta_zero, double h0_prime)
{
double h0 = -99999;
double latitude_rad = _deg2rad (latitude);
double delta_zero_rad = _deg2rad (delta_zero);
double argument = (sin(_deg2rad(h0_prime)) - sin(latitude_rad)*sin(delta_zero_rad)) /
(cos(latitude_rad)*cos(delta_zero_rad));
if (fabs(argument) <= 1) h0 = _wrap (_rad2deg(acos(argument)), 0, 180);
return h0;
}
/**
* Calculate the distance between two geodetic location in miles.
*/
double shgeo_dist(shgeo_t *f_geo, shgeo_t *t_geo)
{
static const shnum_t mile_mod = 90.9;
shnum_t theta, dist;
shnum_t lat1, lat2;
shnum_t lon1, lon2;
shgeo_loc(f_geo, &lat1, &lon1, NULL);
shgeo_loc(t_geo, &lat2, &lon2, NULL);
theta = lon1 - lon2;
dist = (sinl(_deg2rad(lat1)) * sinl(_deg2rad(lat2))) +
(cosl(_deg2rad(lat1)) * cosl(_deg2rad(lat2)) * cosl(_deg2rad(theta)));
dist = acosl(dist);
dist = _rad2deg(dist);
dist = dist * mile_mod;
return ((double)dist);
}
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;
}
