// 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;
}
// compute a point that is inside the polygon. (It may not be *far* inside though)
void g2d_polygon_get_interior_point(const zarray_t *poly, double *p)
{
// take the first three points, which form a triangle. Find the middle point
double a[2], b[2], c[2];
zarray_get(poly, 0, a);
zarray_get(poly, 1, b);
zarray_get(poly, 2, c);
p[0] = (a[0]+b[0]+c[0])/3;
p[1] = (a[1]+b[1]+c[1])/3;
}
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");
}
}
// 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;
}
const char *
url_parser_get_parameter_value (url_parser_t *urlp, int idx)
{
char *v;
zarray_get(urlp->vals, idx, &v);
return v;
}
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);
}
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
}
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);
}
}
}
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;
}
}
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);
}
}
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);
}
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 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);
}
}
}
// does polya completely contain polyb?
int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb)
{
// do any of the line segments collide? If so, the answer is no.
if (g2d_polygon_intersects_polygon(polya, polyb))
return 0;
// if none of the edges cross, then the polygon is either fully
// contained or fully outside.
double p[2];
zarray_get(polyb, 0, p);
return g2d_polygon_contains_point(polya, p);
}
// 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;
}
// 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 project_measurements_through_homography(matd_t* H, vx_buffer_t* buf,
zarray_t* pix_found, int size)
{
int npoints = NUM_CHART_BLOBS * 2; // line per chart blob
float points[npoints*3];
float* real_world_coords;
if(size == NUM_TARGETS) real_world_coords = target_coords;
else if(size == NUM_CHART_BLOBS) real_world_coords = chart_coords;
else assert(0);
for(int i = 0; i < size; i++) {
// run each real world point through homography and add to buf
double tmp[3] = {real_world_coords[i*2], real_world_coords[i*2+1], 1};
matd_t* xy_matrix = matd_create_data(3,1,tmp);
matd_t* pix_estimated = matd_op("(M)*M",H, xy_matrix);
MATD_EL(pix_estimated,0,0) /= MATD_EL(pix_estimated,2, 0);
MATD_EL(pix_estimated,1,0) /= MATD_EL(pix_estimated,2, 0);
vx_buffer_add_back(buf,
vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT,
vxo_chain(vxo_mat_translate3(MATD_EL(pix_estimated,0,0), MATD_EL(pix_estimated,1,0), 0),
vxo_mat_scale(2.0),
vxo_circle(vxo_mesh_style(vx_green)))));
// create endpoints for lines
loc_t pos;
zarray_get(pix_found, i, &pos); //
points[6*i + 0] = pos.x;
points[6*i + 1] = pos.y;
points[6*i + 2] = 0;
points[6*i + 3] = MATD_EL(pix_estimated,0,0);
points[6*i + 4] = MATD_EL(pix_estimated,1,0);
points[6*i + 5] = 0;
}
// make lines
vx_resc_t *verts = vx_resc_copyf(points, npoints*3);
vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT,
vxo_lines(verts, npoints, GL_LINES,
vxo_points_style(vx_blue, 2.0f))));
}
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]);
}
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;
}
static void * movie_run(void * params)
{
state_t * state = params;
zarray_t * frames = zarray_create(sizeof(movie_frame_t*));
char * last_filename = NULL;
char * current_filename = NULL;
gzFile gzMovie = NULL;
int movie_frames = 0;
int lost_frames = 0;
uint64_t movie_start_mtime = 0;
while (state->rendering) {
pthread_mutex_lock(&state->movie_mutex);
while (state->rendering && current_filename == state->movie_file && zarray_size(state->movie_pending) == 0)
pthread_cond_wait(&state->movie_cond, &state->movie_mutex);
if (!state->rendering) // exit signaled while waiting on cond
break;
for (int i = 0; i < zarray_size(state->movie_pending); i++) {
movie_frame_t * tmp = NULL;
zarray_get(state->movie_pending, i, &tmp);
zarray_add(frames, &tmp);
}
zarray_clear(state->movie_pending);
current_filename = state->movie_file;
pthread_mutex_unlock(&state->movie_mutex);
// Depending on setup, execute different actions
// 1) Open 2) Close 3) Write a frame
if (zarray_size(frames) > 0 && current_filename != NULL) {
// Write the most recent frame, dump the rest
movie_frame_t * movie_img = NULL;
zarray_get(frames, zarray_size(frames)-1, &movie_img);
zarray_remove_index(frames, zarray_size(frames)-1, 0);
char header[256];
sprintf(header, "#mtime=%"PRIu64"\nP6 %d %d %d\n", movie_img->mtime, movie_img->width, movie_img->height, 255);
int header_len = strlen(header);
int imglen = movie_img->stride*movie_img->height;// RGB format
gzwrite(gzMovie, header, header_len);
gzwrite(gzMovie, movie_img->buf, imglen);
int res = gzflush(gzMovie, Z_SYNC_FLUSH);
if (res != Z_OK) {
int errval = 0;
printf("Error writing movie %s. Closing file\n", gzerror(gzMovie, &errval));
gzclose(gzMovie);
free(current_filename);
current_filename = NULL;
}
movie_frames++;
movie_frame_destroy (movie_img);
}
// cleanup the frames:
lost_frames += zarray_size(frames);
zarray_vmap(frames, movie_frame_destroy);
zarray_clear(frames);
// Got a new filename, need to start recording
if (current_filename != NULL && last_filename == NULL)
{
gzMovie = gzopen(current_filename, "w3");
movie_frames = 0;
lost_frames = 0;
movie_start_mtime = vx_mtime();
printf("NFO: Starting movie at %s\n", current_filename);
if (gzMovie == NULL) {
printf("WRN: Unable to start movie at %s\n", current_filename);
free(current_filename); // XXX Would need to edit
// state->movie_file ?
current_filename = NULL;
}
}
if (current_filename == NULL && last_filename != NULL)
{
gzclose(gzMovie);
printf("NFO: Wrote/lost %d/%d movie frames at %.2f fps to %s\n",
movie_frames, lost_frames, 1e3 * movie_frames / (vx_mtime() - movie_start_mtime), last_filename);
free(last_filename);
}
last_filename = current_filename;
}
// XXX Cleanup? might shutdown while recording?
return NULL;
}
// Pass in a codes describing which resources are no longer in use. Decrement user counts,
// and return a list of all resources whos counts have reached zero, which therefore
// should be deleted from the display using a OP_DEALLOC_RESOURCES opcode
void vx_resc_manager_buffer_resources(vx_resc_manager_t * mgr, const uint8_t * data, int datalen)
{
if (0) print_manager(mgr);
vx_code_input_stream_t * cins = vx_code_input_stream_create(data, datalen);
int code = cins->read_uint32(cins);
assert(code == OP_BUFFER_RESOURCES);
int worldID = cins->read_uint32(cins);
char * name = strdup(cins->read_str(cins)); //freed when cur_resources is eventually removed from the buffer map
int count = cins->read_uint32(cins);
zhash_t * cur_resources = zhash_create(sizeof(uint64_t), sizeof(vx_resc_t*), zhash_uint64_hash, zhash_uint64_equals);
vx_resc_t * vr = NULL;
for (int i = 0; i < count; i++) {
uint64_t id = cins->read_uint64(cins);
zhash_put(cur_resources, &id, &vr, NULL, NULL);
}
assert(cins->pos == cins->len); // we've emptied the stream
vx_code_input_stream_destroy(cins);
// 1 Update our records
zhash_t * worldBuffers = NULL;
zhash_get(mgr->allLiveSets, &worldID, &worldBuffers);
if (worldBuffers == NULL) {
worldBuffers = zhash_create(sizeof(char*), sizeof(zhash_t*), zhash_str_hash, zhash_str_equals);
zhash_put(mgr->allLiveSets, &worldID, &worldBuffers, NULL, NULL);
}
zhash_t * old_resources = NULL;
char * old_name = NULL;
zhash_put(worldBuffers, &name, &cur_resources, &old_name, &old_resources);
free(old_name);
// 2 Figure out which resources have become unused:
if(old_resources != NULL) {
removeAll(old_resources, cur_resources);
zarray_t * dealloc = zarray_create(sizeof(uint64_t));
// now 'old_resources' contains only the resources that are no longer referenced
// iterate through each one, and see if there is a buffer somewhere that references it
zhash_iterator_t prev_itr;
zhash_iterator_init(old_resources, &prev_itr);
uint64_t id = -1;
vx_resc_t * vr = NULL;
while(zhash_iterator_next(&prev_itr, &id, &vr)) {
// Check all worlds
zhash_iterator_t world_itr;// gives us all worlds
zhash_iterator_init(mgr->allLiveSets, &world_itr);
uint32_t wIDl = -1;
zhash_t * buffer_map = NULL;
while(zhash_iterator_next(&world_itr, &wIDl, &buffer_map)) {
zhash_iterator_t buffer_itr; // gives us all buffers
zhash_iterator_init(buffer_map, &buffer_itr);
char * bName = NULL;
zhash_t * resc_map = NULL;
while(zhash_iterator_next(&buffer_itr, &bName, &resc_map)) {
if (zhash_contains(resc_map, &id)) {
goto continue_outer_loop;
}
}
}
// If none of the worlds have this resource, we need to flag removal
zarray_add(dealloc, &id);
continue_outer_loop:
;
}
// 3 Issue dealloc commands
if (zarray_size(dealloc) > 0) {
vx_code_output_stream_t * couts = vx_code_output_stream_create(512);
couts->write_uint32(couts, OP_DEALLOC_RESOURCES);
couts->write_uint32(couts, zarray_size(dealloc));
for (int i = 0; i < zarray_size(dealloc); i++) {
uint64_t id = 0;
zarray_get(dealloc, i, &id);
couts->write_uint64(couts, id);
}
mgr->disp->send_codes(mgr->disp, couts->data, couts->pos);
vx_code_output_stream_destroy(couts);
// Also remove the resources we deallocated from remoteResc
for (int i = 0; i < zarray_size(dealloc); i++) {
uint64_t id = 0;
zarray_get(dealloc, i, &id);
assert(zhash_contains(mgr->remoteResc, &id));
zhash_remove(mgr->remoteResc, &id, NULL, NULL);
}
}
zarray_destroy(dealloc);
zhash_destroy(old_resources);
}
if (0) {
print_manager(mgr);
printf("\n\n");
}
}
static void update_view(vx_gtk_buffer_manager_t * gtk)
{
// This order of these two mutex locks should prevent deadloc
// even if a user sends op codes while the user holds the GDK mutex
gdk_threads_enter();
pthread_mutex_lock(>k->mutex);
// Clear XXX Double buffered?
GList * children = gtk_container_get_children(GTK_CONTAINER(gtk->window));
for(GList * iter = children; iter != NULL; iter = g_list_next(iter))
gtk_widget_destroy(GTK_WIDGET(iter->data));
g_list_free(children);
// Rebuild from scratch
GtkWidget * box = gtk_vbox_new(0, 10);
GtkWidget * widget = NULL;
zarray_t *layers = zhash_values(gtk->layers); // contents: layer_info_t*
zarray_sort(layers, layer_info_compare);
for (int lidx = 0; lidx < zarray_size(layers); lidx++) {
layer_info_t *linfo = NULL;
zarray_get(layers, lidx, &linfo);
// Draw the layer name:
widget = gtk_label_new("");
char * text = sprintf_alloc("<b>Layer %d</b>", linfo->layer_id);
gtk_label_set_markup(GTK_LABEL(widget), text);
free(text);
//gtk_container_add(GTK_CONTAINER(box), widget);
gtk_box_pack_start(GTK_BOX(box), widget, FALSE, FALSE, 0);
// Make a checkbox for each buffer
zarray_t *buffers = zhash_values(linfo->buffers); // contents: buffer_info_t*
zarray_sort(buffers, buffer_info_compare);
for (int i = 0; i < zarray_size(buffers); i++) {
buffer_info_t * buffer = NULL;
zarray_get(buffers, i, &buffer);
assert(buffer != NULL);
widget = gtk_check_button_new_with_label(buffer->name);
gtk_toggle_button_set_active(GTK_TOGGLE_BUTTON(widget), buffer->enabled);
g_signal_connect (G_OBJECT(widget), "toggled",
G_CALLBACK (buffer_checkbox_changed), buffer);
//gtk_container_add(GTK_CONTAINER(box), widget);
gtk_box_pack_start(GTK_BOX(box), widget, FALSE, FALSE, 0);
}
zarray_destroy(buffers);
}
gtk_container_add(GTK_CONTAINER(gtk->window), box);
gtk_widget_show_all(box);
zarray_destroy(layers);
pthread_mutex_unlock(>k->mutex);
gdk_threads_leave();
}
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;
}
// 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;
}
static void save_scene(state_t * state, vx_code_input_stream_t * cins)
{
char * filename = NULL;
if (vx_code_input_stream_available(cins) > 0) {
const char * f2 = cins->read_str(cins);
filename = strdup(f2);
} else { // generate a unique ID
uint64_t mtime = vx_mtime();
time_t now = time(NULL);
struct tm * now2 = localtime(&now);
filename=sprintf_alloc("v%4d%02d%02d_%02d%02d%02d_%03d.vxs",
now2->tm_year + 1900,
now2->tm_mon + 1,
now2->tm_mday,
now2->tm_hour,
now2->tm_min,
now2->tm_sec,
(int)(mtime % 1000));
}
vx_code_output_stream_t * couts = vx_gl_renderer_serialize(state->glrend);
// Append camera positions
if (state->last_render_info != NULL) {
render_info_t *rinfo = state->last_render_info;
for (int i = 0; i <4; i++)
couts->write_uint32(couts, rinfo->viewport[i]);
couts->write_uint32(couts, zarray_size(rinfo->layers));
for (int i = 0; i < zarray_size(rinfo->layers); i++) {
layer_info_t * linfo = NULL;
zarray_get(rinfo->layers, i, &linfo);
couts->write_uint32(couts, linfo->layer_id);
vx_camera_pos_t * pos = NULL;
int * viewport = NULL;
zhash_get(rinfo->camera_positions, &linfo->layer_id, &pos);
zhash_get(rinfo->layer_positions, &linfo->layer_id, &viewport);
vx_tcp_util_pack_camera_pos(pos, couts);
for (int i = 0; i < 4; i++)
couts->write_uint32(couts, viewport[i]);
}
}
FILE * fp = fopen(filename, "w");
fwrite(couts->data, sizeof(uint8_t), couts->pos, fp);
fclose(fp);
printf("Wrote %d bytes to %s\n", couts->pos, filename);
vx_code_output_stream_destroy(couts);
free(filename);
}
