int queue_insert_multiple_noorder(void) { // This function exercises the insert bubbling code paths priority_queue_t queue = priority_queue_create(realloc, 0); if (!queue) return 1; if (priority_queue_insert(queue, (void*)34567, 3) != 0) return 1; if (priority_queue_insert(queue, (void*)12345, 1) != 0) return 1; if (priority_queue_insert(queue, (void*)45678, 4) != 0) return 1; if (priority_queue_insert(queue, (void*)23456, 2) != 0) return 1; if (priority_queue_insert(queue, (void*)56789, 5) != 0) return 1; if (priority_queue_get_obj(queue) != (void*)12345) return 1; if (priority_queue_get_obj(queue) != (void*)23456) return 1; if (priority_queue_get_obj(queue) != (void*)34567) return 1; if (priority_queue_get_obj(queue) != (void*)45678) return 1; if (priority_queue_get_obj(queue) != (void*)56789) return 1; if (priority_queue_peek_pri(queue) != -1) return 1; priority_queue_free(queue, free); return 0; }
int queue_insert_multiple_inorder(void) { // This function happens to exercise all of the remove bubbling code paths // (no children, 1 child, 2 children) priority_queue_t queue = priority_queue_create(realloc, 0); if (!queue) return 1; if (priority_queue_insert(queue, (void*)12345, 1) != 0) return 1; if (priority_queue_insert(queue, (void*)23456, 2) != 0) return 1; if (priority_queue_insert(queue, (void*)34567, 3) != 0) return 1; if (priority_queue_insert(queue, (void*)45678, 4) != 0) return 1; if (priority_queue_insert(queue, (void*)56789, 5) != 0) return 1; if (priority_queue_get_obj(queue) != (void*)12345) return 1; if (priority_queue_get_obj(queue) != (void*)23456) return 1; if (priority_queue_get_obj(queue) != (void*)34567) return 1; if (priority_queue_get_obj(queue) != (void*)45678) return 1; if (priority_queue_get_obj(queue) != (void*)56789) return 1; if (priority_queue_peek_pri(queue) != -1) return 1; priority_queue_free(queue, free); return 0; }
char *test_insert() { priority_queue_p queue = priority_queue_create(sizeof(int), compare_intp); priority_queue_insert(queue, test_data(10)); mu_assert(*(int*)vector_get(queue->vector, 0) == 10, "should have set first item"); priority_queue_free(queue); return NULL; }
static void free_team (struct gomp_team *team) { #ifndef HAVE_SYNC_BUILTINS gomp_mutex_destroy (&team->work_share_list_free_lock); #endif gomp_barrier_destroy (&team->barrier); gomp_mutex_destroy (&team->task_lock); priority_queue_free (&team->task_queue); free (team); }
char *test_create() { priority_queue_p queue = priority_queue_create(sizeof(int), compare_intp); mu_assert(queue != NULL, "should have created queue"); mu_assert(queue->vector != NULL, "should have created vector"); mu_assert(queue->comparator != NULL, "should have set comparator"); priority_queue_free(queue); return NULL; }
int queue_insert_one(void) { priority_queue_t queue = priority_queue_create(realloc, 0); if (!queue) return 1; if (priority_queue_insert(queue, (void*)12345, 1) != 0) return 1; if (priority_queue_peek_pri(queue) != 1) return 1; if (priority_queue_peek_obj(queue) != (void*)12345) return 1; priority_queue_free(queue, free); return 0; }
int queue_insert_resize(void) { priority_queue_t queue = priority_queue_create(realloc, 1); if (!queue) return 1; if (priority_queue_insert(queue, (void*)34567, 3) != 0) return 1; if (priority_queue_insert(queue, (void*)12345, 1) != 0) return 1; if (priority_queue_insert(queue, (void*)45678, 4) != 0) return 1; if (priority_queue_insert(queue, (void*)23456, 2) != 0) return 1; if (priority_queue_insert(queue, (void*)56789, 5) != 0) return 1; if (priority_queue_get_obj(queue) != (void*)12345) return 1; if (priority_queue_get_obj(queue) != (void*)23456) return 1; if (priority_queue_get_obj(queue) != (void*)34567) return 1; if (priority_queue_get_obj(queue) != (void*)45678) return 1; if (priority_queue_get_obj(queue) != (void*)56789) return 1; if (priority_queue_peek_pri(queue) != -1) return 1; priority_queue_free(queue, free); return 0; }
int gotcha_diag(diagnostic_problem problem, const_tv_term alpha) { priority_queue opened; gotcha_node initial; dnf_tree consistent_input; tv_term_list_list systems; unsigned int old_diagnoses; int terminate = 0; int result = 1; diagnostic_problem_reset(problem); sort_int_list(alpha->neg); sort_int_list(alpha->pos); start_stopwatch("observation filtering time"); consistent_input = dnf_tree_filter(dnf_tree_copy(problem->u.tv_tdnf_sd.model), alpha); stop_stopwatch("observation filtering time"); systems = new_tv_term_list_list(); get_systems(consistent_input, systems); /* Start the timer. */ start_stopwatch("search time"); opened = priority_queue_new(cmp_nodes, (priority_queue_element_destroy_func_t)gotcha_node_free, (void *)systems->sz); initial = gotcha_node_new(systems->sz); priority_queue_push(opened, initial); increase_int_counter("pushed"); maximize_int_counter("max", 1); while (!terminate && !priority_queue_empty(opened)) { gotcha_node current = priority_queue_pop(opened); candidate_found(current->cardinality); if (is_terminate()) { gotcha_node_free(current); break; } if (current->depth != 0) { /* The root node has no siblings. */ if (!enqueue_sibling(problem, opened, current, systems)) { result = 0; gotcha_node_free(current); break; } } if (is_diagnosis(current, systems)) { tv_term diagnosis = build_term(current, systems); if (NULL == diagnosis) { result = 0; gotcha_node_free(current); break; } old_diagnoses = problem->diagnoses->sz; if (!add_diagnosis_from_tv_term(problem, diagnosis, &terminate, 1)) { result = 0; rfre_tv_term(diagnosis); gotcha_node_free(current); break; } if (cones != NULL && problem->diagnoses->sz - 1 == old_diagnoses) { assert(problem->diagnoses->sz > 0); /* @todo: Fix the termination here. */ expand_cone_exhaustive(cones, alpha, problem->diagnoses->arr[problem->diagnoses->sz - 1]); } rfre_tv_term(diagnosis); gotcha_node_free(current); } else { if (!enqueue_next_best_state(problem, opened, current, systems)) { result = 0; gotcha_node_free(current); break; } } } priority_queue_free(opened); fre_tv_term_list_list(systems); /* Stop the timer. */ stop_stopwatch("search time"); dnf_tree_free(consistent_input); return result; }
void carmen_localize_ackerman_initialize_particles_uniform(carmen_localize_ackerman_particle_filter_p filter, carmen_robot_ackerman_laser_message *laser, carmen_localize_ackerman_map_p map) { priority_queue_p queue = priority_queue_init(filter->param->num_particles); double *laser_x, *laser_y; int i, j, x_l, y_l; float angle, prob, ctheta, stheta; carmen_point_t point; queue_node_p mark; int *beam_valid; /* compute the correct laser_skip */ if (filter->param->laser_skip <= 0) { filter->param->laser_skip = floor(filter->param->integrate_angle / laser->config.angular_resolution); } fprintf(stderr, "\rDoing global localization... (%.1f%% complete)", 0.0); filter->initialized = 0; /* copy laser scan into temporary memory */ laser_x = (double *)calloc(laser->num_readings, sizeof(double)); carmen_test_alloc(laser_x); laser_y = (double *)calloc(laser->num_readings, sizeof(double)); carmen_test_alloc(laser_y); beam_valid = (int *)calloc(laser->num_readings, sizeof(int)); carmen_test_alloc(beam_valid); for(i = 0; i < laser->num_readings; i++) { if (laser->range[i] < laser->config.maximum_range && laser->range[i] < filter->param->max_range) beam_valid[i] = 1; else beam_valid[i] = 0; } /* do all calculations in map coordinates */ for(i = 0; i < laser->num_readings; i++) { angle = laser->config.start_angle + i * laser->config.angular_resolution; laser_x[i] = (filter->param->front_laser_offset + laser->range[i] * cos(angle)) / map->config.resolution; laser_y[i] = (laser->range[i] * sin(angle)) / map->config.resolution; } for(i = 0; i < filter->param->global_test_samples; i++) { if(i % 10000 == 0) { fprintf(stderr, "\rDoing global localization... (%.1f%% complete)", i / (double)filter->param->global_test_samples * 100.0); carmen_ipc_sleep(0.001); } do { point.x = carmen_uniform_random(0, map->config.x_size - 1); point.y = carmen_uniform_random(0, map->config.y_size - 1); } while(map->carmen_map.map[(int)point.x][(int)point.y] > filter->param->occupied_prob || map->carmen_map.map[(int)point.x][(int)point.y] == -1); point.theta = carmen_uniform_random(-M_PI, M_PI); prob = 0.0; ctheta = cos(point.theta); stheta = sin(point.theta); for(j = 0; j < laser->num_readings && (queue->last == NULL || prob > queue->last->prob); j += filter->param->laser_skip) { if (beam_valid[j]) { x_l = point.x + laser_x[j] * ctheta - laser_y[j] * stheta; y_l = point.y + laser_x[j] * stheta + laser_y[j] * ctheta; if(x_l >= 0 && y_l >= 0 && x_l < map->config.x_size && y_l < map->config.y_size) prob += map->gprob[x_l][y_l]; else prob -= 100; } } priority_queue_add(queue, point, prob); } /* transfer samples from priority queue back into particles */ mark = queue->first; for(i = 0; i < queue->num_elements; i++) { filter->particles[i].x = (mark->point.x * map->config.resolution) + map->config.x_origin; filter->particles[i].y = (mark->point.y * map->config.resolution) + map->config.y_origin; filter->particles[i].theta = mark->point.theta; mark = mark->next; } priority_queue_free(queue); free(laser_x); free(laser_y); free(beam_valid); if(filter->param->do_scanmatching) { for(i = 0; i < filter->param->num_particles; i++) { point.x = filter->particles[i].x; point.y = filter->particles[i].y; point.theta = filter->particles[i].theta; carmen_localize_ackerman_laser_scan_gd(laser->num_readings, laser->range, laser->config.angular_resolution, laser->config.start_angle, &point, filter->param->front_laser_offset, map, filter->param->laser_skip); filter->particles[i].x = point.x; filter->particles[i].y = point.y; filter->particles[i].theta = point.theta; filter->particles[i].weight = 0.0; } } filter->initialized = 1; filter->first_odometry = 1; filter->global_mode = 1; filter->distance_travelled = 0; fprintf(stderr, "\rDoing global localization... (%.1f%% complete)\n\n", 100.0); }
static void pq_free(void *p) { priority_queue_free(p); }
static void recursive_insert(lm_trie_t *trie, ngram_raw_t **raw_ngrams, uint32 *counts, int order) { uint32 unigram_idx = 0; uint32 *words; float *probs; const uint32 unigram_count = (uint32)counts[0]; priority_queue_t *ngrams = priority_queue_create(order, &ngram_ord_comparator); ngram_raw_ord_t *ngram; uint32 *raw_ngrams_ptr; int i; words = (uint32 *)ckd_calloc(order, sizeof(*words)); //for blanks catching probs = (float *)ckd_calloc(order - 1, sizeof(*probs)); //for blanks prob generating ngram = (ngram_raw_ord_t *)ckd_calloc(1, sizeof(*ngram)); ngram->order = 1; ngram->instance.words = &unigram_idx; priority_queue_add(ngrams, ngram); raw_ngrams_ptr = (uint32 *)ckd_calloc(order - 1, sizeof(*raw_ngrams_ptr)); for (i = 2; i <= order; ++i) { ngram_raw_ord_t *tmp_ngram = (ngram_raw_ord_t *)ckd_calloc(1, sizeof(*tmp_ngram)); tmp_ngram->order = i; raw_ngrams_ptr[i-2] = 0; tmp_ngram->instance = raw_ngrams[i - 2][raw_ngrams_ptr[i-2]]; priority_queue_add(ngrams, tmp_ngram); } for (;;) { ngram_raw_ord_t *top = (ngram_raw_ord_t *)priority_queue_poll(ngrams); if (top->order == 1) { trie->unigrams[unigram_idx].next = unigram_next(trie, order); words[0] = unigram_idx; probs[0] = trie->unigrams[unigram_idx].prob; if (++unigram_idx == unigram_count + 1) { ckd_free(top); break; } priority_queue_add(ngrams, top); } else { for (i = 0; i < top->order - 1; i++) { if (words[i] != top->instance.words[i]) { //need to insert dummy suffixes to make ngram of higher order reachable int j; assert(i > 0); //unigrams are not pruned without removing ngrams that contains them for (j = i; j < top->order - 1; j++) { middle_t *middle = &trie->middle_begin[j - 1]; bitarr_address_t address = middle_insert(middle, top->instance.words[j], j + 1, order); //calculate prob for blank float calc_prob = probs[j - 1] + trie->unigrams[top->instance.words[j]].bo; probs[j] = calc_prob; lm_trie_quant_mwrite(trie->quant, address, j - 1, calc_prob, 0.0f); } } } memcpy(words, top->instance.words, top->order * sizeof(*words)); if (top->order == order) { float *weights = top->instance.weights; bitarr_address_t address = longest_insert(trie->longest, top->instance.words[top->order - 1]); lm_trie_quant_lwrite(trie->quant, address, weights[0]); } else { float *weights = top->instance.weights; middle_t *middle = &trie->middle_begin[top->order - 2]; bitarr_address_t address = middle_insert(middle, top->instance.words[top->order - 1], top->order, order); //write prob and backoff probs[top->order - 1] = weights[0]; lm_trie_quant_mwrite(trie->quant, address, top->order - 2, weights[0], weights[1]); } raw_ngrams_ptr[top->order - 2]++; if (raw_ngrams_ptr[top->order - 2] < counts[top->order - 1]) { top->instance = raw_ngrams[top->order-2][raw_ngrams_ptr[top->order - 2]]; priority_queue_add(ngrams, top); } else { ckd_free(top); } } } assert(priority_queue_size(ngrams) == 0); priority_queue_free(ngrams, NULL); ckd_free(raw_ngrams_ptr); ckd_free(words); ckd_free(probs); }