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);
}
