/* * Copyright © 2002, Roland Roberts * * RCS Revision * @(#) $Id$ * $Source$ * * Synopsis * * Find the best parameters for a double-arm barndoor tracker given * certain construction constraints. * * Description * * When designing a double-arm barndoor tracker, normally only three * parameters are considered: (1) the distance between the drive-arm and * camera-platform hinges; (2) the distance between the drive-arm hinge * and the drive screw when the arm is closed; and (3) the distance * between the camera-platform hinge and the point of sliding contact * between the drive arm and camera platform. * * Use of these parameters assumes that the attachment points all lie in * the same plane, namely that formed by the drive-arm and camera-platform * hinges. In practice, this is rarely the case; every picture I've seen * has the actual attachment points for the drive screw offset from this * plane. Additionally, it is not unusual for the point of sliding * contact to be shifted slightly due to the addition of a low-friction * bearing (e.g., a teflon pad). * * This program calculates the optimum values of the nominal parameters, * 'r', 'b', and 'c' given initial values of those parameters and fixed * values for the offsets 'u', and 'w'. The definitions of these * parameters is: * * r - the nominal drive-arm length; the distance from the drive-arm * hinge to the point of attachment for the drive screw. * * b - the nominal distance between the camera-platform hinge and the * point of sliding contact between the two arms. * * c - the nominal distance between the drive-arm and camera-platform * hinges. * * u - the offset *above* the hinge plane to the actual point of * attachment of the drive screw to the drive platform, meaured * when the tracker is the fully closed position. * * w - the offset *below* the hinge plane to the actual point of * attachment of the drive screw to the base platform, meaured when * the tracker is the fully closed position. * * Bugs/Notes * */ #ifdef HAVE_CONFIG_H # include "config.h" #endif #ifndef __GNUC__ # define __attribute__(x) #endif static char __attribute__ ((unused)) RCSid[] = "@(#) $Id$"; /****************************** Include Files *******************************/ #include #include #include #include #include #include /************************** Project Include Files ***************************/ #include "version.h" /******************************* Definitions ********************************/ /********************************** Types ***********************************/ enum BD_ERRTYPE { BD_ERRTYPE_CHISQ, BD_ERRTYPE_RMS, BD_ERRTYPE_ABS, BD_ERRTYPE_ABSMAX }; struct OPTIONS { const char *progName; int verbose; enum BD_ERRTYPE errType; double b, c, r; double u, v, w; double dL; double startTime, stopTime, dT; }; /********************************* Globals **********************************/ /******************************* File Globals *******************************/ /* Short options list. */ static const char _optsShort[] = "b:c:w:u:r:t:E:hVv"; /* Long options list. */ static struct option _optsLong[] = { { "b", required_argument, 0, 'b' }, { "c", required_argument, 0, 'c' }, { "w", required_argument, 0, 'w' }, { "u", required_argument, 0, 'u' }, { "r", required_argument, 0, 'r' }, { "dt", required_argument, 0, 't' }, { "end-time", required_argument, 0, 'E' }, { "help", no_argument, 0, 'h' }, { "version", no_argument, 0, 'V' }, { "verbose", no_argument, 0, 'v' }, { 0, 0, 0, 0 } }; static char *_optsHelp = "Options:\n" ; static struct OPTIONS opt; /******************************** Prototypes ********************************/ static inline void ShowVersion (); static inline void ShowUsage (); static void InitOptions (int argc, char **argv); static int SetOptions (int argc, char **argv); /* * ShowVersion * * Synopsis * Print (on stdout) a formatted message containing version information. * * Arguments * None * * Author * Roland Roberts * * Bugs/Notes * * * Return Values * None */ static inline void ShowVersion () { printf ("%s version %s\n released %s\n", opt.progName, version, releaseDate); return; } /* * ShowUsage * * Synopsis * Print (on stdout) a formatted message containing usage information. The * usage information is preceeded by version information. * * Arguments * None * * Author * Roland Roberts * * Bugs/Notes * * * Return Values * None */ static inline void ShowUsage () { ShowVersion (); printf ("Usage: %s OPTIONS\n%s", opt.progName, _optsHelp); return; } /* * InitOptions * * Synopsis * Set default/initial options for the program run. This is a convenience * function whose whole purpose is to insure a known state for the OPTIONS * structure. * * Arguments * +--- Access, R=read, W=write * | +--- Type, name and description * V V * [R] int argc * Passed down from main(). * [R] char **argv * Passed down from main(). * * Author * Roland Roberts * * Bugs/Notes * * * Return Values * None */ static void InitOptions (int argc, char **argv) { opt.verbose = 0; if ((opt.progName = strrchr (argv[0], '/')) == 0) opt.progName = strdup (argv[0]); else opt.progName = strdup (opt.progName+1); /* Distance from drive-arm hinge to attachment point of the drive screw. */ opt.r = 17.142; /* Distance from camera platform hinge to point of sliding contact between camera platform and drive arm. */ opt.b = 10.0; /* Distance between hinges of camera platform and drive arm. */ opt.c = 5.0; /* Perpendicular offset of pivot point on drive arm from plane of drive arm surface. */ opt.u = 0.25; /* Perpendicular offset of pivot point on base from hinge plane. */ opt.w = 1.50; /* Amount drive screw is extended in one second. */ opt.dL = 1.0 / 1200.0; /* Start time for integration. */ opt.startTime = 0.0; /* Stop time for integration. */ opt.stopTime = 3600.0; /* Step size for integration; fundamental unit of time is the second, so this should be in seconds. */ opt.dT = 60.0; /* Type of error calculation to use for minimization. The default is to do a chi-squared calculation same as if we are fitting to the ideal function phi(T) = T / 15. */ opt.errType = BD_ERRTYPE_RMS; return; } /* * SetOptions * * Synopsis * Set options (in global `struct OPTIONS opt') based on command-line * parameters. This does not act on any of the options settings; it * merely initializes them to the correct state based on the command-line. * * Arguments * +--- Access, R=read, W=write * | +--- Type, name and description * V V * [R] int argc * Passed down from main(). * [R] char **argv * Passed down from main(). * * Author * Roland Roberts * * Bugs/Notes * Needs to catch case where no Freeway port is selected and either provide * a default (hard) or just complain and fail. * * Return Values * N - option index of the next entry in argv which needs to be processed. * This is a growth option as nothing should ever be left unprocessed. */ static int SetOptions (int argc, char **argv) { int c, index; optind = index = 0; while ((c = getopt_long (argc, argv, _optsShort, _optsLong, &index)) != EOF) { switch (c) { case 'r': opt.r = strtod (optarg, 0); break; case 'b' : opt.b = strtod (optarg, 0); break; case 'c' : opt.c = strtod (optarg, 0); break; case 'w' : opt.w = strtod (optarg, 0); break; case 'u' : opt.u = strtod (optarg, 0); break; case 't' : opt.dT = strtod (optarg, 0); break; case 'E' : opt.stopTime = strtod (optarg, 0); break; case 'v' : opt.verbose++; break; case 'q' : opt.verbose = 0; break; case 'V' : ShowVersion (); exit (0); case 'h' : ShowUsage (); exit (0); case '?' : fprintf (stderr, "%s: unrecognized option '%s'\n", opt.progName, argv[optind-1]); ShowUsage (); exit (1); default : break; } } return 0; } static void my_FdF (const gsl_vector *X, void *params, double *F, gsl_vector *dF) { struct OPTIONS *opt = (struct OPTIONS *) params; double r = gsl_vector_get (X, 0); double beta = gsl_vector_get (X, 1); const double gamma = 2.0 * M_PI / 86400.0; const double scale = 3600.0 * 180.0 / M_PI; double u = opt->u; double w = opt->w; double p2 = r * r + u * u; double q2 = r * r + w * w; double delta = atan (u/r) + atan (w/r); double E, dE_dr, dE_dbeta; double T, L; size_t i; if (opt->verbose > 1) { if (dF == NULL) { printf ("my_F ({%g,%g})\n", r, beta); printf (" T L theta phi err\n"); } else { if (F == NULL) printf ("my_dF ({%g,%g})\n", r, beta); else printf ("my_FdF ({%g,%g})\n", r, beta); printf (" T L theta phi err dE/dr dE/dbeta\n"); } } /* Total error for the function. */ E = 0.0; dE_dr = 0.0; dE_dbeta = 0.0; for (T = opt->startTime, i = 0; T <= opt->stopTime; T += opt->dT, i++) { double A, dA_dr, B, C, xerr; double chi, theta, phi; double dphi_dr, dphi_dbeta; L = opt->dL * T + u + w; A = (p2 + q2 - L * L) / (2.0 * sqrt (p2*q2)); if (A > 1.0) A = 1.0; chi = acos (A); theta = chi - delta; phi = theta + asin (sin(theta)/beta); xerr = scale * (phi - gamma * T); /* errType == DB_ERRTYPE_CHISQ. */ E += xerr * xerr; if (dF != NULL) { B = sin(theta)/beta; C = 1.0 / sqrt (1.0 - B * B); dphi_dbeta = -B * C / beta; dA_dr = (r/(sqrt(p2*q2))) * (2.0 - A * (sqrt(p2/q2) + sqrt(q2/p2))); dphi_dr = -dA_dr / sin(chi) * (1.0 + B * cos(theta)); if (isinf(dphi_dr) || isnan(dphi_dr)) dphi_dr = 0.0; /* errType == DB_ERRTYPE_CHISQ. */ dE_dbeta += E * dphi_dbeta; dE_dr += E * dphi_dr; } if (opt->verbose > 1) if (dF != NULL) printf ("%5.0f %6.3f %6.3f %6.3f %6.2f %8.3g %8.5g\n", T, L, theta * 180/M_PI, phi * 180/M_PI, E, E * dphi_dr, E * dphi_dbeta); else printf ("%5.0f %6.3f %6.3f %6.3f %6.2f\n", T, L, theta * 180/M_PI, phi * 180/M_PI, E); } if (F != NULL) *F = E; if (dF != NULL) { gsl_vector_set (dF, 0, 2.0 * dE_dr); gsl_vector_set (dF, 1, 2.0 * dE_dbeta); } return; } static double my_F (const gsl_vector *X, void *params) { double F; my_FdF (X, params, &F, NULL); return F; } static void my_dF (const gsl_vector *X, void *params, gsl_vector *dF) { my_FdF (X, params, NULL, dF); return; } int main (int argc, char **argv) { /* Number of parameters. */ const size_t np = 2; const gsl_multimin_fdfminimizer_type *T; gsl_multimin_fdfminimizer *s; gsl_vector *X; gsl_multimin_function_fdf barndoor; size_t iter; int status; double RMS; InitOptions (argc, argv); SetOptions (argc, argv); barndoor.f = &my_F; barndoor.df = &my_dF; barndoor.fdf = &my_FdF; barndoor.n = np; barndoor.params = &opt; /* Starting point for search taken from options. */ X = gsl_vector_alloc (np); gsl_vector_set (X, 0, opt.r); gsl_vector_set (X, 1, opt.b / opt.c); /* T = gsl_multimin_fdfminimizer_steepest_descent; */ /* T = gsl_multimin_fdfminimizer_conjugate_fr; */ /* T = gsl_multimin_fdfminimizer_conjugate_pr; */ T = gsl_multimin_fdfminimizer_vector_bfgs; s = gsl_multimin_fdfminimizer_alloc (T, np); /* The ability to find a solution seems to be *very* sensitive to the value of STEP in this initialization. STEP=0.1 and you get no NaN all over the place; STEP=0.01 has the same problem. It is less sensitive to TOL. */ gsl_multimin_fdfminimizer_set (s, &barndoor, X, 0.05, 0.001); iter = 0; print_state (iter, s); do { iter++; status = gsl_multimin_fdfminimizer_iterate (s); if (status) break; status = gsl_multimin_test_gradient (s->gradient, 1e-4); if (opt.verbose) { if (status == GSL_SUCCESS) printf ("\nMinimum found at:\n"); print_state (iter, s); } RMS = sqrt(s->f/((opt.stopTime-opt.startTime)/opt.dT + 1)); } while ((RMS > 0.4) && (status == GSL_CONTINUE) && (iter < 100)); gsl_multimin_fdfminimizer_free (s); gsl_vector_free (X); exit (0); } int print_state (size_t iter, gsl_multimin_fdfminimizer * s) { printf ("iter: %3u x = %15.8f %15.8f " "f(x) = %12g RMS = %12g " "|g(x)| = %12g g(x) = %g %g\n", iter, gsl_vector_get (s->x, 0), gsl_vector_get (s->x, 1), s->f, sqrt(s->f/((opt.stopTime-opt.startTime)/opt.dT + 1)), gsl_blas_dnrm2 (s->gradient), gsl_vector_get (s->gradient, 0), gsl_vector_get (s->gradient, 1) ); } /* * Revision History (auto-generated) * $Log$ */