Example #1
0
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);
}
Example #2
0
static void vx_world_buffer_destroy(vx_buffer_t * buffer)
{
    vx_world_t * vw = buffer->world;

    pthread_mutex_lock(&vw->buffer_mutex);

    free(buffer->name);
    zarray_vmap(buffer->back_objs, vx_object_dec_destroy);
    zarray_destroy(buffer->back_objs);

    zarray_vmap(buffer->pending_objs, vx_object_dec_destroy);
    zarray_destroy(buffer->pending_objs);

    zarray_vmap(buffer->front_objs, vx_object_dec_destroy);
    zarray_destroy(buffer->front_objs);

    zhash_vmap_values(buffer->front_resc, vx_resc_dec_destroy);
    zhash_destroy(buffer->front_resc);

    vx_code_output_stream_destroy(buffer->front_codes);

    pthread_mutex_destroy(&buffer->mutex);
    free(buffer);

    pthread_mutex_unlock(&vw->buffer_mutex);
}
Example #3
0
void
url_parser_destroy (url_parser_t *urlp)
{
    free(urlp->protocol);
    free(urlp->host);
    free(urlp->path);
    zarray_vmap(urlp->keys, free);
    zarray_vmap(urlp->vals, free);
    zarray_destroy(urlp->keys);
    zarray_destroy(urlp->vals);
    free(urlp);
}
Example #4
0
void workerpool_destroy(workerpool_t *wp)
{
    if (wp == NULL)
        return;

    // force all worker threads to exit.
    if (wp->nthreads > 1) {
        for (int i = 0; i < wp->nthreads; i++)
            workerpool_add_task(wp, NULL, NULL);

        pthread_mutex_lock(&wp->mutex);
        pthread_cond_broadcast(&wp->startcond);
        pthread_mutex_unlock(&wp->mutex);

        for (int i = 0; i < wp->nthreads; i++)
            pthread_join(wp->threads[i], NULL);

        pthread_mutex_destroy(&wp->mutex);
        pthread_cond_destroy(&wp->startcond);
        pthread_cond_destroy(&wp->endcond);
        free(wp->threads);
    }

    zarray_destroy(wp->tasks);
    free(wp);
}
Example #5
0
File: vx_util.c Project: DH-std/A3
void vx_util_unproject(double * winxyz, double * model_matrix, double * projection_matrix, int * viewport, double * vec3_out)
{
    zarray_t * fp = zarray_create(sizeof(matd_t*));

    matd_t * mm = matd_create_data(4, 4, model_matrix);
    zarray_add(fp, &mm);
    matd_t * pm = matd_create_data(4, 4, projection_matrix);
    zarray_add(fp, &pm);

    matd_t *invpm = matd_op("(MM)^-1", pm, mm);
    zarray_add(fp, &invpm);

    double v[4] = { 2*(winxyz[0]-viewport[0]) / viewport[2] - 1,
                    2*(winxyz[1]-viewport[1]) / viewport[3] - 1,
                    2*winxyz[2] - 1,
                    1 };
    matd_t * vm = matd_create_data(4, 1, v);
    zarray_add(fp, &vm);

    matd_t * objxyzh = matd_op("MM", invpm, vm);
    zarray_add(fp, &objxyzh);

    vec3_out[0] = objxyzh->data[0] / objxyzh->data[3];
    vec3_out[1] = objxyzh->data[1] / objxyzh->data[3];
    vec3_out[2] = objxyzh->data[2] / objxyzh->data[3];

    // cleanup
    zarray_vmap(fp, matd_destroy);
    zarray_destroy(fp);
}
	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);
	}
Example #7
0
void default_cam_mgr_rotate(default_cam_mgr_t * state, double *q, uint32_t animate_ms)
{
    zarray_t * fp = zarray_create(sizeof(matd_t*));

    matd_t * eye = matd_create_data(3,1,state->eye1);       zarray_add(fp, &eye);
    matd_t *lookat = matd_create_data(3,1,state->lookat1);  zarray_add(fp, &lookat);
    matd_t *up = matd_create_data(3,1,state->up1);          zarray_add(fp, &up);


    matd_t * toEye = matd_subtract(eye, lookat); zarray_add(fp, &toEye);
    matd_t * nextToEye = matd_create(3,1); zarray_add(fp, &nextToEye);
    vx_util_quat_rotate(q, toEye->data, nextToEye->data);

    matd_t * nextEye = matd_add(lookat, nextToEye); zarray_add(fp, &nextEye);
    matd_t * nextUp = matd_copy(up); zarray_add(fp, &nextUp);

    vx_util_quat_rotate(q, up->data, nextUp->data);

    // copy back results
    memcpy(state->eye1, nextEye->data, sizeof(double)*3);
    memcpy(state->up1, nextUp->data, sizeof(double)*3);
    state->mtime1 = vx_util_mtime() + animate_ms;

    // Disable any prior fit command
    default_destroy_fit(state);

    // cleanup
    zarray_vmap(fp, matd_destroy);
    zarray_destroy(fp);

    default_cam_mgr_follow_disable(state);

}
Example #8
0
File: vx_util.c Project: DH-std/A3
void vx_util_project(double * xyz, double * M44, double * P44, int * viewport, double * win_out3)
{
    zarray_t * fp = zarray_create(sizeof(matd_t*));

    matd_t * M = matd_create_data(4,4, M44); zarray_add(fp, &M);
    matd_t * P = matd_create_data(4,4, P44); zarray_add(fp, &P);
    matd_t * xyzp = matd_create(4,1); zarray_add(fp, &xyzp);
    memcpy(xyzp->data, xyz, 3*sizeof(double));
    xyzp->data[3] = 1.0;

    matd_t * p = matd_op("MMM", P, M, xyzp); zarray_add(fp, &p);

        p->data[0] = p->data[0] / p->data[3];
        p->data[1] = p->data[1] / p->data[3];
        p->data[2] = p->data[2] / p->data[3];


    double res[] =  { viewport[0] + viewport[2]*(p->data[0]+1)/2.0,
                      viewport[1] + viewport[3]*(p->data[1]+1)/2.0,
                      (viewport[2] + 1)/2.0 };
    memcpy(win_out3, res, 3*sizeof(double));

    // cleanup
    zarray_vmap(fp, matd_destroy);
    zarray_destroy(fp);
}
Example #9
0
static void state_destroy(state_t * state)
{
    if (verbose) printf("State destroying\n");
    state->rendering = 0;
    pthread_join(state->render_thread, NULL);
    if (verbose) printf("render thread joined\n");

    pthread_cond_signal(&state->movie_cond);
    pthread_join(state->movie_thread, NULL);
    if (verbose) printf("movie thread joined\n");

    zhash_vmap_values(state->layer_info_map, layer_info_destroy);
    zhash_destroy(state->layer_info_map); // XXX values

    for (int i =0; i < 2; i++) {
        free(state->pixdatas[i]);
        if (state->pixbufs[i] != NULL) {
            g_object_unref(state->pixbufs[i]);
        }
    }


    task_thread_schedule_blocking(gl_thread, gl_cleanup_task, state);

    pthread_mutex_destroy(&state->mutex);
    pthread_mutex_destroy(&state->movie_mutex);
    pthread_cond_destroy(&state->movie_cond);

    zarray_destroy(state->listeners);
    if (state->last_render_info != NULL)
        render_info_destroy(state->last_render_info);
    vx_resc_manager_destroy(state->mgr);
    vx_gtk_buffer_manager_destroy(state->buffer_manager);
    free(state);
}
Example #10
0
void
getopt_destroy (getopt_t *gopt)
{
    // free the extra arguments and container
    zarray_vmap (gopt->extraargs, free);
    zarray_destroy (gopt->extraargs);

    // deep free of the getopt_option structs. Also frees key/values, so
    // after this loop, hash tables will no longer work
    zarray_vmap (gopt->options, getopt_option_destroy);
    zarray_destroy (gopt->options);

    // free tables
    zhash_destroy (gopt->lopts);
    zhash_destroy (gopt->sopts);

    free (gopt);
}
Example #11
0
File: vx_util.c Project: DH-std/A3
void vx_util_lookat(double * _eye, double * _lookat, double * _up, double * _out44)
{
    zarray_t * fp = zarray_create(sizeof(matd_t*));

    matd_t * eye = matd_create_data(3,1, _eye);
    zarray_add(fp, &eye);

    matd_t * lookat = matd_create_data(3,1, _lookat);
    zarray_add(fp, &lookat);

    matd_t * up = matd_create_data(3,1, _up);
    zarray_add(fp, &up);

    up = matd_vec_normalize(up);

    zarray_add(fp, &up); // note different pointer than before!

    matd_t * tmp1 = matd_subtract(lookat, eye); zarray_add(fp, &tmp1);

    matd_t * f = matd_vec_normalize(tmp1);      zarray_add(fp, &f);
    matd_t * s = matd_crossproduct(f, up);      zarray_add(fp, &s);
    matd_t * u = matd_crossproduct(s, f);       zarray_add(fp, &u);

    matd_t * M = matd_create(4,4); // set the rows of M with s, u, -f
    zarray_add(fp, &M);
    memcpy(M->data,s->data,3*sizeof(double));
    memcpy(M->data + 4,u->data,3*sizeof(double));
    memcpy(M->data + 8,f->data,3*sizeof(double));
    for (int i = 0; i < 3; i++)
        M->data[2*4 +i] *= -1;
    M->data[3*4 + 3] = 1.0;


    matd_t * T = matd_create(4,4);
    T->data[0*4 + 3] = -eye->data[0];
    T->data[1*4 + 3] = -eye->data[1];
    T->data[2*4 + 3] = -eye->data[2];
    T->data[0*4 + 0] = 1;
    T->data[1*4 + 1] = 1;
    T->data[2*4 + 2] = 1;
    T->data[3*4 + 3] = 1;
    zarray_add(fp, &T);

    matd_t * MT = matd_op("MM",M,T);
    zarray_add(fp, &MT);


    memcpy(_out44, MT->data, 16*sizeof(double));

    // cleanup
    zarray_vmap(fp, matd_destroy);
    zarray_destroy(fp);
}
Example #12
0
static void  render_info_destroy(render_info_t * rinfo)
{
    zarray_destroy(rinfo->layers); // just pointers

    zhash_vmap_values(rinfo->camera_positions, vx_camera_pos_destroy);
    zhash_destroy(rinfo->camera_positions);

    zhash_vmap_values(rinfo->layer_positions, free);
    zhash_destroy(rinfo->layer_positions);

    free(rinfo);
}
Example #13
0
File: vx_util.c Project: DH-std/A3
void vx_util_angle_axis_to_quat(double theta, double * axis3, double * qout)
{
    zarray_t * fp = zarray_create(sizeof(matd_t*));

    matd_t * axis = matd_create_data(3,1, axis3); zarray_add(fp, &axis);
    matd_t * axis_norm = matd_vec_normalize(axis); zarray_add(fp, &axis_norm);

    qout[0] = cos(theta/2);
    double s = sin(theta/2);

    qout[1] = axis_norm->data[0] * s;
    qout[2] = axis_norm->data[1] * s;
    qout[3] = axis_norm->data[2] * s;

    // cleanup
    zarray_vmap(fp, matd_destroy);
    zarray_destroy(fp);
}
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(&gtk->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(&gtk->mutex);
    gdk_threads_leave();
}
Example #15
0
vx_camera_pos_t * default_cam_mgr_get_cam_pos(default_cam_mgr_t * state, int * viewport, uint64_t mtime)
{
    vx_camera_pos_t * p = calloc(1, sizeof(vx_camera_pos_t));
    memcpy(p->viewport, viewport, 4*sizeof(int));

    p->perspective_fovy_degrees = state->perspective_fovy_degrees;
    p->zclip_near = state->zclip_near;
    p->zclip_far = state->zclip_far;

    // process a fit command if necessary:
    if (state->fit != NULL) {
        fit_t * f = state->fit;

        // consume the fit command
        state->fit = NULL; // XXX minor race condition, could lose a fit cmd

        // XXX We can probably do better than this using the viewport...
        state->lookat1[0] = (f->xy0[0] + f->xy1[0]) / 2;
        state->lookat1[1] = (f->xy0[1] + f->xy1[1]) / 2;
        state->lookat1[2] = 0;

        // dimensions of fit
        double Fw = f->xy1[0] - f->xy0[0];
        double Fh = f->xy1[1] - f->xy0[1];

        // aspect ratios
        double Far = Fw / Fh;
        double Var = p->viewport[2] * 1.0 / p->viewport[3];

        double tAngle = tan(p->perspective_fovy_degrees/2*M_PI/180.0);
        double height = fabs(0.5 * (Var > Far ? Fh : Fw / Var) / tAngle);

        state->eye1[0] = state->lookat1[0];
        state->eye1[1] = state->lookat1[1];
        state->eye1[2] = height;

        state->up1[0] = 0;
        state->up1[1] = 1;
        state->up1[2] = 0;

        state->mtime1 = f->mtime;

        free(f);
    }

    if (mtime > state->mtime1) {
        memcpy(p->eye, state->eye1, 3*sizeof(double));
        memcpy(p->up, state->up1, 3*sizeof(double));
        memcpy(p->lookat, state->lookat1, 3*sizeof(double));
        p->perspectiveness = state->perspectiveness1;
    } else  if (mtime <= state->mtime0) {
        memcpy(p->eye, state->eye0, 3*sizeof(double));
        memcpy(p->up, state->up0, 3*sizeof(double));
        memcpy(p->lookat, state->lookat0, 3*sizeof(double));
        p->perspectiveness = state->perspectiveness0;
    } else {
        double alpha1 = ((double) mtime - state->mtime0) / (state->mtime1 - state->mtime0);
        double alpha0 = 1.0 - alpha1;

        scaled_combination(state->eye0,    alpha0, state->eye1,    alpha1, p->eye,    3);
        scaled_combination(state->up0,     alpha0, state->up1,     alpha1, p->up,     3);
        scaled_combination(state->lookat0, alpha0, state->lookat1, alpha1, p->lookat, 3);
        p->perspectiveness = state->perspectiveness0*alpha0 + state->perspectiveness1*alpha1;

        // Tweak so eye-to-lookat is the right distance
        {
            zarray_t * fp = zarray_create(sizeof(matd_t*));

            matd_t * eye = matd_create_data(3,1, p->eye); zarray_add(fp, &eye);
            matd_t * lookat = matd_create_data(3,1, p->lookat); zarray_add(fp, &lookat);
            matd_t * up = matd_create_data(3,1, p->up); zarray_add(fp, &up);

            matd_t * eye0 = matd_create_data(3,1, state->eye0); zarray_add(fp, &eye0);
            matd_t * lookat0 = matd_create_data(3,1, state->lookat0); zarray_add(fp, &lookat0);
            matd_t * up0 = matd_create_data(3,1, state->up0); zarray_add(fp, &up0);

            matd_t * eye1 = matd_create_data(3,1, state->eye1); zarray_add(fp, &eye1);
            matd_t * lookat1 = matd_create_data(3,1, state->lookat1); zarray_add(fp, &lookat1);
            matd_t * up1 = matd_create_data(3,1, state->up1); zarray_add(fp, &up1);


            double dist0 = matd_vec_dist(eye0, lookat0);
            double dist1 = matd_vec_dist(eye1, lookat1);

            matd_t * dist = matd_create_scalar(dist0*alpha0 + dist1*alpha1); zarray_add(fp, &dist);

            matd_t * eye2p = matd_subtract(eye,lookat); zarray_add(fp, &eye2p);
            eye2p = matd_vec_normalize(eye2p); zarray_add(fp, &eye2p);

            eye = matd_op("M + (M*M)", lookat, eye2p, dist);

            // Only modified eye
            memcpy(p->eye, eye->data, 3*sizeof(double));

            zarray_vmap(fp, matd_destroy);

            zarray_destroy(fp);
        }
    }

    // Need to do more fixup depending on interface mode!
    {
        if (state->interface_mode <= 2.0) {
            // stack eye on lookat:
            p->eye[0] = p->lookat[0];
            p->eye[1] = p->lookat[1];
            p->lookat[2] = 0;

            // skip fabs() for ENU/NED compat
            //p->eye[2] = fabs(p->eye[2]);


            {
                matd_t * up = matd_create_data(3,1, p->up);
                up->data[2] = 0; // up should never point in Z
                matd_t * up_norm = matd_vec_normalize(up);

                memcpy(p->up, up_norm->data, sizeof(double)*3);
                matd_destroy(up);
                matd_destroy(up_norm);
            }

        } else if (state->interface_mode == 2.5) {
            zarray_t * fp = zarray_create(sizeof(matd_t*));

            matd_t * eye = matd_create_data(3,1, p->eye); zarray_add(fp, &eye);
            matd_t * lookat = matd_create_data(3,1, p->lookat); zarray_add(fp, &lookat);
            matd_t * up = matd_create_data(3,1, p->up); zarray_add(fp, &up);

            lookat->data[2] = 0.0;

            // Level horizon
            matd_t * dir = matd_subtract(lookat, eye); zarray_add(fp, &dir);
            matd_t * dir_norm = matd_vec_normalize(dir); zarray_add(fp, &dir_norm);
            matd_t * left = matd_crossproduct(up, dir_norm); zarray_add(fp, &left);
            left->data[2] = 0.0;

            left = matd_vec_normalize(left); zarray_add(fp, &left);


            // Don't allow upside down
            //up->data[2] = fmax(0.0, up->data[2]); // XXX NED?

            // Find an 'up' direction perpendicular to left
            matd_t * dot_scalar = matd_create_scalar(matd_vec_dot_product(up, left)); zarray_add(fp, &dot_scalar);
            up = matd_op("M - (M*M)", up,  left, dot_scalar); zarray_add(fp, &up);
            up = matd_vec_normalize(up); zarray_add(fp, &up);

            // Now find eye position by computing new lookat dir
            matd_t * eye_dir = matd_crossproduct(up, left); zarray_add(fp, &eye_dir);

            matd_t *eye_dist_scalar = matd_create_scalar(matd_vec_dist(eye, lookat)); zarray_add(fp, &eye_dist_scalar);

            eye = matd_op("M + (M*M)", lookat, eye_dir, eye_dist_scalar); zarray_add(fp, &eye);

            // export results back to p:
            memcpy(p->eye, eye->data, sizeof(double)*3);
            memcpy(p->lookat, lookat->data, sizeof(double)*3);
            memcpy(p->up, up->data, sizeof(double)*3);

            zarray_vmap(fp, matd_destroy);
            zarray_destroy(fp);
        }
    }

    // Fix up for bad zoom
    if (1) {
        matd_t * eye = matd_create_data(3,1, p->eye);
        matd_t * lookat = matd_create_data(3,1, p->lookat);
        matd_t * up = matd_create_data(3,1, p->up);

        matd_t * lookeye = matd_subtract(lookat, eye);
        matd_t * lookdir = matd_vec_normalize(lookeye);
        double dist =  matd_vec_dist(eye, lookat);
        dist  = fmin(state->zclip_far / 3.0, dist);
        dist  = fmax(state->zclip_near * 3.0, dist);

        matd_scale_inplace(lookdir, dist);

        matd_t * eye_fixed = matd_subtract(lookat, lookdir);

        memcpy(p->eye, eye_fixed->data, sizeof(double)*3);

        matd_destroy(eye);
        matd_destroy(lookat);
        matd_destroy(up);
        matd_destroy(lookeye);
        matd_destroy(lookdir);
        matd_destroy(eye_fixed);
    }

    // copy the result back into 'state'
    {
        memcpy(state->eye0, p->eye, 3*sizeof(double));
        memcpy(state->up0, p->up, 3*sizeof(double));
        memcpy(state->lookat0, p->lookat, 3*sizeof(double));
        state->perspectiveness0 = p->perspectiveness;
        state->mtime0 = mtime;
    }

    return p;
}
Example #16
0
static void wav_group_destroy(wav_group_t * wgrp)
{
    zarray_destroy(wgrp->group_idx);
    free(wgrp);
}
Example #17
0
void hsv_find_balls_blob_detector(image_u32_t* im, frame_t frame, metrics_t met, zarray_t* blobs_out)
{
    assert(frame.xy0.x < frame.xy1.x && frame.xy0.y < frame.xy1.y);
    assert(frame.xy0.x >= 0 && frame.xy0.y >= 0 && frame.xy1.x < im->width && frame.xy1.y < im->height);
    assert(frame.ex0.x < frame.ex1.x && frame.ex0.y < frame.ex1.y);
    assert(frame.ex0.x >= 0 && frame.ex0.y >= 0 && frame.ex1.x < im->width && frame.ex1.y < im->height);

    // Int to node
    zhash_t* node_map = zhash_create(sizeof(uint32_t), sizeof(node_t*),
            zhash_uint32_hash, zhash_uint32_equals);

    for(int i = frame.xy0.y; i < frame.xy1.y; i++) {
        for(int j = frame.xy0.x; j < frame.xy1.x; j++) {
            if((i < frame.ex0.y || i > frame.ex1.y) || (j < frame.ex0.x || j > frame.ex1.x)) {

                uint32_t idx_im = i * im->stride + j; // Indframe.ex relative to image

                // Pixel color data
                uint32_t abgr = im->buf[idx_im];
                hsv_t hsv = {0,0,0};
                rgb_to_hsv(abgr, &hsv);
                hsv_t error = {fabs(hsv.hue - met.hsv.hue), 
                                fabs(hsv.sat - met.hsv.sat), 
                                fabs(hsv.val - met.hsv.val)};

                // 'Acceptable'
                 if((error.hue < met.error.hue) && 
                    (error.sat < met.error.sat) && 
                    (error.val < met.error.val)) 
                 {
                    // Create new node, set itself up as a parent
                    node_t* n = calloc(1, sizeof(node_t));
                    n->id = idx_im;
                    n->parent_id = idx_im;
                    n->parent_node = n;
                    n->num_children = 0;

                    node_t* tmp_node;
                    uint32_t tmp_idx;

                    // Add node to node map
                    if(zhash_put(node_map, &idx_im, &n, &tmp_idx, &tmp_node)==1) 
                    {
                        assert(0);
                    }

                    //Check if apart of another blob, or starting a new blob 
                    // if apart of another, point to the parent, if a new blob, point to self 
                    //Check neighbours
                    if(!met.lines) {    // only check this if don't want lines for tape detection
                        if(j > frame.xy0.x) {
                            tmp_idx = idx_im - 1; // is Left neighbour similar color 
                            if(zhash_get(node_map, &tmp_idx, &tmp_node) == 1) {
                                node_t* neighbour = tmp_node;
                                connect(n, neighbour);                    
                            }
                        }
                    }
                    if(i > frame.xy0.y) { 
                        tmp_idx = idx_im - im->stride; // is Bottom neighbor similar color
                        if(tmp_idx > 0 && zhash_get(node_map, &tmp_idx, &tmp_node) == 1) {
                            node_t* neighbour = tmp_node;
                            connect(neighbour,n);                    
                        }
                    }
                }
            }
        }
    }


    //count number of children for each parent, go through node_map
    // if a node is not a parent, add 1 to it's parent->num_children and delete from hash
    // if is a parent do nothing
    zarray_t* vals = zhash_values(node_map);
    for(int i = 0; i < zarray_size(vals); i++) {
        node_t* node;
        zarray_get(vals, i, &node);
        resolve_r(node);

        if(node->parent_id != node->id) {
            node->parent_node->num_children++;
            // key should exist, if it doesn't find out why
            assert(zhash_remove(node_map, &node->id, NULL, NULL));
        }
    }

    // search parent only hash and add to blobs out conditionally
    vals = zhash_values(node_map);
    for(int i = 0; i < zarray_size(vals); i++) {
        node_t* node;
        zarray_get(vals, i, &node);
        if(node->num_children > met.min_size) {
            loc_t pos;
            pos.x = node->parent_id%im->stride;
            pos.y = node->parent_id/im->stride;
            zarray_add(blobs_out, &pos);
            // printf("parent %d\n", node->id);
        }
    }
    zarray_destroy(vals);
    zhash_vmap_values(node_map, free);
    zhash_destroy(node_map);
}
Example #18
0
// this loop tries to run at X fps, and issue render commands
static void * render_loop(void * foo)
{
    state_t * state = foo;
    if (verbose)printf("Starting render thread!\n");

    uint64_t render_count = 0;
    uint64_t last_mtime = vx_mtime();
    double avgDT = 1.0f/state->target_frame_rate;
    uint64_t avg_loop_us = 3000; // initial render time guess
    while (state->rendering) {
        int64_t sleeptime = (1000000 / state->target_frame_rate) - (int64_t) avg_loop_us;
        if (sleeptime > 0)
            usleep(sleeptime); // XXX fix to include render time

        // Diagnostic tracking
        uint64_t mtime_start = vx_mtime(); // XXX
        avgDT = avgDT*.9 + .1 * (mtime_start - last_mtime)/1000;
        last_mtime = mtime_start;
        render_count++;

        if (verbose) {
            if (render_count % 100 == 0)
                printf("Average render DT = %.3f FPS = %.3f avgloopus %"PRIu64" sleeptime = %"PRIi64"\n",
                       avgDT, 1.0/avgDT, avg_loop_us, sleeptime);
        }

        // prep the render data
        render_buffer_t rbuf;
        rbuf.state = state;
        rbuf.width = gtku_image_pane_get_width(state->imagePane);
        rbuf.height = gtku_image_pane_get_height(state->imagePane);
        if (rbuf.width == 0 && rbuf.height == 0)
            continue; // if the viewport is 0,0

        // smartly reuse, or reallocate the output pixel buffer when resizing occurs
        GdkPixbuf * pixbuf = state->pixbufs[state->cur_pb_idx];
        if (pixbuf == NULL || gdk_pixbuf_get_width(pixbuf) != rbuf.width || gdk_pixbuf_get_height(pixbuf) != rbuf.height) {
            if (pixbuf != NULL) {
                g_object_unref(pixbuf);
                free(state->pixdatas[state->cur_pb_idx]);
            }

            state->pixdatas[state->cur_pb_idx] = malloc(rbuf.width*rbuf.height*3); // can't stack allocate, can be too big (retina)
            pixbuf = gdk_pixbuf_new_from_data(state->pixdatas[state->cur_pb_idx], GDK_COLORSPACE_RGB, FALSE, 8,
                                              rbuf.width, rbuf.height, rbuf.width*3, NULL, NULL); // no destructor fn for pix data, handle manually
            state->pixbufs[state->cur_pb_idx] = pixbuf;
        }

        // second half of init:
        rbuf.out_buf = gdk_pixbuf_get_pixels(pixbuf);
        rbuf.format = GL_RGB;
        rbuf.rendered = 0;

        // 1 compute all the viewports
        render_info_t * rinfo = render_info_create();
        rinfo->viewport[0] = rinfo->viewport[1] = 0;
        rinfo->viewport[2] = rbuf.width;
        rinfo->viewport[3] = rbuf.height;

        {
            zhash_iterator_t itr;
            uint32_t layer_id = 0;
            layer_info_t * linfo = NULL;
            zhash_iterator_init(state->layer_info_map, &itr);
            while(zhash_iterator_next(&itr, &layer_id, &linfo)){
                zarray_add(rinfo->layers, &linfo);
            }
            zarray_sort(rinfo->layers, zvx_layer_info_compare);
        }

        zarray_t * fp = zarray_create(sizeof(matd_t*));

        matd_t *mm = matd_create(4,4); zarray_add(fp, &mm);
        matd_t *pm = matd_create(4,4); zarray_add(fp, &pm);

        pthread_mutex_lock(&state->mutex);
        for (int i = 0; i < zarray_size(rinfo->layers); i++) {
            layer_info_t *linfo = NULL;
            zarray_get(rinfo->layers, i, &linfo);

            int * viewport = vx_viewport_mgr_get_pos(linfo->vp_mgr, rinfo->viewport, mtime_start);
            vx_camera_pos_t *pos = default_cam_mgr_get_cam_pos(linfo->cam_mgr, viewport, mtime_start);

            // store viewport, pos
            zhash_put(rinfo->layer_positions, &linfo->layer_id, &viewport, NULL, NULL);
            zhash_put(rinfo->camera_positions, &linfo->layer_id, &pos, NULL, NULL);

            // feed the actual camera/projection matrix to the gl side
            vx_camera_pos_model_matrix(pos,mm->data);
            vx_camera_pos_projection_matrix(pos, pm->data);

            matd_t * pmmm = matd_multiply(pm,mm); zarray_add(fp, &pmmm);

            float pm16[16];
            vx_util_copy_floats(pmmm->data, pm16, 16);

            float eye3[16];
            vx_util_copy_floats(pos->eye, eye3, 3);

            vx_gl_renderer_set_layer_render_details(state->glrend, linfo->layer_id, viewport, pm16, eye3);
        }

        // 2 Render the data
        task_thread_schedule_blocking(gl_thread, render_task, &rbuf);

        render_info_t * old = state->last_render_info;
        state->last_render_info = rinfo;

        pthread_mutex_unlock(&state->mutex);


        // 3 if a render occurred, then swap gtk buffers
        if (rbuf.rendered) {

            // point to the correct buffer for the next render:
            state->cur_pb_idx = (state->cur_pb_idx +1 ) % 2;

            // flip y coordinate in place:
            vx_util_flipy(rbuf.width*3, rbuf.height, rbuf.out_buf);

            // swap the image's backing buffer
            g_object_ref(pixbuf); // XXX Since gtku always unrefs with each of these calls, increment accordingly
            gtku_image_pane_set_buffer(state->imagePane, pixbuf);
        }

        // 3.1 If a movie is in progress, also need to serialize the frame
        pthread_mutex_lock(&state->movie_mutex);
        if (state->movie_file != NULL) {

            int last_idx = (state->cur_pb_idx + 1) % 2;
            GdkPixbuf * pb = state->pixbufs[last_idx];

            movie_frame_t * movie_img = calloc(1, sizeof(movie_frame_t));
            movie_img->mtime = mtime_start;
            movie_img->width = gdk_pixbuf_get_width(pb);
            movie_img->height = gdk_pixbuf_get_height(pb);
            movie_img->stride = 3*movie_img->width;
            movie_img->buf = malloc(movie_img->stride*movie_img->height);
            memcpy(movie_img->buf, state->pixdatas[last_idx], movie_img->stride*movie_img->height);


            // Alloc in this thread, dealloc in movie thread
            zarray_add(state->movie_pending, & movie_img);

            pthread_cond_signal(&state->movie_cond);

        }
        pthread_mutex_unlock(&state->movie_mutex);


        // cleanup
        if (old)
            render_info_destroy(old);
        zarray_vmap(fp, matd_destroy);
        zarray_destroy(fp);


        uint64_t mtime_end = vx_mtime();
        avg_loop_us = (uint64_t)(.5*avg_loop_us + .5 * 1000 * (mtime_end - mtime_start));
    }
    if (verbose) printf("Render thread exiting\n");

    pthread_exit(NULL);

}
Example #19
0
zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
{
    ////////////////////////////////////////////////////////
    // step 1. threshold the image, creating the edge image.

    int w = im->width, h = im->height, s = im->stride;

    image_u8_t *threshim = threshold(td, im);
    assert(threshim->stride == s);

    image_u8_t *edgeim = image_u8_create(w, h);

    if (1) {
        image_u8_t *sumim = image_u8_create(w, h);

        // apply a horizontal sum kernel of width 3
        for (int y = 0; y < h; y++) {
            for (int x = 1; x+1 < w; x++) {

                sumim->buf[y*s + x] =
                    threshim->buf[y*s + x - 1] +
                    threshim->buf[y*s + x + 0] +
                    threshim->buf[y*s + x + 1];
            }
        }
        timeprofile_stamp(td->tp, "sumim");

        // deglitch
        if (td->qtp.deglitch) {
            for (int y = 1; y+1 < h; y++) {
                for (int x = 1; x+1 < w; x++) {
                    // edge: black pixel next to white pixel
                    if (threshim->buf[y*s + x] == 0 &&
                        sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] == 8) {
                        threshim->buf[y*s + x] = 1;
                        sumim->buf[y*s + x - 1]++;
                        sumim->buf[y*s + x + 0]++;
                        sumim->buf[y*s + x + 1]++;
                    }

                    if (threshim->buf[y*s + x] == 1 &&
                        sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] == 1) {
                        threshim->buf[y*s + x] = 0;
                        sumim->buf[y*s + x - 1]--;
                        sumim->buf[y*s + x + 0]--;
                        sumim->buf[y*s + x + 1]--;
                   }
                }
            }

            timeprofile_stamp(td->tp, "deglitch");
        }

        // apply a vertical sum kernel of width 3; check if any
        // over-threshold pixels are adjacent to an under-threshold
        // pixel.
        //
        // There are two types of edges: white pixels neighboring a
        // black pixel, and black pixels neighboring a white pixel. We
        // label these separately.  (Values 0xc0 and 0x3f are picked
        // such that they add to 255 (see below) and so that they can be
        // viewed as pixel intensities for visualization purposes.)
        //
        // symmetry of detection. We don't want to use JUST "black
        // near white" (or JUST "white near black"), because that
        // biases the detection towards one side of the edge. This
        // measurably reduces detection performance.
        //
        // On large tags, we could treat "neighbor" pixels the same
        // way. But on very small tags, there may be other edges very
        // near the tag edge. Since each of these edges is effectively
        // two pixels thick (the white pixel near the black pixel, and
        // the black pixel near the white pixel), it becomes likely
        // that these two nearby edges will actually touch.
        //
        // A partial solution to this problem is to define edges to be
        // adjacent white-near-black and black-near-white pixels.
        //

        for (int y = 1; y+1 < h; y++) {
            for (int x = 1; x+1 < w; x++) {
                if (threshim->buf[y*s + x] == 0) {
                    // edge: black pixel next to white pixel
                    if (sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] > 0)
                        edgeim->buf[y*s + x] = 0xc0;
                } else {
                    // edge: white pixel next to black pixel when both
                    // edge types are on, we get less bias towards one
                    // side of the edge.
                    if (sumim->buf[y*s + x - s] + sumim->buf[y*s + x] + sumim->buf[y*s + x + s] < 9)
                        edgeim->buf[y*s + x] = 0x3f;
                }
            }
        }

        if (td->debug) {
            for (int y = 0; y < h; y++) {
                for (int x = 0; x < w; x++) {
                    threshim->buf[y*s + x] *= 255;
                }
            }

            image_u8_write_pnm(threshim, "debug_threshold.pnm");
            image_u8_write_pnm(edgeim, "debug_edge.pnm");
//            image_u8_destroy(edgeim2);
        }

        image_u8_destroy(threshim);
        image_u8_destroy(sumim);
    }

    timeprofile_stamp(td->tp, "edges");

    ////////////////////////////////////////////////////////
    // step 2. find connected components.

    unionfind_t *uf = unionfind_create(w * h);

    for (int y = 1; y < h - 1; y++) {
        for (int x = 1; x < w -1; x++) {
            uint8_t v = edgeim->buf[y*s + x];
            if (v==0)
                continue;

            // (dx,dy) pairs for 8 connectivity:
            //          (REFERENCE) (1, 0)
            // (-1, 1)    (0, 1)    (1, 1)
            //
            // i.e., the minimum value of dx should be:
            //   y=0:   1
            //   y=1:  -1
            for (int dy = 0; dy <= 1; dy++) {
                for (int dx = 1-2*dy; dx <= 1; dx++) {
                    if (edgeim->buf[(y+dy)*s + (x+dx)] == v) {
                        unionfind_connect(uf, y*w + x, (y+dy)*w + x + dx);
                    }
                }
            }
        }
    }

    timeprofile_stamp(td->tp, "unionfind");

    zhash_t *clustermap = zhash_create(sizeof(uint64_t), sizeof(zarray_t*),
                                       zhash_uint64_hash, zhash_uint64_equals);

    for (int y = 1; y < h-1; y++) {
        for (int x = 1; x < w-1; x++) {

            uint8_t v0 = edgeim->buf[y*s + x];
            if (v0 == 0)
                continue;

            uint64_t rep0 = unionfind_get_representative(uf, y*w + x);

            // 8 connectivity. (4 neighbors to check).
//            for (int dy = 0; dy <= 1; dy++) {
//                for (int dx = 1-2*dy; dx <= 1; dx++) {

            // 4 connectivity. (2 neighbors to check)
            for (int n = 1; n <= 2; n++) {
                int dy = n & 1;
                int dx = (n & 2) >> 1;

                uint8_t v1 = edgeim->buf[(y+dy)*s + x + dx];
                if (v0 + v1 != 255)
                    continue;
                uint64_t rep1 = unionfind_get_representative(uf, (y+dy)*w + x+dx);

                uint64_t clusterid;
                if (rep0 < rep1)
                    clusterid = (rep1 << 32) + rep0;
                else
                    clusterid = (rep0 << 32) + rep1;

                zarray_t *cluster = NULL;
                if (!zhash_get(clustermap, &clusterid, &cluster)) {
                    cluster = zarray_create(sizeof(struct pt));
                    zhash_put(clustermap, &clusterid, &cluster, NULL, NULL);
                }

                // NB: We will add some points multiple times to a
                // given cluster.  I don't know an efficient way to
                // avoid that here; we remove them later on when we
                // sort points by pt_compare_theta.
                if (1) {
                    struct pt p = { .x = x, .y = y};
                    zarray_add(cluster, &p);
                }
                if (1) {
                    struct pt p = { .x = x+dx, .y = y+dy};
                    zarray_add(cluster, &p);
                }
            }
        }
    }

    // make segmentation image.
    if (td->debug) {
        image_u8_t *d = image_u8_create(w, h);
        assert(d->stride == s);

        uint8_t *colors = (uint8_t*) calloc(w*h, 1);

        for (int y = 0; y < h; y++) {
            for (int x = 0; x < w; x++) {
                uint32_t v = unionfind_get_representative(uf, y*w+x);
                uint32_t sz = unionfind_get_set_size(uf, y*w+x);
                if (sz < td->qtp.min_cluster_pixels)
                    continue;

                uint8_t color = colors[v];

                if (color == 0) {
                    const int bias = 20;
                    color = bias + (random() % (255-bias));
                    colors[v] = color;
                }

                float mix = 0.7;
                mix = 1.0;
                d->buf[y*d->stride + x] = mix*color + (1-mix)*im->buf[y*im->stride + x];
            }
        }

        free(colors);

        image_u8_write_pnm(d, "debug_segmentation.pnm");
        image_u8_destroy(d);
    }

    timeprofile_stamp(td->tp, "make clusters");


    ////////////////////////////////////////////////////////
    // step 3. process each connected component.

    zarray_t *clusters = zhash_values(clustermap);
    zhash_destroy(clustermap);

    zarray_t *quads = zarray_create(sizeof(struct quad));

    int sz = zarray_size(clusters);
    int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads);
    struct quad_task tasks[sz / chunksize + 1];

    int ntasks = 0;
    for (int i = 0; i < sz; i += chunksize) {
        tasks[ntasks].td = td;
        tasks[ntasks].cidx0 = i;
        tasks[ntasks].cidx1 = imin(sz, i + chunksize);
        tasks[ntasks].h = h;
        tasks[ntasks].w = w;
        tasks[ntasks].quads = quads;
        tasks[ntasks].clusters = clusters;
        tasks[ntasks].im = im;

        workerpool_add_task(td->wp, do_quad_task, &tasks[ntasks]);
        ntasks++;
    }

    workerpool_run(td->wp);

    timeprofile_stamp(td->tp, "fit quads to clusters");

    if (td->debug) {
        FILE *f = fopen("debug_lines.ps", "w");
        fprintf(f, "%%!PS\n\n");

        image_u8_t *im2 = image_u8_copy(im);
        image_u8_darken(im2);
        image_u8_darken(im2);

        // assume letter, which is 612x792 points.
        double scale = fmin(612.0/im->width, 792.0/im2->height);
        fprintf(f, "%.15f %.15f scale\n", scale, scale);
        fprintf(f, "0 %d translate\n", im2->height);
        fprintf(f, "1 -1 scale\n");

        postscript_image(f, im);

        for (int i = 0; i < zarray_size(quads); i++) {
            struct quad *q;
            zarray_get_volatile(quads, i, &q);

            float rgb[3];
            int bias = 100;

            for (int i = 0; i < 3; i++)
                rgb[i] = bias + (random() % (255-bias));

            fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f);
            fprintf(f, "%.15f %.15f moveto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto stroke\n",
                    q->p[0][0], q->p[0][1],
                    q->p[1][0], q->p[1][1],
                    q->p[2][0], q->p[2][1],
                    q->p[3][0], q->p[3][1],
                    q->p[0][0], q->p[0][1]);
        }

        fclose(f);
    }

//        printf("  %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);

/*
        if (td->debug) {
            for (int i = 0; i < 4; i++) {
            int i0 = indices[i];
                int i1 = indices[(i+1)&3];

                if (i1 < i0)
                    i1 += zarray_size(cluster);

                for (int j = i0; j <= i1; j++) {
                    struct pt *p;
                    zarray_get_volatile(cluster, j % zarray_size(cluster), &p);

                    edgeim->buf[p->y*edgeim->stride + p->x] = 30+64*i;
                }
            }
            } */

    unionfind_destroy(uf);

    for (int i = 0; i < zarray_size(clusters); i++) {
        zarray_t *cluster;
        zarray_get(clusters, i, &cluster);
        zarray_destroy(cluster);
    }

    zarray_destroy(clusters);

    image_u8_destroy(edgeim);

    return quads;
}
Example #20
0
loc_t* fit_lines(image_u32_t* im, node_t* n, vx_buffer_t* buf, metrics_t met, loc_t* out)
{
    // usleep(2000000);
    srand(time(NULL));
    // isolate valid entries 
    zarray_t* loc_arr = zarray_create(sizeof(loc_t*));
   
    for(int i = 0; i < im->height; i++) {
        if(n->sides[i].leftmost.x == im->width) continue;        // not apart of blob


        loc_t* loc = malloc(sizeof(loc_t));
        loc->x = n->sides[i].leftmost.x;
        loc->y = n->sides[i].leftmost.y;
        loc->valid = 0;
        zarray_add(loc_arr, &loc);
    }
    for(int i = 0; i < im->height; i++) {
        if(n->sides[i].rightmost.x == -1) continue;

        loc_t* loc = malloc(sizeof(loc_t));
        loc->x = n->sides[i].rightmost.x;
        loc->y = n->sides[i].rightmost.y;
        loc->valid = 0;
        zarray_add(loc_arr, &loc);
    }

    // printf("\n\nall\n");
    // for(int i = 0; i < zarray_size(loc_arr); i++)
    // {
    //     loc_t* p1;
    //     zarray_get(loc_arr, i, &p1);
    //     printf("(%d, %d)\n", p1->x, p1->y);
    // }
    // printf("\n\n");
    int iterations = 0;
    int best_score = 0;
    int lines_found = 0;
    loc_t line_match[8];
    int max_iterations = 500;
    while(lines_found < 2  && zarray_size(loc_arr) > met.num_outliers)      // still a lot of points left 
    {  
        if(iterations > max_iterations) break;
        // reset image and array
        // vx_object_t *vim = vxo_image_from_u32(im, 0, 0);
        // vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, vim));
        add_arr_of_locs_to_buffer(loc_arr, buf, 1.0, vx_black, met);

        int num_outliers = met.num_outliers/met.consensus_accuracy;

        while(best_score < ((zarray_size(loc_arr) - num_outliers)/(4 - lines_found)))
        {
            reset_locs(loc_arr);
            // pick random sample (2 points)
            loc_t* p1;
            loc_t* p2;
            zarray_get(loc_arr, (rand()%zarray_size(loc_arr)), &p1);
            p2 = p1;
            while(p1 == p2)
                zarray_get(loc_arr, (rand()%zarray_size(loc_arr)), &p2);    // don't get same point

            // find consensus score of line from other points 
            // if inside consensus range add 1
            int tmp_score = 0;
            for(int j = 0; j < zarray_size(loc_arr); j++) {
                loc_t* tmp;
                zarray_get(loc_arr, j, &tmp);
                if(fabs(dist_from_point_to_line(p1, p2, tmp)) < (double)met.consensus_accuracy) 
                {

                    tmp->valid = 1;
                    // printf("dist: (%d, %d, %d)  %lf\n", tmp->x, tmp->y, tmp->valid,
                    //         fabs(dist_from_point_to_line(p1, p2, tmp)));
                    tmp_score++;
                }
            }
            // keep best line so far 
            if(tmp_score > best_score) {
                if(lines_found != 0) {      // if 2nd line intersects, throw it out and find another 
                    loc_t intersect = get_line_intersection(line_match[0], line_match[1], *p1, *p2);
                        if(in_range(im, intersect.x, intersect.y)) continue;
                }
                best_score = tmp_score;
                // printf("      score:%d,  %d,  %d  %lf\n", best_score, ((zarray_size(loc_arr)-1)/(4-lines_found)), 
                //                                     zarray_size(loc_arr), 10/met.std_dev_from_square);
                line_match[lines_found*2] = *p1;
                line_match[lines_found*2+1] = *p2;
            }
            iterations++;
            if(iterations > max_iterations) break;
        }
        // loc_t ext_lines[2];
        // extend_lines_to_edge_of_image(im, line_match[lines_found*2], line_match[lines_found*2+1], ext_lines);
        // add_line_to_buffer(im, buf, 2.0, ext_lines[0], ext_lines[1], vx_yellow);
        // delete all points associated with the found line
        zarray_t* endpoints_arr = zarray_create(sizeof(loc_t*));
        for(int i = 0; i < zarray_size(loc_arr); i++) {
            loc_t* tmp;
            zarray_get(loc_arr, i, &tmp);
            // printf("removed: (%d, %d, %d) \n", tmp->x, tmp->y, tmp->valid);
            if(tmp->valid) 
            {
                // add_circle_to_buffer(buf,  1.0, *tmp, vx_red);
                zarray_add(endpoints_arr, &tmp);
                zarray_remove_index(loc_arr, i, 0);
                i--;
            }
        }
        // find endpoints of line 
        loc_t ext_lines[2];
        extend_lines_to_edge_of_image(im, line_match[lines_found*2], line_match[lines_found*2+1], ext_lines);
        line_t endpoints = find_line_endpoints(endpoints_arr, &ext_lines[0], &ext_lines[1]);
        add_circle_to_buffer(buf,  2.0, endpoints.start, vx_red);
        add_circle_to_buffer(buf,  2.0, endpoints.end, vx_red);
        line_match[lines_found*2] = endpoints.start;
        line_match[lines_found*2+1] = endpoints.end;

        lines_found++;
        best_score = 0;
        // vx_buffer_swap(buf);
        // usleep(500000);
    }

    loc_t* ret = calloc(lines_found*2, sizeof(loc_t));
    for(int i = 0; i < lines_found; i++) {
        ret[i*2] = line_match[i*2];
        ret[i*2+1] = line_match[i*2+1]; 
        loc_t ext_lines[2];
        extend_lines_to_edge_of_image(im, line_match[i*2], line_match[i*2+1], ext_lines);
        add_line_to_buffer(im, buf, 2.0, ext_lines[0], ext_lines[1], vx_blue);
    }

    zarray_vmap(loc_arr, free);
    zarray_destroy(loc_arr);
    return(ret);

    // int corners_found = 0;
    // loc_t corners[4];
    // find_corners_from_lines(im,line_match, 4, &corners_found, corners);
    // for(int i = 0; i < corners_found; i++) {
    //     printf("(%d, %d)\n", corners[i].x, corners[i].y);
    //     add_circle_to_buffer(buf, 3.0, corners[i], vx_blue);
    // }

    // printf("(%d, %d) (%d, %d)   %lf\n", line_match[0].x, line_match[0].y, 
    //                                     line_match[1].x, line_match[1].y, 
    //                                     (double)best_score/N);

}
Example #21
0
double* getTag(char* path)
{

    apriltag_family_t *tf = NULL;
    tf = tag36h11_create();

    tf->black_border = 1;
    apriltag_detector_t *td = apriltag_detector_create();
    apriltag_detector_add_family(td, tf);
    td->quad_decimate = 1.0;
    td->quad_sigma = 0.0;
    td->nthreads = 4;
    td->debug = 0;
    td->refine_decode = 0;
    td->refine_pose = 0;
    int quiet = 0;
    int maxiters = 1;
    const int hamm_hist_max = 10;


    int hamm_hist[hamm_hist_max];
    memset(hamm_hist, 0, sizeof(hamm_hist));
    image_u8_t *im = image_u8_create_from_pnm(path);
    if (im == NULL) {
	    printf("couldn't find %s\n", path);
	    return NULL;
    }

    zarray_t *detections = apriltag_detector_detect(td, im);

     nrows = zarray_size(detections);
     ncols = 9;
    
     if (nrows == 0)
	return NULL;

    double* output_matrix = new double[nrows*ncols];

    for (int i = 0; i < zarray_size(detections); i++) {
	    apriltag_detection_t *det;
	    zarray_get(detections, i, &det);


	    output_matrix[ncols*i+0] = det->id;
	    for(int j=0; j<4; j++)
	    {
		output_matrix[ncols*i+ 2*j +1] = det->p[j][0];
		output_matrix[ncols*i+ 2*j +2] = det->p[j][1];
	    }
	    
	    hamm_hist[det->hamming]++;

	    apriltag_detection_destroy(det);
    }

    

    zarray_destroy(detections);
    image_u8_destroy(im);

    // don't deallocate contents of inputs; those are the argv
    apriltag_detector_destroy(td);

    tag36h11_destroy(tf);
    return output_matrix;
}
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;
}
Example #23
0
vx_object_t * vxo_objmtl(const char * obj_filename)
{
    int  texture_flag = 0;

    FILE * fp_obj = fopen(obj_filename, "r");
    if (fp_obj == NULL)
        return NULL;

    #define LNSZ 1024
    char line_buffer[LNSZ];

    // Store 3D vertices by value
    zarray_t * vertices = zarray_create(sizeof(float)*3);
    zarray_t * textures = zarray_create(sizeof(float)*3);
    zarray_t * normals = zarray_create(sizeof(float)*3);

    wav_group_t * cur_group = NULL;
    zarray_t * group_list = zarray_create(sizeof(wav_group_t*));

    zhash_t * mtl_map = NULL; // created on reading mtllib entry
    //zhash_t * obj_map = zhash_create(sizeof(char*), sizeof(vx_object_t*), zhash_str_hash, zhash_str_equals);

    // Read in the entire file, save vertices, and indices for later processing.
    while (1) {
        int eof = fgets(line_buffer, LNSZ, fp_obj) == NULL;

        char * line = str_trim(line_buffer);

        // If possible, batch process the last group
        if (str_starts_with(line, "g ") || eof) {
            if (cur_group != NULL) {
                assert(cur_group->group_idx != NULL);

                zarray_add(group_list, &cur_group);
                cur_group = NULL;
            }
        }

        if (eof)
            break;


        if (str_starts_with(line, "#") || strlen(line) == 0 || !strcmp(line,"\r"))
            continue;

        if (str_starts_with(line, "g ")) {
            assert(mtl_map != NULL);

            char obj_name[LNSZ];
            sscanf(line, "g %s", obj_name);

            cur_group = calloc(1, sizeof(wav_group_t));
            cur_group->group_idx = zarray_create(sizeof(tri_idx_t));

        } else if (str_starts_with(line, "v ")) {
            float vertex[3];
            sscanf(line, "v %f %f %f", &vertex[0], &vertex[1], &vertex[2]);
            zarray_add(vertices, &vertex);
        } else if (str_starts_with(line, "vn ")) {
            float normal[3];
            sscanf(line, "vn %f %f %f", &normal[0], &normal[1], &normal[2]);
            zarray_add(normals, &normal);
        } else if (str_starts_with(line, "vt ")) {
            texture_flag = 1;
            float texture[3];
            sscanf(line, "vt %f %f %f", &texture[0], &texture[1], &texture[2]);
            zarray_add(textures, &texture);
        } else if (str_starts_with(line, "f ")) {
            tri_idx_t idxs;

            if (texture_flag) {
                sscanf(line, "f %d/%d/%d %d/%d/%d %d/%d/%d",
                       &idxs.vIdxs[0], &idxs.tIdxs[0], &idxs.nIdxs[0],
                       &idxs.vIdxs[1], &idxs.tIdxs[1], &idxs.nIdxs[1],
                       &idxs.vIdxs[2], &idxs.tIdxs[2], &idxs.nIdxs[2]);
            }
            else {
                sscanf(line, "f %d//%d %d//%d %d//%d",
                       &idxs.vIdxs[0], &idxs.nIdxs[0],
                       &idxs.vIdxs[1], &idxs.nIdxs[1],
                       &idxs.vIdxs[2], &idxs.nIdxs[2]);
            }

            zarray_add(cur_group->group_idx, &idxs);

        } else if (str_starts_with(line, "usemtl ")) {
            char *mname = calloc(1, sizeof(char)*1024);
            sscanf(line, "usemtl %s", mname);
            zhash_get(mtl_map, &mname, &cur_group->material);
            free(mname);
        } else if (str_starts_with(line, "s ")) {
            // No idea what to do with smoothing instructions
        } else if (str_starts_with(line, "mtllib ")) {
            char * cur_path = strdup(obj_filename);
            const char * dir_name = dirname(cur_path);

            char mtl_basename[LNSZ];
            sscanf(line, "mtllib %s", mtl_basename);

            char mtl_filename[LNSZ];
            sprintf(mtl_filename,"%s/%s", dir_name, mtl_basename);

            mtl_map = load_materials(mtl_filename);
            if (mtl_map == NULL) {
                zarray_destroy(vertices);
                zarray_destroy(normals);
                return NULL; // XXX cleanup!
            }
            free(cur_path);
        } else {
            printf("Did not parse: %s\n", line);

            for (int i = 0; i < strlen(line); i++) {
                printf("0x%x ", (int)line[i]);
            }
            printf("\n");
        }
    }

    if (1) // useful to enable when compensating for model scale
        print_bounds(vertices);

    // Process the model sections in two passes -- first add all the
    // objects which are not transparent. Then render transparent
    // objects after

    vx_object_t * vchain = vxo_chain_create();

    zarray_t * sorted_groups = zarray_create(sizeof(wav_group_t*));
    for (int i = 0, sz = zarray_size(group_list); i < sz; i++) {
        wav_group_t * group = NULL;
        zarray_get(group_list, i, &group);

        // add to front if solid
        if (group->material.d == 1.0f) {
            zarray_insert(sorted_groups, 0, &group);
        } else { // add to back if transparent
            zarray_add(sorted_groups, &group);
        }
    }

    int total_triangles = 0;
    for (int i = 0, sz = zarray_size(sorted_groups); i < sz; i++) {
        wav_group_t * group = NULL;
        zarray_get(sorted_groups, i, &group);

        int ntri =  zarray_size(group->group_idx);

        vx_resc_t * vert_resc = vx_resc_createf(ntri*9);
        vx_resc_t * norm_resc = vx_resc_createf(ntri*9);

        for (int j = 0; j < ntri; j++) {
            tri_idx_t idxs;
            zarray_get(group->group_idx, j, &idxs);

            for (int  i = 0; i < 3; i++) {
                zarray_get(vertices, idxs.vIdxs[i]-1, &((float*)vert_resc->res)[9*j + i*3]);
                zarray_get(normals,  idxs.nIdxs[i]-1, &((float*)norm_resc->res)[9*j + i*3]);
            }
        }

        vx_style_t * sty = vxo_mesh_style_fancy(group->material.Ka, group->material.Kd, group->material.Ks, group->material.d, group->material.Ns, group->material.illum);
        vxo_chain_add(vchain, vxo_mesh(vert_resc, ntri*3, norm_resc, GL_TRIANGLES, sty));

        total_triangles += ntri;
    }

    //Cleanup:
    // 1. Materials, names are by reference, but materials are by value
    zhash_vmap_keys(mtl_map, free);
    zhash_destroy(mtl_map);

    // 2. Geometry
    zarray_destroy(vertices); // stored by value
    zarray_destroy(normals); // stored by value

    // 2b wav_group_t are stored by reference

    zarray_vmap(group_list, wav_group_destroy);
    zarray_destroy(group_list);
    zarray_destroy(sorted_groups); // duplicate list, so don't need to free

    fclose(fp_obj);

    return vchain;
}
Example #24
0
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;
}
Example #25
0
void cam_callback(const sensor_msgs::ImageConstPtr &image,
                  const sensor_msgs::CameraInfoConstPtr &cinfo) {
  // Get camera info
  static bool init_cam = false;
  static cv::Mat K = cv::Mat::zeros(cv::Size(3, 3), CV_64F);
  static cv::Mat D = cv::Mat::zeros(cv::Size(1, 5), CV_64F);

  // Stop if camera not calibrated
  if (cinfo->K[0] == 0.0) throw std::runtime_error("Camera not calibrated.");

  // TODO: convert to function later
  // Assign camera info only once
  if (!init_cam) {
    for (int i = 0; i < 3; ++i) {
      double *pk = K.ptr<double>(i);
      for (int j = 0; j < 3; ++j) {
        pk[j] = cinfo->K[3 * i + j];
      }
    }
    double *pd = D.ptr<double>();
    for (int k = 0; k < 5; k++) {
      pd[k] = cinfo->D[k];
    }
    init_cam = true;
  }

  // use cv_bridge and convert to grayscale image
  cv_bridge::CvImagePtr cv_ptr;
  // use toCvCopy because we will modify the image
  cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::MONO8);

  cv::Mat image_rgb;
  cv::cvtColor(cv_ptr->image, image_rgb, CV_GRAY2RGB);

#if defined(BUILD_UMICH)
  // Use apriltag_umich
  // Currently not using this version
  static april_tag_family_t *tf = tag36h11_create();
  static april_tag_detector_t *td = april_tag_detector_create(tf);

  image_u8_t *im = image_u8_create_from_gray(
      cv_ptr->image.cols, cv_ptr->image.rows, cv_ptr->image.data);
  zarray_t *detections = april_tag_detector_detect(td, im);

  ROS_INFO("Tags detected: %d", zarray_size(detections));

  for (size_t i = 0; i < zarray_size(detections); i++) {
    april_tag_detection_t *det;
    zarray_get(detections, i, &det);

    for (int j = 0; j < 4; j++) {
      const Point2 p = Point2(det->p[j][0], det->p[j][1]);
    }
    april_tag_detection_destroy(det);
  }

  zarray_destroy(detections);
  image_u8_destroy(im);

#elif defined(BUILD_MIT)
  // Use apriltag_mit
  static AprilTags::TagDetector tag_detector(AprilTags::tagCodes36h11);
  std::vector<AprilTags::TagDetection> detections =
      tag_detector.extractTags(cv_ptr->image);

  // Check detection size, only do work if there's tag detected
  if (detections.size()) {
    std::vector<Point2> pi;  // Points in image
    std::vector<Point3> pw;  // Points in world
    for (auto it = detections.begin(); it != detections.end(); it++) {
      const int id = it->id;
      const Point2 c2 = Point2(it->cxy.first, it->cxy.second);

      for (int j = 0; j < 4; j++) {
        const Point2 p2 = Point2(it->p[j].first, it->p[j].second);
        pi.push_back(p2);
        Point3 p3(tagsWorld[id].p[j].x, tagsWorld[id].p[j].y, 0.0);
        pw.push_back(p3);

        // Display tag corners
        cv::circle(image_rgb, p2, 6, colors[j], 2);
      }
      // Display tag id
      std::ostringstream ss;
      ss << id;
      auto color = cv::Scalar(0, 255, 255);
      if (tagsWorld.find(id) != tagsWorld.end()) {
        color = cv::Scalar(255, 255, 0);
      }
      cv::putText(image_rgb, ss.str(), Point2(c2.x - 5, c2.y + 5),
                  cv::FONT_HERSHEY_PLAIN, 2, color, 2);
    }

    // Get pose
    static cv::Mat r = cv::Mat::zeros(cv::Size(1, 3), CV_64F);
    static cv::Mat cTw = cv::Mat::zeros(cv::Size(1, 3), CV_64F);
    cv::Mat wTc(cv::Size(3, 3), CV_64F);
    cv::Mat cRw(cv::Size(3, 3), CV_64F), wRc(cv::Size(3, 3), CV_64F);
    cv::solvePnP(pw, pi, K, D, r, cTw, false);
    cv::Rodrigues(r, cRw);
    wRc = cRw.inv();
    wTc = -wRc * cTw;
    // ROS_INFO("%f, %f, %f", r.at<double>(0,0), r.at<double>(1,0),
    // r.at<double>(2,0));
    cv::Mat q = rodriguesToQuat(r);

    // Publish
    geometry_msgs::PoseStamped pose_cam;
    pose_cam.header.stamp = image->header.stamp;
    pose_cam.header.frame_id = "0";

    double *pt = wTc.ptr<double>();
    pose_cam.pose.position.x = pt[0];
    pose_cam.pose.position.y = pt[1];
    pose_cam.pose.position.z = pt[2];

    double *pq = q.ptr<double>();
    pose_cam.pose.orientation.w = pq[0];
    pose_cam.pose.orientation.x = pq[1];
    pose_cam.pose.orientation.y = pq[2];
    pose_cam.pose.orientation.z = pq[3];

    pose_pub.publish(pose_cam);
  }
#endif

  // Publish image
  cv_bridge::CvImage cv_image(image->header, sensor_msgs::image_encodings::BGR8,
                              image_rgb);
  image_pub.publish(cv_image.toImageMsg());
  // cv::imshow("image", image_rgb);
  // cv::waitKey(1);
}
Example #26
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;
}
Example #27
0
// 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");
    }
}
Example #28
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;
}
Example #29
0
// returns the 35 points associated to the test chart in [x1,y1,x2,y2] 
// format if there are more than 35 points will return NULL
matd_t* build_homography(image_u32_t* im, vx_buffer_t* buf, metrics_t met)
{
    frame_t frame = {{0,0}, {im->width-1, im->height-1},
                        {0,0}, {1,1}};
    int good_size = 0;
    zarray_t* blobs = zarray_create(sizeof(node_t));
    hsv_find_balls_blob_detector(im, frame, met, blobs, buf);

    // remove unqualified blobs
    if(met.qualify) {
        for(int i = 0; i < zarray_size(blobs); i++) {
            node_t n;
            zarray_get(blobs, i, &n);

            if(!blob_qualifies(im, &n, met, buf))
                zarray_remove_index(blobs, i, 0);
        }
    }
    if(zarray_size(blobs) == NUM_TARGETS ||zarray_size(blobs) == NUM_CHART_BLOBS) good_size = 1;

    zarray_sort(blobs, compare);
    int pix_array[zarray_size(blobs)*2];

    // iterate through
    int idx = 0;
    double size = 2.0;
    for(int i = 0; i < zarray_size(blobs); i++) {
        node_t n;
        zarray_get(blobs, i, &n);
        loc_t center = {    .x = n.ave_loc.x/n.num_children,
                            .y = n.ave_loc.y/n.num_children};
        loc_t parent = {    .x = n.id % im->stride,
                            .y = n.id / im->stride};


        if(buf != NULL) {
            add_circle_to_buffer(buf, size, center, vx_maroon);
            // add_circle_to_buffer(buf, size, parent, vx_olive);

            // add_sides_to_buffer(im, buf, 1.0, &n, vx_orange, met);
            loc_t* lp = fit_lines(im, &n, buf, met, NULL);
            if(lp != NULL) {
                // printf("(%d, %d) (%d, %d) (%d, %d) (%d, %d) \n",
                //         lp[0].x, lp[0].y, lp[1].x, lp[1].y, lp[2].x, lp[2].y, lp[3].x, lp[3].y);
                loc_t intersect = get_line_intersection(lp[0], lp[1], lp[2], lp[3]);
                if(in_range(im, intersect.x, intersect.y)) {
                    loc_t ext_lines[2];
                    extend_lines_to_edge_of_image(im, intersect, center, ext_lines);
                    add_line_to_buffer(im, buf, 2.0, ext_lines[0], ext_lines[1], vx_blue);                
                }
                for(int i = 0; i < 4; i++) {
                    pix_array[i*2] = lp[i].x;
                    pix_array[i*2+1] = lp[i].y;
                    add_circle_to_buffer(buf, 3.0, lp[i], vx_orange);
                }
            }



            free(n.sides);

            // loc_t corners[4] = {{n.box.right, n.box.top},
            //                     {n.box.right, n.box.bottom},
            //                     {n.box.left, n.box.bottom},
            //                     {n.box.left, n.box.top}};
            // print extremes of box
            // if(1) {
            //     add_circle_to_buffer(buf, size, corners[0], vx_green);
            //     add_circle_to_buffer(buf, size, corners[1], vx_yellow);
            //     add_circle_to_buffer(buf, size, corners[2], vx_red);
            //     add_circle_to_buffer(buf, size, corners[3], vx_blue);
            //     for(int j = 0; j < 4; j++) {
            //         // add_circle_to_buffer(buf, size, corners[j], vx_maroon);
            //     }
            // }
        }
    }

    matd_t* H;
    H = dist_homography(pix_array, NUM_TARGETS);

    // if(0) {//zarray_size(blobs) == NUM_CHART_BLOBS){
    //     H = dist_homography(pix_array, NUM_CHART_BLOBS);
    // }
    // else if(zarray_size(blobs) == NUM_TARGETS){
    //     H = dist_homography(pix_array, NUM_TARGETS);
    //     if(met.add_lines) connect_lines(blobs, buf);
    // }
    // else {
    //     if(met.dothis)
    //         printf("num figures: %d\n", zarray_size(blobs));
    //     return(NULL);
    // }

    // make projected points
    // project_measurements_through_homography(H, buf, blobs, zarray_size(blobs));
    zarray_destroy(blobs);

    return(H);
}


/*
{ R00, R01, R02, TX,
   R10, R11, R12, TY,
   R20, R21, R22, TZ,
    0, 0, 0, 1 });
*/
double get_rotation(const char* axis, matd_t* H)
{
    double cosine, sine, theta;

    if(strncmp(axis,"x", 1)) {
        cosine = MATD_EL(H, 1, 1);
        sine = MATD_EL(H, 2, 1);
    }
    else if(strncmp(axis,"y", 1)) {
        cosine = MATD_EL(H, 0, 0);
        sine = MATD_EL(H, 0, 2);
    }
    else if(strncmp(axis,"z", 1)) {
        cosine = MATD_EL(H, 0, 0);
        sine = MATD_EL(H, 1, 0);
    }
    else assert(0);

    theta = atan2(sine, cosine);
    return(theta);
}

// if buf is NULL, will not fill with points of the homography
void take_measurements(image_u32_t* im, vx_buffer_t* buf, metrics_t met)
{
    // form homography
    matd_t* H = build_homography(im, buf, met);
    if(H == NULL) return;

    // get model view from homography
    matd_t* Model = homography_to_pose(H, 654, 655, 334, 224);
    // printf("\n");
    // matd_print(H, matrix_format);
    // printf("\n\n");
    // printf("model:\n");
    // matd_print(Model, "%15f");
    // printf("\n\n");
    // matd_print(matd_op("M^-1",Model), matrix_format);
    // printf("\n");
    // extrapolate metrics from model view
    double TX = MATD_EL(Model, 0, 3);
    double TY = MATD_EL(Model, 1, 3);
    double TZ = MATD_EL(Model, 2, 3);

    // double rot_x = get_rotation("x", H);
    // double rot_y = get_rotation("y", H);
    // double rot_z = get_rotation("z", H);

    double cosine = MATD_EL(Model, 0, 0);

    double rot_z = acos(cosine) * 180/1.5 - 180;


    cosine = MATD_EL(Model, 2, 2);
    double rot_x = asin(cosine) * 90/1.3 + 90;

    cosine = MATD_EL(Model, 1, 1);
    double rot_y = asin(cosine);



    char str[200];
    sprintf(str, "<<#00ffff,serif-30>> DIST:%lf  Offset:(%lf, %lf)\n rot: (%lf, %lf, %lf)\n", 
                TZ, TX, TY, rot_x, rot_y, rot_z);
    vx_object_t *text = vxo_text_create(VXO_TEXT_ANCHOR_BOTTOM_LEFT, str); 
    vx_buffer_add_back(buf, vxo_pix_coords(VX_ORIGIN_BOTTOM_LEFT, text));

    // printf("dist: %lf   cos:%lf  angle: %lf\n", TZ, cosine, theta);
}