// do the edges of polya and polyb collide? (Does NOT test for containment).
int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb)
{
// do any of the line segments collide? If so, the answer is no.
// dumb N^2 method.
for (int ia = 0; ia < zarray_size(polya); ia++) {
double pa0[2], pa1[2];
zarray_get(polya, ia, pa0);
zarray_get(polya, (ia+1)%zarray_size(polya), pa1);
g2d_line_segment_t sega;
g2d_line_segment_init_from_points(&sega, pa0, pa1);
for (int ib = 0; ib < zarray_size(polyb); ib++) {
double pb0[2], pb1[2];
zarray_get(polyb, ib, pb0);
zarray_get(polyb, (ib+1)%zarray_size(polyb), pb1);
g2d_line_segment_t segb;
g2d_line_segment_init_from_points(&segb, pb0, pb1);
if (g2d_line_segment_intersect_segment(&sega, &segb, NULL))
return 1;
}
}
return 0;
}
static void do_quad_task(void *p)
{
struct quad_task *task = (struct quad_task*) p;
zarray_t *clusters = task->clusters;
zarray_t *quads = task->quads;
apriltag_detector_t *td = task->td;
int w = task->w, h = task->h;
for (int cidx = task->cidx0; cidx < task->cidx1; cidx++) {
zarray_t *cluster;
zarray_get(clusters, cidx, &cluster);
if (zarray_size(cluster) < td->qtp.min_cluster_pixels)
continue;
// a cluster should contain only boundary points around the
// tag. it cannot be bigger than the whole screen. (Reject
// large connected blobs that will be prohibitively slow to
// fit quads to.)
if (zarray_size(cluster) > 4*(w+h)) {
continue;
}
struct quad quad;
memset(&quad, 0, sizeof(struct quad));
if (fit_quad(td, task->im, cluster, &quad)) {
pthread_mutex_lock(&td->mutex);
zarray_add(quads, &quad);
pthread_mutex_unlock(&td->mutex);
}
}
}
// Do a quick swap:
void vx_buffer_swap(vx_buffer_t * buffer)
{
pthread_mutex_lock(&buffer->mutex);
{
// It's possible that the serialization is running behind,
// and may not have already serialized the pending_objs
// In that case, they are discarded here, momentarily blocking
// the calling thread.
if (zarray_size(buffer->pending_objs) > 0) {
// *+*+ corresponding decrement (if falling behind)
zarray_vmap(buffer->pending_objs, vx_object_dec_destroy);
zarray_clear(buffer->pending_objs);
static int once = 1;
if (once) {
once = 0;
printf("NFO: World serialization fell behind\n");
}
}
// swap
zarray_t * tmp = buffer->pending_objs;
buffer->pending_objs = buffer->back_objs;
buffer->back_objs = tmp;
}
pthread_mutex_unlock(&buffer->mutex);
// Now flag a swap on the serialization thread
pthread_mutex_lock(&buffer->world->queue_mutex);
{
// Ensure that the string is already in the queue, or add it if
// it isn't. Duplicates are prohibited
int index = -1;
for (int i = 0, sz = zarray_size(buffer->world->buffer_queue); i < sz; i++) {
char * test = NULL;
zarray_get(buffer->world->buffer_queue, i, &test);
if (strcmp(test, buffer->name) == 0) {
index = i;
break;
}
}
if (index < 0) {
zarray_add(buffer->world->buffer_queue, &buffer->name);
pthread_cond_signal(&buffer->world->queue_cond);
}
}
pthread_mutex_unlock(&buffer->world->queue_mutex);
}
void
getopt_do_usage (getopt_t *gopt)
{
int leftmargin=2;
int longwidth=12;
int valuewidth=10;
for (unsigned int i = 0; i < zarray_size (gopt->options); i++) {
getopt_option_t *goo = NULL;
zarray_get (gopt->options, i, &goo);
if (goo->spacer)
continue;
longwidth = max (longwidth, strlen(goo->lname));
if (goo->type == GOO_STRING_TYPE)
valuewidth = max (valuewidth, strlen(goo->svalue));
}
for (unsigned int i = 0; i < zarray_size (gopt->options); i++) {
getopt_option_t *goo = NULL;
zarray_get (gopt->options, i, &goo);
if (goo->spacer) {
if (goo->help==NULL || 0==strlen (goo->help))
printf ("\n");
else
printf ("\n%*s%s\n\n", leftmargin, "", goo->help);
continue;
}
printf ("%*s", leftmargin, "");
if (goo->sname[0]==0)
printf (" ");
else
printf ("-%c | ", goo->sname[0]);
printf ("--%*s ", -longwidth, goo->lname);
printf (" [ %s ]", goo->svalue); // XXX: displays current value rather than default value
printf ("%*s", (int) (valuewidth-strlen (goo->svalue)), "");
printf (" %s ", goo->help);
printf ("\n");
}
}
void *worker_thread(void *p)
{
workerpool_t *wp = (workerpool_t*) p;
int cnt = 0;
while (1) {
struct task *task;
pthread_mutex_lock(&wp->mutex);
while (wp->taskspos == zarray_size(wp->tasks)) {
wp->end_count++;
// printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt);
pthread_cond_broadcast(&wp->endcond);
pthread_cond_wait(&wp->startcond, &wp->mutex);
cnt = 0;
// printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self());
}
zarray_get_volatile(wp->tasks, wp->taskspos, &task);
wp->taskspos++;
cnt++;
pthread_mutex_unlock(&wp->mutex);
// pthread_yield();
sched_yield();
// we've been asked to exit.
if (task->f == NULL)
return NULL;
task->f(task->p);
}
return NULL;
}
void vx_world_destroy(vx_world_t * world)
{
zhash_vmap_values(world->buffer_map, vx_world_buffer_destroy); // keys are stored in buffer struct
zhash_destroy(world->buffer_map);
assert(zarray_size(world->listeners) == 0 && "Destroy layers referencing worlds before worlds"); // we can't release these resources properly
zarray_destroy(world->listeners);
pthread_mutex_destroy(&world->buffer_mutex);
pthread_mutex_destroy(&world->listener_mutex);
// Tell the processing thread to quit
world->process_running = 0;
pthread_mutex_lock(&world->queue_mutex);
pthread_cond_signal(&world->queue_cond);
pthread_mutex_unlock(&world->queue_mutex);
pthread_join(world->process_thread, NULL);
pthread_mutex_destroy(&world->queue_mutex);
pthread_cond_destroy(&world->queue_cond);
// These are pointers to data stored elsewhere, just delete the
// data structure
zarray_destroy(world->listener_queue);
zarray_destroy(world->buffer_queue);
free(world);
}
int vx_gtk_display_dispatch_key(vx_display_t * disp, vx_key_event_t *event)
{
assert(disp->impl_type == VX_GTK_DISPLAY_IMPL);
state_t * state = disp->impl;
// Process key shortcuts for the display
pthread_mutex_lock(&state->mutex);
int mouse_pressed_layer_id = state->mouse_pressed_layer_id; //local copy
// Also dispatch to the relevant layers camera manager
{
layer_info_t * linfo = NULL;
zhash_get(state->layer_info_map, &mouse_pressed_layer_id, &linfo);
if (linfo != NULL) // sometimes there could be no layers
linfo->event_handler->key_event(linfo->event_handler, NULL, event);
}
pthread_mutex_unlock(&state->mutex);
pthread_mutex_lock(&state->listener_mutex);
// notify all listeners of this event
for (int i = 0; i < zarray_size(state->listeners); i++) {
vx_display_listener_t * l = NULL;
zarray_get(state->listeners, i, &l);
l->event_dispatch_key(l, state->mouse_pressed_layer_id, event);
}
pthread_mutex_unlock(&state->listener_mutex);
return 1; // gobble all events
}
int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x)
{
int sz = zarray_size(poly);
g2d_line_t line;
if (1) {
double p0[2] = { 0, y };
double p1[2] = { 1, y };
g2d_line_init_from_points(&line, p0, p1);
}
int xpos = 0;
for (int i = 0; i < sz; i++) {
g2d_line_segment_t seg;
double *p0, *p1;
zarray_get_volatile(poly, i, &p0);
zarray_get_volatile(poly, (i+1)%sz, &p1);
g2d_line_segment_init_from_points(&seg, p0, p1);
double q[2];
if (g2d_line_segment_intersect_line(&seg, &line, q))
x[xpos++] = q[0];
}
qsort(x, xpos, sizeof(double), double_sort_up);
return xpos;
}
int g2d_polygon_contains_point_ref(const zarray_t *poly, double q[2])
{
// use winding. If the point is inside the polygon, we'll wrap
// around it (accumulating 6.28 radians). If we're outside the
// polygon, we'll accumulate zero.
int psz = zarray_size(poly);
double acc_theta = 0;
double last_theta;
for (int i = 0; i <= psz; i++) {
double p[2];
zarray_get(poly, i % psz, &p);
double this_theta = atan2(q[1]-p[1], q[0]-p[0]);
if (i != 0)
acc_theta += mod2pi(this_theta - last_theta);
last_theta = this_theta;
}
return acc_theta > M_PI;
}
CameraHandler() {
zarray_t* urls = image_source_enumerate();
bool gotCamera = false;
for (int i = 0; i < zarray_size(urls); ++i) {
char* url;
zarray_get(urls, i, &url);
_isrc = image_source_open(url);
if (_isrc != NULL) {
printf("connected to camera %s\n", url);
gotCamera = true;
free(url);
break;
}
}
zarray_destroy(urls);
if (!gotCamera) {
printf("couldn't find a camera\n");
exit(1);
}
if (pthread_mutex_init(&_dataMutex, NULL)) {
printf("dataMutex not initialized\n");
exit(1);
}
_im = nullptr;
_running = false;
_staticImage = false;
_isrc->start(_isrc);
}
void reset_locs(zarray_t* locs)
{
for(int i = 0; i < zarray_size(locs); i++) {
loc_t* tmp;
zarray_get(locs, i, &tmp);
tmp->valid = 0;
}
}
static void notify_listeners_send_resources(vx_world_t * world, zhash_t * new_resources)
{
pthread_mutex_lock(&world->listener_mutex);
for (int i = 0; i < zarray_size(world->listeners); i++) {
vx_world_listener_t * listener = NULL;
zarray_get(world->listeners, i, &listener);
listener->send_resources(listener, new_resources);
}
pthread_mutex_unlock(&world->listener_mutex);
}
void workerpool_run_single(workerpool_t *wp)
{
for (int i = 0; i < zarray_size(wp->tasks); i++) {
struct task *task;
zarray_get_volatile(wp->tasks, i, &task);
task->f(task->p);
}
zarray_clear(wp->tasks);
}
static void notify_listeners_codes(vx_world_t * world, vx_code_output_stream_t * couts)
{
pthread_mutex_lock(&world->listener_mutex);
for (int i = 0; i < zarray_size(world->listeners); i++) {
vx_world_listener_t * listener = NULL;
zarray_get(world->listeners, i, &listener);
listener->send_codes(listener, couts->data, couts->pos);
}
pthread_mutex_unlock(&world->listener_mutex);
}
void add_arr_of_locs_to_buffer(zarray_t* arr, vx_buffer_t* buf, double size,
const float* color, metrics_t met)
{
for(int i = 0; i < zarray_size(arr); i++)
{
loc_t* tmp;
zarray_get(arr, i, &tmp);
add_circle_to_buffer(buf, size, *tmp, color);
}
}
// returns the layer under the specified x,y. Must be externally synchronized
static int pick_layer(render_info_t * rinfo, float x, float y)
{
int layer_under_mouse_id = 0;
for (int i = 0; i < zarray_size(rinfo->layers); i++) {
layer_info_t * linfo = NULL;
zarray_get(rinfo->layers, i, &linfo);
int * viewport = NULL;
zhash_get(rinfo->layer_positions, &linfo->layer_id, &viewport);
if (check_viewport_bounds(x, y, viewport))
layer_under_mouse_id = linfo->layer_id; // highest ranked layer that matches will be it
}
return layer_under_mouse_id;
}
std::vector<TagMatch> detectTags(image_u8_t *im) {
const int hamm_hist_max = 10;
int hamm_hist[hamm_hist_max];
memset(hamm_hist, 0, sizeof(hamm_hist));
zarray_t *detections = apriltag_detector_detect(td, im);
std::vector<TagMatch> tag_matches;
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);
// image_u8_draw_line(im, det->p[x][0], det->p[x][1], det->p[x+1][0], det->p[x+1][1], 255, 10);
}
if (tag_id == -1 || det->id == tag_id) {
TagMatch tag_match;
tag_match.id = det->family->d*det->family->d;
tag_match.p0 = cv::Point2d(det->p[0][0], det->p[0][1]);
tag_match.p1 = cv::Point2d(det->p[1][0], det->p[1][1]);
tag_match.p2 = cv::Point2d(det->p[2][0], det->p[2][1]);
tag_match.p3 = cv::Point2d(det->p[3][0], det->p[3][1]);
Eigen::Map<Eigen::Matrix3d> H_map(det->H->data);
tag_match.H = H_map.transpose();
tag_matches.push_back(tag_match);
}
hamm_hist[det->hamming]++;
}
apriltag_detections_destroy(detections);
if (!quiet) {
timeprofile_display(td->tp);
printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
printf("Hamming histogram: ");
for (int i = 0; i < hamm_hist_max; i++)
printf("%5d", hamm_hist[i]);
printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
printf("\n");
}
return tag_matches;
}
int main(int argc, char **argv){
printf("one\n");
apriltag_family_t *tf = tag36h11_create();
printf("two\n");
apriltag_detector_t *td = apriltag_detector_create();
printf("three\n");
apriltag_detector_add_family(td,tf);
printf("four\n");
image_u8_t* img = image_u8_create_from_pnm("../WillTagged.pnm");
printf("five\n");
zarray_t* detections = apriltag_detector_detect(td,img);
printf("%i",zarray_size(detections));
printf("\n");
}
// returns null def if no parameter specified.
const char *
url_parser_get_parameter (url_parser_t *urlp, const char *key, const char *def)
{
for (int i = 0; i < zarray_size(urlp->keys); i++) {
char *thiskey;
zarray_get(urlp->keys, i, &thiskey);
if (!strcmp(thiskey, key)) {
char *val;
zarray_get(urlp->vals, i, &val);
return val;
}
}
return def;
}
line_t find_line_endpoints(zarray_t* loc_arr, loc_t* p1, loc_t* p2)
{
double largest_dist = INT_MIN;
loc_t largest_loc = {-1, -1};
double smallest_dist = INT_MAX;
loc_t smallest_loc = {-1, -1};
double slope = get_slope(*p1, *p2, 0, 0, NULL);
// find point on line to compare every node to, simple solution = set x = im->width/2
loc_t* Q = p1;
// get line unit vector, y=mx+b --> vector = (1,m)
double mag = sqrt(1 + slope*slope);
vec_t u = {1/mag, slope/mag};
// assert(sqrt((u.x*u.x) + (u.y*u.y)) == 1); // length of u should be 1
// printf("\nunit vec:(%lf(%lf), %lf) Point(%d, %d)\n", u.x, 1/mag, u.y, Q.x, Q.y);
for(int i = 0; i < zarray_size(loc_arr); i++)
{
loc_t* tmp;
zarray_get(loc_arr, i, &tmp);
// find the projection of point onto line
vec_t QP = {Q->x - tmp->x, Q->y - tmp->y};
double dist = u.x * QP.x + u.y * QP.y;
// printf("dist:%lf small:%lf larg:%lf QP:(%lf, %lf) loc:(%d, %d) \n",
// dist, smallest_dist, largest_dist, QP.x, QP.y, node->loc.x, node->loc.y);
if(dist < smallest_dist) {
smallest_dist = dist;
smallest_loc.x = tmp->x;
smallest_loc.y = tmp->y;
}
if(dist > largest_dist) {
largest_dist = dist;
largest_loc.x = tmp->x;
largest_loc.y = tmp->y;
}
}
line_t ret = { .end = largest_loc,
.start = smallest_loc,
.nodes = NULL };
return(ret);
}
// XXX make static
render_info_t * render_info_copy (render_info_t * rinfo)
{
assert(rinfo != NULL);
render_info_t * rinfo_copy = render_info_create();
// copy a list of ints (ordered)
for (int i = 0; i < zarray_size(rinfo->layers); i++) {
layer_info_t * layer_info = NULL;
zarray_get(rinfo->layers, i, &layer_info);
zarray_add(rinfo_copy->layers, &layer_info);
}
// copy all the camera positions
{
zhash_iterator_t vit;
uint32_t layer_id;
vx_camera_pos_t *pos;
zhash_iterator_init (rinfo->camera_positions, &vit);
while (zhash_iterator_next (&vit, &layer_id, &pos)) {
vx_camera_pos_t *pos_copy = malloc (sizeof(*pos_copy));
memcpy(pos_copy, pos, sizeof(vx_camera_pos_t));
zhash_put (rinfo_copy->camera_positions, &layer_id, &pos_copy, NULL, NULL);
}
}
// copy each individual layer viewports
{
zhash_iterator_t itr;
uint32_t layer_id;
int *viewport;
zhash_iterator_init (rinfo->layer_positions, &itr);
while (zhash_iterator_next (&itr, &layer_id, &viewport)) {
int *viewport_copy = malloc (4*sizeof(int));
memcpy(viewport_copy, viewport, 4*sizeof(int));
zhash_put (rinfo_copy->layer_positions, &layer_id, &viewport_copy, NULL, NULL);
}
}
memcpy(rinfo_copy->viewport, rinfo->viewport, 4*sizeof(int));
return rinfo_copy;
}
static void print_bounds(zarray_t * vertices)
{
// Approximate centroid: Just average all vertices, regardless of
// the frequency they are referenced by an index
float minMax[3][2] = {{FLT_MAX,-FLT_MAX},{FLT_MAX,-FLT_MAX},{FLT_MAX,-FLT_MAX}};
for (int i = 0; i < zarray_size(vertices); i++) {
float pt[3];
zarray_get(vertices, i, &pt);
// iterate over xyz coords
for (int j = 0; j < 3; j++) {
minMax[j][0] = fminf(pt[j], minMax[j][0]);
minMax[j][1] = fmaxf(pt[j], minMax[j][1]);
}
}
printf(" Object bounds: xyz [%.2f, %.2f][%.2f, %.2f][%.2f, %.2f]\n",
minMax[0][0],minMax[0][1],
minMax[1][0],minMax[1][1],
minMax[2][0],minMax[2][1]);
}
void g2d_polygon_make_ccw(zarray_t *poly)
{
// Step one: we want the points in counter-clockwise order.
// If the points are in clockwise order, we'll reverse them.
double total_theta = 0;
double last_theta = 0;
// Count the angle accumulated going around the polygon. If
// the sum is +2pi, it's CCW. Otherwise, we'll get -2pi.
int sz = zarray_size(poly);
for (int i = 0; i <= sz; i++) {
double p0[2], p1[2];
zarray_get(poly, i % sz, &p0);
zarray_get(poly, (i+1) % sz, &p1);
double this_theta = atan2(p1[1]-p0[1], p1[0]-p0[0]);
if (i > 0) {
double dtheta = mod2pi(this_theta-last_theta);
total_theta += dtheta;
}
last_theta = this_theta;
}
int ccw = (total_theta > 0);
// reverse order if necessary.
if (!ccw) {
for (int i = 0; i < sz / 2; i++) {
double a[2], b[2];
zarray_get(poly, i, a);
zarray_get(poly, sz-1-i, b);
zarray_set(poly, i, b, NULL);
zarray_set(poly, sz-1-i, a, NULL);
}
}
}
// Find point p on the boundary of poly that is closest to q.
void g2d_polygon_closest_boundary_point(const zarray_t *poly, const double q[2], double *p)
{
int psz = zarray_size(poly);
double min_dist = HUGE;
for (int i = 0; i < psz; i++) {
double *p0, *p1;
zarray_get_volatile(poly, i, &p0);
zarray_get_volatile(poly, (i+1) % psz, &p1);
g2d_line_segment_t seg;
g2d_line_segment_init_from_points(&seg, p0, p1);
double thisp[2];
g2d_line_segment_closest_point(&seg, q, thisp);
double dist = g2d_distance(q, thisp);
if (dist < min_dist) {
memcpy(p, thisp, sizeof(double[2]));
min_dist = dist;
}
}
}
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(){
bool showGradient = true;
bool found = false;
VideoCapture cap(0); // open the default camera
Size size(854,480); // size of desired frame origionally 1280x720, 1024x576, 854x480
if(!cap.isOpened()) // check if camera opened
return -1;
Mat frame;
Mat src;
/* From apriltag_demo.c */
int maxiters = 5;
const int hamm_hist_max = 10;
int quiet = 0;
apriltag_family_t *tf = tag36h11_create(); // Apriltag family 36h11, can change
tf->black_border = 1; // Set tag family border size
apriltag_detector_t *td = apriltag_detector_create(); // Apriltag detector
apriltag_detector_add_family(td, tf); // Add apriltag family
td->quad_decimate = 1.0; // Decimate input image by factor
td->quad_sigma = 0.0; // No blur (I think)
td->nthreads = 4; // 4 treads provided
td->debug = 0; // No debuging output
td->refine_decode = 0; // Don't refine decode
td->refine_pose = 0; // Don't refine pose
// Output variables
char imgSize[20];
char renderTime[20];
char detectString[50];
char convertTime[50];
char displayString[120];
char outputString[120];
char locationString[120];
double time_taken = 0.0;
long double totalFPS = 0.0;
double count = 0.0;
/* End of apriltag_demo.c */
while(1){
clock_t t;
t = clock();
cap >> src; // Get a new frame from camera
if(found){ resize(src,frame,size); } // Resize to smaller image if tag found
else{ frame = src; } // Keep standard image if no tag
if(showGradient){
cvtColor(src, frame, CV_BGR2GRAY);
cvtColor(frame, frame, CV_GRAY2RGB);
src = gradientEdges(frame); // Show gradient for fun
}else{
cvtColor(src, src, CV_BGR2GRAY);
}
pnm_t *pnm = mat2pnm(&frame); // Convert Mat fram to pnm
image_u8_t *im = pnm_to_image_u8(pnm); // Convert pnm to gray image_u8
if (im == NULL) { // Error - no image created from pnm
std::cout << "Error, not a proper pnm" << std::endl;
return -1;
}
/*** Start from origional Apriltags from apriltag_demo.c ***/
int hamm_hist[hamm_hist_max];
memset(hamm_hist, 0, sizeof(hamm_hist));
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);
sprintf(locationString, "Tag Center: (%f,%f)", det->c[0], det->c[1]);
sprintf(detectString, "detection %2d: id (%2dx%2d)-%-4d, hamming %d, goodness %5.3f, margin %5.3f\n",
i+1, det->family->d*det->family->d, det->family->h, det->id, det->hamming, det->goodness, det->decision_margin);
hamm_hist[det->hamming]++;
// draws a vertical rectangle around tag, not ideal, but easy to implement
// det->p[corner][positon], counter clockwise
Point pt1 = Point(det->p[0][0], det->p[0][1]);
Point pt2 = Point(det->p[2][0], det->p[2][1]);
cv::rectangle(src, pt1, pt2, cvScalar(102,255,0));
apriltag_detection_destroy(det);
}
if(zarray_size(detections) < 1){
found = false;
sprintf(detectString, "No tag detected");
sprintf(locationString, "No tag detected");
}else{
found = false;
}
zarray_destroy(detections);
image_u8_destroy(im);
t = clock() - t;
double time_taken = ((double)t)/(CLOCKS_PER_SEC/1000);
//printf("ms to render: %5.3f\n", time_taken);
if (!quiet) {
//timeprofile_display(td->tp);
totalFPS += (1000.0/time_taken);
count += 1.0;
if(count > 30000.0){
totalFPS = 0.0;
count = 0.0;
}
sprintf(displayString, "fps: %2.2Lf, nquads: %d",totalFPS/count, td->nquads);
//std::cout << displayString;
}
//for (int i = 0; i < hamm_hist_max; i++)
//printf("%5d", hamm_hist[i]);
sprintf(renderTime, "Render: %5.3fms", time_taken);
sprintf(imgSize, "%dx%d", frame.cols, frame.rows);
sprintf(outputString, "%s %s %s", renderTime, convertTime, imgSize);
printf("%s %s\r", detectString, outputString);
if (quiet) {
printf("%12.3f", timeprofile_total_utime(td->tp) / 1.0E3);
}
printf("\n");
/*** End of origional Apriltags from apriltag_demo.c ***/
// displays fps, edges, segments, quads
putText(src, displayString, cvPoint(30,30),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
// displays render time, convert time, and image size
putText(src, outputString, cvPoint(30,50),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
// Displays any detections (if any)
putText(src, detectString, cvPoint(30,70),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
// Displays tag location (if any)
putText(src, locationString, cvPoint(30,90),
FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(150,150,250), 1, CV_AA);
imshow("Display Apriltags", src);
if(waitKey(30) >= 0) break;
}
/* deallocate apriltag constructs */
apriltag_detector_destroy(td);
tag36h11_destroy(tf);
return 0;
}
int g2d_polygon_contains_point(const zarray_t *poly, double q[2])
{
// use winding. If the point is inside the polygon, we'll wrap
// around it (accumulating 6.28 radians). If we're outside the
// polygon, we'll accumulate zero.
int psz = zarray_size(poly);
int last_quadrant;
int quad_acc = 0;
for (int i = 0; i <= psz; i++) {
double *p;
zarray_get_volatile(poly, i % psz, &p);
// p[0] < q[0] p[1] < q[1] quadrant
// 0 0 0
// 0 1 3
// 1 0 1
// 1 1 2
// p[1] < q[1] p[0] < q[0] quadrant
// 0 0 0
// 0 1 1
// 1 0 3
// 1 1 2
int quadrant;
if (p[0] < q[0])
quadrant = (p[1] < q[1]) ? 2 : 1;
else
quadrant = (p[1] < q[1]) ? 3 : 0;
if (i > 0) {
int dquadrant = quadrant - last_quadrant;
// encourage a jump table by mapping to small positive integers.
switch (dquadrant) {
case -3:
case 1:
quad_acc ++;
break;
case -1:
case 3:
quad_acc --;
break;
case 0:
break;
case -2:
case 2:
{
// get the previous point.
double *p0;
zarray_get_volatile(poly, i-1, &p0);
// Consider the points p0 and p (the points around the
//polygon that we are tracing) and the query point q.
//
// If we've moved diagonally across quadrants, we want
// to measure whether we have rotated +PI radians or
// -PI radians. We can test this by computing the dot
// product of vector (p0-q) with the vector
// perpendicular to vector (p-q)
double nx = p[1] - q[1];
double ny = -p[0] + q[0];
double dot = nx*(p0[0]-q[0]) + ny*(p0[1]-q[1]);
if (dot < 0)
quad_acc -= 2;
else
quad_acc += 2;
break;
}
}
}
last_quadrant = quadrant;
}
int v = (quad_acc >= 2) || (quad_acc <= -2);
if (0 && v != g2d_polygon_contains_point_ref(poly, q)) {
printf("FAILURE %d %d\n", v, quad_acc);
exit(-1);
}
return v;
}
// creates and returns a zarray(double[2]). The resulting polygon is
// CCW and implicitly closed. Unnecessary colinear points are omitted.
zarray_t *g2d_convex_hull(const zarray_t *points)
{
zarray_t *hull = zarray_create(sizeof(double[2]));
// gift-wrap algorithm.
// step 1: find left most point.
int insz = zarray_size(points);
double *pleft = NULL;
for (int i = 0; i < insz; i++) {
double *p;
zarray_get_volatile(points, i, &p);
if (pleft == NULL || p[0] < pleft[0])
pleft = p;
}
zarray_add(hull, pleft);
// step 2. gift wrap. Keep searching for points that make the
// smallest-angle left-hand turn. This implementation is carefully
// written to use only addition/subtraction/multiply. No division
// or sqrts. This guarantees exact results for integer-coordinate
// polygons (no rounding/precision problems).
double *p = pleft;
while (1) {
double *q = NULL;
double n0 = 0, n1 = 0; // the normal to the line (p, q) (not
// necessarily unit length).
// Search for the point q for which the line (p,q) is most "to
// the right of" the other points. (i.e., every time we find a
// point that is to the right of our current line, we change
// lines.)
for (int i = 0; i < insz; i++) {
double *thisq;
zarray_get_volatile(points, i, &thisq);
if (thisq == p)
continue;
if (q == NULL) {
q = thisq;
n0 = q[1] - p[1];
n1 = -q[0] + p[0];
} else {
// is point thisq RIGHT OF line (p, q)?
double e0 = thisq[0] - p[0], e1 = thisq[1] - p[1];
double dot = e0*n0 + e1*n1;
if (dot > 0) {
q = thisq;
n0 = q[1] - p[1];
n1 = -q[0] + p[0];
}
}
}
// loop completed?
if (q == pleft)
break;
int colinear = 0;
// is this new point colinear with the last two?
if (zarray_size(hull) > 1) {
double *o;
zarray_get_volatile(hull, zarray_size(hull) - 2, &o);
double e0 = o[0] - p[0];
double e1 = o[1] - p[1];
if (n0*e0 + n1*e1 == 0)
colinear = 1;
}
// if it is colinear, overwrite the last one.
if (colinear)
zarray_set(hull, zarray_size(hull)-1, q, NULL);
else
zarray_add(hull, q);
p = q;
}
return hull;
}
// returns 1 if no error
int
getopt_parse (getopt_t *gopt, int argc, char *argv[], int showErrors)
{
int okay = 1;
zarray_t *toks = zarray_create (sizeof(char*));
// take the input stream and chop it up into tokens
for (int i = 1; i < argc; i++) {
char *arg = strdup (argv[i]);
if (arg[0] != '-') {
// if this isn't an option, put the whole thing in the args list.
zarray_add (toks, &arg);
}
else {
// this is an option. It could be a flag (like -v), or an option
// with arguments (--file=foobar.txt).
char *eq = strstr (arg, "=");
// no equal sign? Push the whole thing.
if (eq == NULL) {
zarray_add (toks, &arg);
}
else {
// there was an equal sign. Push the part
// before and after the equal sign
char *val = strdup (&eq[1]);
eq[0] = 0;
zarray_add (toks, &arg);
// if the part after the equal sign is
// enclosed by quotation marks, strip them.
if (val[0]=='\"') {
int last = strlen (val) - 1;
if (val[last]=='\"')
val[last] = 0;
char *valclean = strdup (&val[1]);
zarray_add (toks, &valclean);
free (val);
}
else
zarray_add (toks, &val);
}
}
}
// now loop over the elements and evaluate the arguments
unsigned int i = 0;
char *tok = NULL;
while (i < zarray_size (toks)) {
// rather than free statement throughout this while loop
if (tok != NULL)
free (tok);
zarray_get (toks, i, &tok);
if (0==strncmp (tok,"--", 2)) {
char *optname = &tok[2];
getopt_option_t *goo = NULL;
zhash_get (gopt->lopts, &optname, &goo);
if (goo == NULL) {
okay = 0;
if (showErrors)
printf ("Unknown option --%s\n", optname);
i++;
continue;
}
goo->was_specified = 1;
if (goo->type == GOO_BOOL_TYPE) {
if ((i+1) < zarray_size (toks)) {
char *val = NULL;
zarray_get (toks, i+1, &val);
if (0==strcmp (val,"true")) {
i+=2;
getopt_modify_string (&goo->svalue, val);
continue;
}
if (0==strcmp (val,"false")) {
i+=2;
getopt_modify_string (&goo->svalue, val);
continue;
}
}
getopt_modify_string (&goo->svalue, strdup("true"));
i++;
continue;
}
if (goo->type == GOO_STRING_TYPE) {
// TODO: check whether next argument is an option, denoting missing argument
if ((i+1) < zarray_size (toks)) {
char *val = NULL;
zarray_get (toks, i+1, &val);
i+=2;
getopt_modify_string (&goo->svalue, val);
continue;
}
okay = 0;
if (showErrors)
printf ("Option %s requires a string argument.\n",optname);
}
}
if (0==strncmp (tok,"-",1) && strncmp (tok,"--",2)) {
int len = strlen (tok);
int pos;
for (pos = 1; pos < len; pos++) {
char sopt[2];
sopt[0] = tok[pos];
sopt[1] = 0;
char *sopt_ptr = (char*) &sopt;
getopt_option_t *goo = NULL;
zhash_get (gopt->sopts, &sopt_ptr, &goo);
if (goo==NULL) {
// is the argument a numerical literal that happens to be negative?
if (pos==1 && isdigit (tok[pos])) {
zarray_add (gopt->extraargs, &tok);
tok = NULL;
break;
}
else {
okay = 0;
if (showErrors)
printf ("Unknown option -%c\n", tok[pos]);
i++;
continue;
}
}
goo->was_specified = 1;
if (goo->type == GOO_BOOL_TYPE) {
getopt_modify_string (&goo->svalue, strdup("true"));
continue;
}
if (goo->type == GOO_STRING_TYPE) {
if ((i+1) < zarray_size (toks)) {
char *val = NULL;
zarray_get (toks, i+1, &val);
// TODO: allow negative numerical values for short-name options ?
if (val[0]=='-')
{
okay = 0;
if (showErrors)
printf ("Ran out of arguments for option block %s\n", tok);
}
i++;
getopt_modify_string (&goo->svalue, val);
continue;
}
okay = 0;
if (showErrors)
printf ("Option -%c requires a string argument.\n", tok[pos]);
}
}
i++;
continue;
}
// it's not an option-- it's an argument.
zarray_add (gopt->extraargs, &tok);
tok = NULL;
i++;
}
if (tok != NULL)
free (tok);
zarray_destroy (toks);
return okay;
}
int main(int argc, char *argv[])
{
april_tag_family_t *tf = tag36h11_create();
april_tag_detector_t *td = april_tag_detector_create(tf);
td->small_tag_refinement = 0;
int maxiters = 1;
zarray_t *inputs = zarray_create(sizeof(char*));
int waitsec = 0;
for (int i = 1; i < argc; i++) {
if (!strcmp(argv[i], "-d"))
td->debug = 1;
else if (!strcmp(argv[i], "-t"))
td->nthreads = atoi(argv[++i]);
else if (!strcmp(argv[i], "-f"))
td->seg_decimate = (i+1 < argc && isdigit(argv[i+1][0])) ? atoi(argv[++i]) : 2;
else if (!strcmp(argv[i], "-i"))
maxiters = atoi(argv[++i]);
else if (!strcmp(argv[i], "-r"))
td->small_tag_refinement = 1;
else if (!strcmp(argv[i], "-w"))
waitsec = atoi(argv[++i]);
else if (!strcmp(argv[i], "-b"))
td->seg_sigma = atof(argv[++i]);
/* else if (!strcmp(argv[i], "--family")) {
char *fam = argv[++i];
if (!strcmp(fam, "36h11"))
td->tag_family = tag36h11_create();
else if (!strcmp(fam, "36h10"))
td->tag_family = tag36h10_create();
} */
else
zarray_add(inputs, &argv[i]);
}
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++) {
char *path;
zarray_get(inputs, input, &path);
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 = april_tag_detector_detect(td, im);
for (int i = 0; i < zarray_size(detections); i++) {
april_tag_detection_t *det;
zarray_get(detections, i, &det);
printf("detection %3d: id %4d, hamming %d, goodness %f\n", i, det->id, det->hamming, det->goodness);
april_tag_detection_destroy(det);
}
zarray_destroy(detections);
timeprofile_display(td->tp);
printf("nedges: %d, nsegments: %d, nquads: %d\n", td->nedges, td->nsegments, td->nquads);
image_u8_destroy(im);
if (zarray_size(inputs) > 1 || iter > 0)
sleep(waitsec);
}
}
april_tag_detector_destroy(td);
tag36h11_destroy(tf);
return 0;
}
