static render_info_t * render_info_create() { render_info_t * rinfo = calloc(1, sizeof(render_info_t)); rinfo->layers = zarray_create(sizeof(layer_info_t*)); rinfo->camera_positions = zhash_create(sizeof(uint32_t), sizeof(vx_camera_pos_t *), zhash_uint32_hash, zhash_uint32_equals); rinfo->layer_positions = zhash_create(sizeof(uint32_t), sizeof(uint32_t *), zhash_uint32_hash, zhash_uint32_equals); return rinfo; }
vx_resc_manager_t * vx_resc_manager_create(vx_display_t * disp) { vx_resc_manager_t * mgr = calloc(1, sizeof(vx_resc_manager_t)); mgr->disp = disp; mgr->allLiveSets = zhash_create(sizeof(uint32_t), sizeof(zhash_t*), zhash_uint32_hash, zhash_uint32_equals); mgr->remoteResc = zhash_create(sizeof(uint64_t), sizeof(vx_resc_t*), zhash_uint64_hash, zhash_uint64_equals); return mgr; }
getopt_t * getopt_create (void) { getopt_t *gopt = calloc (1, sizeof(*gopt)); gopt->lopts = zhash_create (sizeof(char*), sizeof(getopt_option_t*), zhash_str_hash, zhash_str_equals); gopt->sopts = zhash_create (sizeof(char*), sizeof(getopt_option_t*), zhash_str_hash, zhash_str_equals); gopt->options = zarray_create (sizeof(getopt_option_t*)); gopt->extraargs = zarray_create (sizeof(char*)); return gopt; }
static state_t * state_create(vx_display_t * super) { state_t * state = calloc(1, sizeof(state_t)); state->super = super; state->glrend = vx_gl_renderer_create(); // because the resource manager is called in send_codes(), and will itself call send_codes() again pthread_mutexattr_t mutexAttr; pthread_mutexattr_init(&mutexAttr); pthread_mutexattr_settype(&mutexAttr, PTHREAD_MUTEX_RECURSIVE); pthread_mutex_init(&state->mutex, &mutexAttr); state->listeners = zarray_create(sizeof(vx_display_listener_t*)); pthread_mutex_init(&state->listener_mutex, NULL); state->rendering = 1; state->target_frame_rate = 30.0f; // XXX state->buffer_manager = vx_gtk_buffer_manager_create(state->super); state->movie_pending = zarray_create(sizeof(movie_frame_t*)); pthread_mutex_init(&state->movie_mutex, NULL); pthread_cond_init(&state->movie_cond, NULL); pthread_create(&state->movie_thread, NULL, movie_run, state); state->layer_info_map = zhash_create(sizeof(uint32_t), sizeof(layer_info_t*), zhash_uint32_hash, zhash_uint32_equals); return state; }
static state_t * state_create() { state_t * state = calloc(1, sizeof(state_t)); state->running = 1; state->world = vx_world_create(); state->world2 = vx_world_create(); state->world3 = vx_world_create(); state->layers = zhash_create(sizeof(vx_display_t*), sizeof(vx_layer_t*), zhash_ptr_hash, zhash_ptr_equals); // Setup event callbacks. { state->veh.dispatch_order = -10; state->veh.touch_event = touch_event; state->veh.mouse_event = mouse_event; state->veh.key_event = key_event; state->veh.destroy = nodestroy; state->veh.impl = state; } // Setup event callbacks. { state->cl.camera_changed = camera_changed; state->cl.destroy = nodestroy; state->cl.impl = state; } return state; }
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; }
eecs467_default_implementation_t * eecs467_default_implementation_create (vx_world_t *world, vx_event_handler_t *vxeh) { eecs467_default_implementation_t *impl = calloc (1, sizeof(*impl)); impl->world = world; impl->vxeh = vxeh; impl->layers = zhash_create (sizeof(vx_display_t *), sizeof(vx_layer_t *), zhash_ptr_hash, zhash_ptr_equals); pthread_mutex_init (&impl->mutex, NULL); return impl; }
static state_t * state_create() { state_t * state = calloc(1, sizeof(state_t)); state->running = 1; state->app.impl=state; state->app.display_started=display_started; state->app.display_finished=display_finished; state->world = vx_world_create(); state->layers = zhash_create(sizeof(vx_display_t*), sizeof(vx_layer_t*), zhash_ptr_hash, zhash_ptr_equals); pthread_mutex_init (&state->mutex, NULL); return state; }
vx_world_t * vx_world_create() { vx_world_t *world = malloc(sizeof(vx_world_t)); world->worldID = xxxAtomicID++; world->buffer_map = zhash_create(sizeof(char*), sizeof(vx_buffer_t*), zhash_str_hash, zhash_str_equals); world->listeners = zarray_create(sizeof(vx_world_listener_t*)); pthread_mutex_init(&world->buffer_mutex, NULL); pthread_mutex_init(&world->listener_mutex, NULL); pthread_mutex_init(&world->queue_mutex, NULL); pthread_cond_init(&world->queue_cond, NULL); world->listener_queue = zarray_create(sizeof(vx_world_listener_t*)); world->buffer_queue = zarray_create(sizeof(char*)); world->process_running = 1; pthread_create(&world->process_thread, NULL, run_process, world); return world; }
vx_gtk_buffer_manager_t * vx_gtk_buffer_manager_create(vx_display_t * disp) { vx_gtk_buffer_manager_t * man = calloc(1, sizeof(vx_gtk_buffer_manager_t)); man->disp = disp; man->layers = zhash_create(sizeof(uint32_t), sizeof(layer_info_t*), zhash_uint32_hash, zhash_uint32_equals); pthread_cond_init(&man->cond, NULL); pthread_mutex_init(&man->mutex, NULL); pthread_create(&man->thread, NULL, layout_thread, man); man->window = gtk_window_new(GTK_WINDOW_TOPLEVEL); gtk_window_set_default_size (GTK_WINDOW (man->window), 400, 600); // tall g_signal_connect_swapped(man->window, "delete-event", G_CALLBACK(gtk_widget_hide_on_delete), GTK_WIDGET(man->window)); return man; }
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); }
// Call this only from the serialization thread // static void delayed_swap(vx_buffer_t * buffer) { if (!buffer->front_objs) return; // buffer has not yet finished initialization if (verbose) printf("DBG: swap %s\n", buffer->name); pthread_mutex_lock(&buffer->mutex); { // clear existing front buffer->front_codes->pos = 0; // reset // *+*+ corresponding decrement (if keeping up) zarray_vmap(buffer->front_objs, vx_object_dec_destroy); zarray_clear(buffer->front_objs); // swap out the pending objects without tying up this mutex for too long zarray_t * tmp = buffer->front_objs; buffer->front_objs = buffer->pending_objs; buffer->pending_objs = tmp; // empty } pthread_mutex_unlock(&buffer->mutex); vx_world_t * world = buffer->world; zhash_t * old_resources = buffer->front_resc; // Serialize each object into resources and opcodes vx_code_output_stream_t * codes = buffer->front_codes; codes->write_uint32(codes, OP_BUFFER_CODES); codes->write_uint32(codes, world->worldID); codes->write_str(codes, buffer->name); codes->write_uint32(codes, buffer->draw_order); // XXX We should move this op code elsewhere zhash_t * resources = zhash_create(sizeof(uint64_t),sizeof(vx_resc_t*), zhash_uint64_hash, zhash_uint64_equals); for (int i = 0; i < zarray_size(buffer->front_objs); i++) { vx_object_t * obj = NULL; zarray_get(buffer->front_objs, i, &obj); obj->append(obj, resources, codes); } // *&&* we will hold on to these until next swap() call zhash_vmap_values(resources, vx_resc_inc_ref); zhash_t * all_resources = zhash_copy(resources); addAll(all_resources, old_resources); // Claim use of the union of the last frame, and the current frame // this avoids a race condition where another buffer (maybe in another world) // could force deallocation of some resources before the new render codes arrive if (verbose) printf("DBG: claim union via codes\n"); if (verbose > 1) { printf(" claimed %d IDS ", zhash_size(all_resources)); zhash_vmap_values(all_resources, _print_id); printf("\n"); } send_buffer_resource_codes(buffer, all_resources); if (verbose) printf("DBG: Send actual resources\n"); if (verbose > 1) { printf(" send %d IDS ", zhash_size(all_resources)); zhash_vmap_values(all_resources, _print_id); printf("\n"); } notify_listeners_send_resources(world, all_resources); if (verbose) printf("DBG: send render codes\n"); notify_listeners_codes(world, codes); // Claim the actual set of resources which are in use now, which may result in some // resources being deleted from the gl context if (verbose) printf("DBG: reduce claim to actual in use\n"); if (verbose > 1) { printf(" actual %d IDS ", zhash_size(resources)); zhash_vmap_values(resources, _print_id); printf("\n\n"); } send_buffer_resource_codes(buffer, resources); buffer->front_resc = resources; // set of current resources zhash_destroy(all_resources); // *&&* corresponding decrement of resources zhash_vmap_values(old_resources, vx_resc_dec_destroy); zhash_destroy(old_resources); }
// 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"); } }
// 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; }
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); }
int main(int argc, char ** argv) { eecs467_init(argc, argv); state_t * state = (state_t*) calloc(1, sizeof(state_t)); global_state = state; state->gopt = getopt_create(); state->app.display_finished = display_finished; state->app.display_started = display_started; state->app.impl = state; state->update_arm_cont = 0; state->update_arm = 0; state->arm = new RexArm(); state->body = new Body(); state->ds = new DataSmoother(0.4, 0.3, 0, 0); state->running = 1; state->set_cbs = 0; state->controlBoxColor[GRIPPER] = vx_green; state->controlBoxColor[WRIST] = vx_yellow; state->controlBoxColor[ARM] = vx_orange; state->controlBoxColor[ROTATE] = vx_red; for (int i = 0; i < NUM_CONTROL_BOXES; i++) { state->controlBoxes[i] = new BoundingBox(); state->controlBoxes[i]->setDimensions(CB_WIDTH, CB_HEIGHT, CB_DEPTH); } lcm_t * lcm = lcm_create (NULL); state->lcm = lcm; BoundingBox floorBoard, base; floorBoard.setPosition(0, 0, 0); floorBoard.setDimensions(100, 100, 10); state->cfs.addBoundingBox(&floorBoard); base.setPosition(0, 0, 4); base.setDimensions(7, 7, 8); state->cfs.addBoundingBox(&base); //signal(SIGINT, terminal_signal_handler); pthread_mutex_init(&state->layer_mutex, NULL); pthread_mutex_init(&state->lcm_mutex, NULL); pthread_mutex_init(&state->running_mutex, NULL); pthread_mutex_init(&state->fsm_mutex, NULL); state->layer_map = zhash_create(sizeof(vx_display_t*), sizeof(vx_layer_t*), zhash_uint64_hash, zhash_uint64_equals); getopt_add_bool(state->gopt, 'h', "help", 0, "Show this help"); //getopt_add_bool(state->gopt, 'v', "verbose", 0, "Show extra debugging output"); //getopt_add_int (state->gopt, 'l', "limitKBs", "-1", "Remote display bandwidth limit. < 0: unlimited."); //getopt_add_double (state->gopt, 'd', "decimate", "0", "Decimate image by this amount before showing in vx"); if (!getopt_parse(state->gopt, argc, argv, 0) || getopt_get_bool(state->gopt,"help")) { getopt_do_usage(state->gopt); exit(-1); } pthread_create(&state->lcm_handle_thread, NULL, lcm_handle_loop, state); //pthread_create(&state->gui_thread, NULL, gui_create, state); pthread_create(&state->arm_commander_thread, NULL, arm_commander, state); pthread_create(&state->fsm_thread, NULL, FSM, state); //pthread_join(state->gui_thread, NULL); gui_create(state); printf("after gui_create\n"); // clean up delete state->arm; delete state->body; delete state->ds; for (int i = 0; i < NUM_CONTROL_BOXES; i++) { delete state->controlBoxes[i]; } vx_global_destroy(); getopt_destroy(state->gopt); printf("Exited Cleanly!\n"); return 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; }