int main(int argc, char *argv[])
{
auto options = std::unique_ptr<getopt_t, void(*)(getopt_t*)> (getopt_create(), getopt_destroy);
// getopt_t *options = getopt_create();
getopt_add_bool(options.get(), 'h', "help", 0, "Show this help");
getopt_add_bool(options.get(), 'd', "debug", 0, "Enable debugging output (slow)");
getopt_add_bool(options.get(), 'w', "window", 1, "Show the detected tags in a window");
getopt_add_bool(options.get(), 'q', "quiet", 0, "Reduce output");
getopt_add_int(options.get(), '\0', "border", "1", "Set tag family border size");
getopt_add_int(options.get(), 't', "threads", "4", "Use this many CPU threads");
getopt_add_double(options.get(), 'x', "decimate", "1.0", "Decimate input image by this factor");
getopt_add_double(options.get(), 'b', "blur", "0.0", "Apply low-pass blur to input");
getopt_add_bool(options.get(), '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
getopt_add_bool(options.get(), '1', "refine-decode", 0, "Spend more time trying to decode tags");
getopt_add_bool(options.get(), '2', "refine-pose", 0, "Spend more time trying to precisely localize tags");
getopt_add_double(options.get(), 's', "size", "0.04047", "Physical side-length of the tag (meters)");
getopt_add_int(options.get(), 'c', "camera", "0", "Camera ID");
getopt_add_int(options.get(), 'i', "tag_id", "-1", "Tag ID (-1 for all tags in family)");
if (!getopt_parse(options.get(), argc, argv, 1) || getopt_get_bool(options.get(), "help")) {
printf("Usage: %s [options]\n", argv[0]);
getopt_do_usage(options.get());
exit(0);
}
AprilTagDetector tag_detector(options);
auto lcm = std::make_shared<lcm::LCM>();
Eigen::Matrix3d camera_matrix = Eigen::Matrix3d::Identity();
// camera_matrix(0,0) = bot_camtrans_get_focal_length_x(mCamTransLeft);
// camera_matrix(1,1) = bot_camtrans_get_focal_length_y(mCamTransLeft);
// camera_matrix(0,2) = bot_camtrans_get_principal_x(mCamTransLeft);
// camera_matrix(1,2) = bot_camtrans_get_principal_y(mCamTransLeft);
camera_matrix(0,0) = 535.04778754;
camera_matrix(1,1) = 533.37100256;
camera_matrix(0,2) = 302.83654976;
camera_matrix(1,2) = 237.69023961;
Eigen::Vector4d distortion_coefficients(-7.74010810e-02, -1.97835565e-01, -4.47956948e-03, -5.42361499e-04);
// camera matrix:
// [[ 535.04778754 0. 302.83654976]
// [ 0. 533.37100256 237.69023961]
// [ 0. 0. 1. ]]
// distortion coefficients: [ -7.74010810e-02 -1.97835565e-01 -4.47956948e-03 -5.42361499e-04
// 9.30985112e-01]
cv::VideoCapture capture(getopt_get_int(options.get(), "camera"));
if (!capture.isOpened()) {
std::cout << "Cannot open the video cam" << std::endl;
return -1;
}
cv::Mat frame;
Eigen::Isometry3d tag_to_camera = Eigen::Isometry3d::Identity();
crazyflie_t::webcam_pos_t tag_to_camera_msg;
while (capture.read(frame)) {
std::vector<TagMatch> tags = tag_detector.detectTags(frame);
if (tags.size() > 0) {
tag_to_camera = getRelativeTransform(tags[0], camera_matrix, distortion_coefficients, tag_detector.getTagSize());
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = 1;
} else {
tag_to_camera_msg = encodeWebcamPos(tag_to_camera);
tag_to_camera_msg.frame_id = -1;
}
tag_to_camera_msg.timestamp = timestamp_now();
lcm->publish("WEBCAM_POS", &tag_to_camera_msg);
}
return 0;
}
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);
}
}
