static void process_layer(state_t * state, vx_code_input_stream_t * cins)
{
// update (or create) the layer_info_t
int layer_id = cins->read_uint32(cins);
int world_id = cins->read_uint32(cins);
int draw_order = cins->read_uint32(cins);
int bg_color = cins->read_uint32(cins);
layer_info_t * linfo = NULL;
zhash_get(state->layer_info_map, &layer_id, &linfo);
if (linfo == NULL) {
linfo = calloc(1,sizeof(layer_info_t));
linfo->cam_mgr = default_cam_mgr_create();
linfo->event_handler = default_event_handler_create(linfo->cam_mgr, UI_ANIMATE_MS);
linfo->vp_mgr = vx_viewport_mgr_create();
zhash_put(state->layer_info_map, &layer_id, &linfo, NULL, NULL);
}
linfo->layer_id = layer_id;
linfo->draw_order = draw_order;
bg_color+=world_id; // XXX unused variable
}
vx_buffer_t * vx_world_get_buffer(vx_world_t * world, const char * name)
{
vx_buffer_t * buffer = NULL;
pthread_mutex_lock(&world->buffer_mutex);
zhash_get(world->buffer_map, &name, &buffer);
if (buffer == NULL) {
buffer = calloc(1, sizeof(vx_buffer_t));
buffer->name = strdup(name);
buffer->world = world;
buffer->draw_order = 0;
buffer->back_objs = zarray_create(sizeof(vx_object_t*));
buffer->pending_objs = zarray_create(sizeof(vx_object_t*));
buffer->front_objs = zarray_create(sizeof(vx_object_t*));
buffer->front_resc = zhash_create(sizeof(uint64_t), sizeof(vx_resc_t*), zhash_uint64_hash, zhash_uint64_equals);
buffer->front_codes = vx_code_output_stream_create(128);
pthread_mutex_init(&buffer->mutex, NULL);
vx_buffer_t * oldBuffer= NULL;
zhash_put(buffer->world->buffer_map, &buffer->name, &buffer, NULL, &oldBuffer);
assert(oldBuffer == NULL);
}
pthread_mutex_unlock(&world->buffer_mutex);
return buffer;
}
void
eecs467_default_display_started (vx_application_t *app, vx_display_t *disp)
{
eecs467_default_implementation_t *impl = app->impl;
vx_layer_t *layer = vx_layer_create (impl->world);
vx_layer_set_background_color (layer, vx_black);
vx_layer_set_display (layer, disp);
pthread_mutex_lock (&impl->mutex);
// store a reference to the world and layer that we associate with each vx_display_t
zhash_put (impl->layers, &disp, &layer, NULL, NULL);
pthread_mutex_unlock (&impl->mutex);
if (impl->vxeh != NULL)
vx_layer_add_event_handler (layer, impl->vxeh);
vx_layer_camera_op (layer, OP_PROJ_PERSPECTIVE);
float eye[3] = { 0, 0, 1 };
float lookat[3] = { 0, 0, 0 };
float up[3] = { 0, 1, 0 };
vx_layer_camera_lookat (layer, eye, lookat, up, 1);
vx_code_output_stream_t *couts = vx_code_output_stream_create (128);
couts->write_uint32 (couts, OP_LAYER_CAMERA);
couts->write_uint32 (couts, vx_layer_id (layer));
couts->write_uint32 (couts, OP_INTERFACE_MODE);
couts->write_float (couts, 2.5f);
disp->send_codes (disp, couts->data, couts->pos);
vx_code_output_stream_destroy (couts);
}
void process_buffer(vx_gtk_buffer_manager_t * man, vx_code_input_stream_t * cins)
{
int world_id = cins->read_uint32(cins);
const char * name = cins->read_str(cins);
int draw_order = cins->read_uint32(cins);
// find a matching layer:
int update = 0;
pthread_mutex_lock(&man->mutex);
{
layer_info_t * linfo = NULL;
int layer_id = 0;
zhash_iterator_t itr;
zhash_iterator_init(man->layers, &itr);
while(zhash_iterator_next(&itr, &layer_id, &linfo)) {
if (linfo->world_id == world_id) {
break;
}
}
if (linfo == NULL) { // XXX Assert?
pthread_mutex_unlock(&man->mutex);
return;
}
// Now ensure there's a buffer_info_t in this layer.
buffer_info_t * buffer = NULL;
int success = zhash_get(linfo->buffers, &name, &buffer);
// buffer doesn't exist yet?
if (!success) {
buffer = calloc(1, sizeof(buffer_info_t));
buffer->layer_id = linfo->layer_id;
buffer->name = strdup(name);
buffer->enabled = 1;
buffer->draw_order = draw_order;
buffer->man = man;
zhash_put(linfo->buffers, &buffer->name, &buffer, NULL, NULL);
update = 1;
}
// draw order changed?
if (success && draw_order != buffer->draw_order) {
assert(buffer != NULL);
buffer->draw_order = draw_order;
update = 1;
}
}
pthread_mutex_unlock(&man->mutex);
if (update == 1)
queue_update_view(man);
}
// 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;
}
// Add all elements in B to A
static void addAll(zhash_t * A, zhash_t * B)
{
zhash_iterator_t itr;
zhash_iterator_init(B, &itr);
uint64_t id = -1;
void * value;
while(zhash_iterator_next(&itr, &id, &value)) {
if (!zhash_contains(A, &id))
zhash_put(A, &id, &value, NULL, NULL);
}
}
static void display_started(vx_application_t * app, vx_display_t * disp)
{
state_t * state = app->impl;
vx_layer_t * layer = vx_layer_create(state->world);
vx_layer_set_display(layer, disp);
pthread_mutex_lock(&state->mutex);
// store a reference to the world and layer that we associate with each vx_display_t
zhash_put(state->layers, &disp, &layer, NULL, NULL);
pthread_mutex_unlock(&state->mutex);
}
void
getopt_add_string (getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help)
{
char sname[2];
sname[0] = sopt;
sname[1] = 0;
char *sname_ptr = (char*) &sname;
if (strlen (lname) < 1) { // must have long name
fprintf (stderr, "getopt_add_string(): must supply option name\n");
exit (EXIT_FAILURE);
}
if (sopt == '-') { // short name cannot be '-' (no way to reference)
fprintf (stderr, "getopt_add_string(): invalid option character: '%c'\n", sopt);
exit (EXIT_FAILURE);
}
if (zhash_contains (gopt->lopts, &lname)) {
fprintf (stderr, "getopt_add_string(): duplicate option name: --%s\n", lname);
exit (EXIT_FAILURE);
}
if (sopt != '\0' && zhash_contains (gopt->sopts, &sname_ptr)) {
fprintf (stderr, "getopt_add_string(): duplicate option: -%s ('%s')\n", sname, lname);
exit (EXIT_FAILURE);
}
getopt_option_t *goo = calloc (1, sizeof(*goo));
goo->sname = strdup (sname);
goo->lname = strdup (lname);
goo->svalue = strdup(def);
goo->type = GOO_STRING_TYPE;
goo->help = strdup (help);
zhash_put (gopt->lopts, &goo->lname, &goo, NULL, NULL);
zhash_put (gopt->sopts, &goo->sname, &goo, NULL, NULL);
zarray_add (gopt->options, &goo);
}
void process_layer(vx_gtk_buffer_manager_t * man, vx_code_input_stream_t * cins)
{
int layer_id = cins->read_uint32(cins);
int world_id = cins->read_uint32(cins);
int draw_order = cins->read_uint32(cins);
/* int bg_color = cins->read_uint32(cins); */
int update = 0;
pthread_mutex_lock(&man->mutex);
{
layer_info_t * linfo = NULL;
int success = zhash_get(man->layers, &layer_id, &linfo);
if (!success) {
linfo = calloc(1,sizeof(layer_info_t));
linfo->layer_id = layer_id;
linfo->world_id = world_id;
linfo->buffers = zhash_create(sizeof(char*), sizeof(buffer_info_t*), zhash_str_hash, zhash_str_equals);
zhash_put(man->layers, &layer_id, &linfo, NULL, NULL);
update = 1;
}
if (success && draw_order != linfo->draw_order) {
assert(linfo != NULL);
linfo->draw_order = draw_order;
update = 1;
}
assert(linfo->world_id == world_id);
}
pthread_mutex_unlock(&man->mutex);
if (update == 1)
queue_update_view(man);
}
// 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");
}
}
// 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);
}
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;
}
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);
}
static void display_started(vx_application_t * app, vx_display_t * disp)
{
state_t * state = app->impl;
{
vx_layer_t * layer = vx_layer_create(state->world);
vx_layer_set_display(layer, disp);
//vx_layer_camera_op(layer, OP_PROJ_ORTHO);
if (1) {
float view_rel[] = {0.0, 0.0, 1.0, 1.0};
vx_layer_set_viewport_rel(layer, view_rel);
}
if (1) {
float eye3[] = {CAM_RADIUS,0,45.0f};
float lookat3[] = {CAM_RADIUS,0,0.0f};
float up3[] = {0,1,0};
vx_layer_camera_lookat(layer, eye3, lookat3, up3, 0);
}
if (1) {
float xy0[] = {-10,-10};
float xy1[] = {10,10};
vx_layer_camera_fit2D(layer, xy0, xy1, 0);
}
pthread_mutex_lock(&state->mutex);
// store a reference to the world and layer that we associate with each vx_display_t
zhash_put(state->layers, &disp, &layer, NULL, NULL);
pthread_mutex_unlock(&state->mutex);
// must redraw after each new display connects
draw(state, state->world);
}
{
vx_layer_t * layer = vx_layer_create(state->world2);
vx_layer_set_display(layer, disp);
vx_layer_camera_op(layer, OP_PROJ_ORTHO);
{
vx_layer_set_viewport_abs(layer, OP_ANCHOR_TOP_RIGHT, 100, 100);
}
{
float eye3[] = {10.0f,10.0f,100.0f};
float lookat3[] = {10.0f,10.0f,0.0f};
float up3[] = {0,1,0};
vx_layer_camera_lookat(layer, eye3, lookat3, up3, 0);
}
{
float xy0[] = {-10,-10};
float xy1[] = {10,10};
vx_layer_camera_fit2D(layer, xy0, xy1, 0);
}
vx_layer_add_event_handler(layer, &state->veh);
vx_layer_add_camera_listener(layer, &state->cl);
/*
pthread_mutex_lock(&state->mutex);
// store a reference to the world and layer that we associate with each vx_display_t
zhash_put(state->layers, &disp, &layer, NULL, NULL);
pthread_mutex_unlock(&state->mutex);
*/
// must redraw after each new display connects
draw(state, state->world2);
}
if (0) {
vx_layer_t * layer = vx_layer_create(state->world3);
vx_layer_set_display(layer, disp);
vx_layer_set_background_color(layer, vx_green);
// Manually send the codes for animating a layer (not in
// vx_layer_api yet):
{
float viewport[] = {0, 0.5, 0.5, 0.5};
vx_code_output_stream_t * couts = vx_code_output_stream_create(128);
couts->write_uint32(couts, OP_LAYER_VIEWPORT_REL);
couts->write_uint32(couts, vx_layer_id(layer));
for (int i = 0; i < 4; i++)
couts->write_float(couts, viewport[i]);
couts->write_uint32(couts, 2000); // animate over 2 seconds
disp->send_codes(disp, couts->data, couts->pos);
vx_code_output_stream_destroy(couts);
}
draw(state, state->world3); // XXX Why is this required to get
// the layer to show up?
}
}
// returns a hash <char*, material_t>, where keys and values are owned
// by the map, so be sure to free strings and dec ref the styles
static zhash_t * load_materials(const char * mtl_filename)
{
FILE * fp_mtl = fopen(mtl_filename, "r");
if (fp_mtl == NULL)
return NULL;
#define LNSZ 1024
char line_buffer[LNSZ];
// store materials by value
zhash_t * mat_map = zhash_create(sizeof(char*), sizeof(material_t), zhash_str_hash, zhash_str_equals);
char cur_name[LNSZ];
material_t cur_material;
memset(&cur_material,0, sizeof(cur_material));
cur_material.illum = -1;
// We commit to reading the
while (1) {
int eof = fgets(line_buffer, LNSZ, fp_mtl) == NULL;
char * line = str_trim(line_buffer);
// If possible, commit the old material
if (str_starts_with(line, "newmtl") || eof) {
if (cur_material.illum >= 0) {
char * key = strdup(cur_name);
char * oldkey = NULL;
material_t oldmat;
zhash_put(mat_map, &key, &cur_material, &oldkey, & oldmat);
assert(oldkey == NULL);
assert(cur_material.illum <= 2 && "can't handle anything higher than illum=2");
}
}
if (eof)
break;
if (str_starts_with(line, "#") || strlen(line) == 0 || !strcmp(line, "\r"))
continue;
if (str_starts_with(line, "newmtl")) {
sscanf(line, "newmtl %s", cur_name);
} else if (str_starts_with(line, "Ns")) {
sscanf(line, "Ns %f", &cur_material.Ns);
} else if (str_starts_with(line, "Ni")) {
sscanf(line, "Ni %f", &cur_material.Ni);
} else if (str_starts_with(line, "d") || str_starts_with(line, "Tr")) {
sscanf(line, "%*s %f", &cur_material.d);
} else if (str_starts_with(line, "Tr")) {
sscanf(line, "Tr %f", &cur_material.Tr);
} else if (str_starts_with(line, "Tf")) {
sscanf(line, "Tf %f %f %f", &cur_material.Tf[0],&cur_material.Tf[1],&cur_material.Tf[2]);
}else if (str_starts_with(line, "illum")) {
sscanf(line, "illum %d", &cur_material.illum);
} else if (str_starts_with(line, "Ka")) {
sscanf(line, "Ka %f %f %f", &cur_material.Ka[0],&cur_material.Ka[1],&cur_material.Ka[2]);
} else if (str_starts_with(line, "Kd")) {
sscanf(line, "Kd %f %f %f", &cur_material.Kd[0],&cur_material.Kd[1],&cur_material.Kd[2]);
} else if (str_starts_with(line, "Ks")) {
sscanf(line, "Ks %f %f %f", &cur_material.Ks[0],&cur_material.Ks[1],&cur_material.Ks[2]);
} else if (str_starts_with(line, "Ke")) {
sscanf(line, "Ke %f %f %f", &cur_material.Ke[0],&cur_material.Ke[1],&cur_material.Ke[2]);
} else {
printf("Did not parse: %s\n", line);
for (int i = 0; i < strlen(line); i++) {
printf("0x%x ", (int)line[i]);
}
printf("\n");
}
}
fclose(fp_mtl);
return mat_map;
}
