AprilTagDetector(const std::unique_ptr<getopt_t, void(*)(getopt_t*)> &options){ tf = tag16h5_create(); tf->black_border = getopt_get_int(options.get(), "border"); td = apriltag_detector_create(); apriltag_detector_add_family(td, tf); show_window = getopt_get_bool(options.get(), "window"); td->quad_decimate = getopt_get_double(options.get(), "decimate"); td->quad_sigma = getopt_get_double(options.get(), "blur"); td->nthreads = getopt_get_int(options.get(), "threads"); td->debug = getopt_get_bool(options.get(), "debug"); td->refine_edges = getopt_get_bool(options.get(), "refine-edges"); td->refine_decode = getopt_get_bool(options.get(), "refine-decode"); td->refine_pose = getopt_get_bool(options.get(), "refine-pose"); quiet = getopt_get_bool(options.get(), "quiet"); tag_size = getopt_get_double(options.get(), "size"); tag_id = getopt_get_int(options.get(), "tag_id"); }
int main(int argc, char *argv[]) { getopt_t *getopt = getopt_create(); getopt_add_bool(getopt, 'h', "help", 0, "Show this help"); getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)"); getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output"); getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use"); getopt_add_int(getopt, '\0', "border", "1", "Set tag family border size"); getopt_add_int(getopt, 'i', "iters", "1", "Repeat processing on input set this many times"); getopt_add_int(getopt, 't', "threads", "4", "Use this many CPU threads"); getopt_add_double(getopt, 'x', "decimate", "1.0", "Decimate input image by this factor"); getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input"); getopt_add_bool(getopt, '1', "refine-decode", 0, "Spend more time trying to decode tags"); getopt_add_bool(getopt, '2', "refine-pose", 0, "Spend more time trying to precisely localize tags"); if (!getopt_parse(getopt, argc, argv, 1) || getopt_get_bool(getopt, "help")) { printf("Usage: %s [options] <input files>\n", argv[0]); getopt_do_usage(getopt); exit(0); } const zarray_t *inputs = getopt_get_extra_args(getopt); apriltag_family_t *tf = NULL; const char *famname = getopt_get_string(getopt, "family"); if (!strcmp(famname, "tag36h11")) tf = tag36h11_create(); else if (!strcmp(famname, "tag36h10")) tf = tag36h10_create(); else if (!strcmp(famname, "tag36artoolkit")) tf = tag36artoolkit_create(); else if (!strcmp(famname, "tag25h9")) tf = tag25h9_create(); else if (!strcmp(famname, "tag25h7")) tf = tag25h7_create(); else { printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n"); exit(-1); } tf->black_border = getopt_get_int(getopt, "border"); apriltag_detector_t *td = apriltag_detector_create(); apriltag_detector_add_family(td, tf); td->quad_decimate = getopt_get_double(getopt, "decimate"); td->quad_sigma = getopt_get_double(getopt, "blur"); td->nthreads = getopt_get_int(getopt, "threads"); td->debug = getopt_get_bool(getopt, "debug"); td->refine_decode = getopt_get_bool(getopt, "refine-decode"); td->refine_pose = getopt_get_bool(getopt, "refine-pose"); int quiet = getopt_get_bool(getopt, "quiet"); int maxiters = getopt_get_int(getopt, "iters"); const int hamm_hist_max = 10; for (int iter = 0; iter < maxiters; iter++) { if (maxiters > 1) printf("iter %d / %d\n", iter + 1, maxiters); for (int input = 0; input < zarray_size(inputs); input++) { int hamm_hist[hamm_hist_max]; memset(hamm_hist, 0, sizeof(hamm_hist)); char *path; zarray_get(inputs, input, &path); if (!quiet) printf("loading %s\n", path); image_u8_t *im = image_u8_create_from_pnm(path); if (im == NULL) { printf("couldn't find %s\n", path); continue; } zarray_t *detections = apriltag_detector_detect(td, im); for (int i = 0; i < zarray_size(detections); i++) { apriltag_detection_t *det; zarray_get(detections, i, &det); if (!quiet) printf("detection %3d: id (%2dx%2d)-%-4d, hamming %d, goodness %8.3f, margin %8.3f\n", i, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin); hamm_hist[det->hamming]++; apriltag_detection_destroy(det); } zarray_destroy(detections); if (!quiet) { timeprofile_display(td->tp); printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads); } if (!quiet) printf("Hamming histogram: "); for (int i = 0; i < hamm_hist_max; i++) printf("%5d", hamm_hist[i]); if (quiet) { printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3); } printf("\n"); image_u8_destroy(im); } } // don't deallocate contents of inputs; those are the argv apriltag_detector_destroy(td); tag36h11_destroy(tf); return 0; }
int main (int argc, char *argv[]) { // so that redirected stdout won't be insanely buffered. setvbuf (stdout, (char *) NULL, _IONBF, 0); state_t *state = calloc (1, sizeof *state); state->meters_per_tick = METERS_PER_TICK; // IMPLEMENT ME state->alpha = ALPHA; state->beta = BETA; // file for gyro integration test //state->fp = fopen("gyro-yaw-integration.txt","w"); state->fp = fopen("yaw.txt","w"); // Tests for the position estimate /* update_position(state,4456,4456); update_position(state,0,1208); update_position(state,4456,4456); while(1); */ // Used for testing the gyro bias /* for(int i=0; i<100; i++){ state->yaw_cal_array[i] = 15*i; } state->gyro_bias = find_gyro_bias(state); printf("Gyro Bias: %llu\n", state->gyro_bias); */ //printf("getting options\n"); state->gopt = getopt_create (); getopt_add_bool (state->gopt, 'h', "help", 0, "Show help"); getopt_add_bool (state->gopt, 'g', "use-gyro", 0, "Use gyro for heading instead of wheel encoders"); getopt_add_string (state->gopt, '\0', "odometry-channel", "BOTLAB_ODOMETRY", "LCM channel name"); getopt_add_string (state->gopt, '\0', "feedback-channel", "MAEBOT_MOTOR_FEEDBACK", "LCM channel name"); getopt_add_string (state->gopt, '\0', "sensor-channel", "MAEBOT_SENSOR_DATA", "LCM channel name"); getopt_add_double (state->gopt, '\0', "alpha", ALPHA_STRING, "Longitudinal covariance scaling factor"); getopt_add_double (state->gopt, '\0', "beta", BETA_STRING, "Lateral side-slip covariance scaling factor"); getopt_add_double (state->gopt, '\0', "gyro-rms", GYRO_RMS_STRING, "Gyro RMS deg/s"); if (!getopt_parse (state->gopt, argc, argv, 1) || getopt_get_bool (state->gopt, "help")) { printf ("Usage: %s [--url=CAMERAURL] [other options]\n\n", argv[0]); getopt_do_usage (state->gopt); exit (EXIT_FAILURE); } state->use_gyro = getopt_get_bool (state->gopt, "use-gyro"); state->odometry_channel = getopt_get_string (state->gopt, "odometry-channel"); state->feedback_channel = getopt_get_string (state->gopt, "feedback-channel"); state->sensor_channel = getopt_get_string (state->gopt, "sensor-channel"); state->alpha = getopt_get_double (state->gopt, "alpha"); state->beta = getopt_get_double (state->gopt, "beta"); state->gyro_rms = getopt_get_double (state->gopt, "gyro-rms") * DTOR; state->yaw_calibrated = 0; state->yaw = 0; state->yaw_old = 0; //printf("subscribing to channels\n"); // initialize LCM state->lcm = lcm_create (NULL); maebot_motor_feedback_t_subscribe (state->lcm, state->feedback_channel, motor_feedback_handler, state); maebot_sensor_data_t_subscribe (state->lcm, state->sensor_channel, sensor_data_handler, state); printf ("ticks per meter: %f\n", 1.0/state->meters_per_tick); while (1){ lcm_handle (state->lcm); } }