static void person_filter_motion_update(carmen_dot_person_filter_p f) {
double ax, ay;
// kalman filter time update with brownian motion model
ax = carmen_uniform_random(-1.0, 1.0) * f->a;
ay = carmen_uniform_random(-1.0, 1.0) * f->a;
//printf("ax = %.4f, ay = %.4f\n", ax, ay);
f->x += ax;
f->y += ay;
f->px = ax*ax*f->px + f->qx;
f->pxy = ax*ax*f->pxy + f->qxy;
f->py = ay*ay*f->py + f->qy;
if (f->hidden_cnt == 0) {
f->vx[f->vpos] = ax;
f->vy[f->vpos] = ay;
f->vpos = (f->vpos+1) % person_filter_velocity_window;
if (f->vlen < person_filter_velocity_window)
f->vlen++;
else
f->vlen = person_filter_velocity_window;
}
}
moving_objects3_particle_t
sample_motion_model(moving_objects3_particle_t particle_t_1, double delta_time)
{
moving_objects3_particle_t particle_t;
// compute actual velocity
double velocity_t = particle_t_1.velocity + carmen_uniform_random(-MAX_ACCELERATION * delta_time, MAX_ACCELERATION * delta_time);
// find delta_thetas
double delta_theta_1 = carmen_uniform_random(-MAX_ANGULAR_VELOCITY * delta_time, MAX_ANGULAR_VELOCITY * delta_time);
double delta_theta_2 = carmen_uniform_random(-MAX_ANGULAR_VELOCITY * delta_time, MAX_ANGULAR_VELOCITY * delta_time);
// find motion distance
double motion_distance = velocity_t * delta_time;
// find delta_x and delta_y
double delta_x = motion_distance * cos(particle_t_1.pose.theta + delta_theta_1);
double delta_y = motion_distance * sin(particle_t_1.pose.theta + delta_theta_1);
// generate new particle sample
particle_t.pose.x = particle_t_1.pose.x + delta_x;
particle_t.pose.y = particle_t_1.pose.y + delta_y;
particle_t.pose.theta = particle_t_1.pose.theta + delta_theta_1 + delta_theta_2;
particle_t.velocity = velocity_t;
particle_t.geometry.length = particle_t_1.geometry.length;
particle_t.geometry.width = particle_t_1.geometry.width;
return particle_t;
}
static void pick_random_place(void)
{
carmen_map_point_t map_pt;
carmen_world_point_t world_pt;
map_pt.map = c_space;
do {
map_pt.x = carmen_uniform_random(0, c_space->config.x_size);
map_pt.y = carmen_uniform_random(0, c_space->config.y_size);
} while (c_space->map[map_pt.x][map_pt.y] < 0);
carmen_map_to_world(&map_pt, &world_pt);
carmen_navigator_set_goal(world_pt.pose.x, world_pt.pose.y);
carmen_navigator_go();
up_to_speed = 0;
}
static void door_filter_motion_update(carmen_dot_door_filter_p f) {
double ax, ay;
// kalman filter time update with brownian motion model
ax = carmen_uniform_random(-1.0, 1.0) * f->a;
ay = carmen_uniform_random(-1.0, 1.0) * f->a;
//printf("ax = %.4f, ay = %.4f\n", ax, ay);
f->x += ax;
f->y += ay;
f->px = ax*ax*f->px + f->qx;
f->pxy = ax*ax*f->pxy + f->qxy;
f->py = ay*ay*f->py + f->qy;
}
static void
resample(carmen_fused_odometry_particle *xt)
{
double sum_weights = 0.0;
int m;
for (m = 0; m < num_particles; m++)
sum_weights += xt[m].weight;
double invM = sum_weights / (double) num_particles; // To avoid problems with a Measurament Model that does not returns probability 1, we sum the weights up
double r = invM * carmen_uniform_random(0.0, 1.0);
double c = xt[0].weight;
for (m = 0; m < num_particles; m++)
_xt[m] = xt[m];
int i = 0; // The book appears to have a bug: i should start with zero.
for (m = 1; m <= num_particles; m++)
{
double U = r + (double)(m - 1) * invM;
while (U > c)
{
i = i + 1;
c = c + _xt[i].weight;
}
xt[m - 1] = _xt[i];
}
}
/* adds error to a laser scan */
static void
add_laser_error(carmen_laser_laser_message * laser,
carmen_simulator_ackerman_laser_config_t *laser_config)
{
int i;
for (i = 0; i < laser_config->num_lasers; i ++)
{
if (laser->range[i] > laser_config->max_range)
laser->range[i] = laser_config->max_range;
else if (carmen_uniform_random(0, 1.0) < laser_config->prob_of_random_max)
laser->range[i] = laser_config->max_range;
else if (carmen_uniform_random(0, 1.0) < laser_config->prob_of_random_reading)
laser->range[i] = carmen_uniform_random(0, laser_config->num_lasers);
else
laser->range[i] += carmen_gaussian_random(0.0, laser_config->variance);
}
}
std::vector<moving_objects3_particle_t>
resample(std::vector<moving_objects3_particle_t> particle_set_t)
{
std::vector<moving_objects3_particle_t> particle_set;
double inv_num_particles = 1.0/NUM_OF_PARTICLES;
double r = carmen_uniform_random(0.0,inv_num_particles);
double c = particle_set_t[0].weight;
double U;
int i = 0;
for (int m = 0; m < NUM_OF_PARTICLES; m++)
{
U = r + (m * inv_num_particles);
while (U > c)
{
i++;
c += particle_set_t[i].weight;
}
particle_set.push_back(particle_set_t[i]);
}
return particle_set;
}
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);
}
carmen_roadmap_t *carmen_roadmap_initialize(carmen_map_p new_map)
{
int x, y, i;
int **grid, *grid_ptr, **sample_grid;
int progress;
int size;
int growth_counter;
int x_offset[8] = {1, 1, 0, -1, -1, -1, 0, 1};
int y_offset[8] = {0, 1, 1, 1, 0, -1, -1, -1};
int total;
int sample;
int random_value;
int max_label;
carmen_roadmap_t *roadmap;
carmen_map_p c_space;
carmen_list_t *node_list;
c_space = carmen_map_copy(new_map);
node_list = carmen_list_create(sizeof(carmen_roadmap_vertex_t),
MAX_NUM_VERTICES);
size = c_space->config.x_size*c_space->config.y_size;
grid = (int **)calloc(c_space->config.x_size, sizeof(int *));
carmen_test_alloc(grid);
grid[0] = (int *)calloc(size, sizeof(int));
carmen_test_alloc(grid[0]);
for (x = 1; x < c_space->config.x_size; x++)
grid[x] = grid[0] + x*c_space->config.y_size;
sample_grid = (int **)calloc(c_space->config.x_size, sizeof(int *));
carmen_test_alloc(sample_grid);
sample_grid[0] = (int *)calloc(size, sizeof(int));
carmen_test_alloc(sample_grid);
for (x = 1; x < c_space->config.x_size; x++)
sample_grid[x] = sample_grid[0] + x*c_space->config.y_size;
grid_ptr = grid[0];
for (i = 0; i < size; i++)
*(grid_ptr++) = 0;
for (x = 0; x < c_space->config.x_size; x++)
for (y = 0; y < c_space->config.y_size; y++) {
if (x == 0 || y == 0 || x == c_space->config.x_size-1 ||
y == c_space->config.y_size-1 ||
c_space->map[x][y] > 0.001 || c_space->map[x][y] < 0) {
grid[x][y] = 1;
c_space->map[x][y] = 1;
}
}
growth_counter = 1;
do {
progress = 0;
for (x = 1; x < c_space->config.x_size-1; x++)
for (y = 1; y < c_space->config.y_size-1; y++) {
if (grid[x][y] == growth_counter) {
for (i = 0; i < 8; i+=2) {
if (grid[x+x_offset[i]][y+y_offset[i]] == 0) {
grid[x+x_offset[i]][y+y_offset[i]] = growth_counter+1;
c_space->map[x][y] = growth_counter+1;
}
progress = 1;
}
}
}
growth_counter++;
} while (progress);
total = 0;
for (x = 0; x < c_space->config.x_size; x++)
for (y = 0; y < c_space->config.y_size; y++) {
if (x == 0 || x == c_space->config.x_size-1 ||
y == 0 || y == c_space->config.y_size-1) {
sample_grid[x][y] = total;
c_space->map[x][y] = 1;
}
if (grid[x][y] < 6) {
sample_grid[x][y] = total;
if (grid[x][y] < 4)
c_space->map[x][y] = 1e6;
continue;
}
for (i = 0; i < 8; i+=2) {
if (grid[x+x_offset[i]][y+y_offset[i]] >= grid[x][y])
break;
}
if (i >= 8) {
total += 100;
sample_grid[x][y] = total;
continue;
}
for (i = 0; i < 8; i+=2) {
if (grid[x+x_offset[i]][y+y_offset[i]] >= grid[x][y]+1)
break;
}
if (i == 8) {
total += 50;
sample_grid[x][y] = total;
continue;
}
total++;
sample_grid[x][y] = total;
}
max_label = 0;
x = 0;
y = 0;
for (sample = 0; sample < MAX_NUM_VERTICES; sample++) {
random_value = (int)carmen_uniform_random(0, total);
if (random_value < sample_grid[0][size/2]) {
grid_ptr = sample_grid[0];
for (i = 0; i < size-1; i++) {
if (random_value < *(grid_ptr++))
break;
}
} else {
grid_ptr = sample_grid[0]+size/2;
for (i = size/2; i < size-1; i++) {
if (random_value < *(grid_ptr++))
break;
}
}
if (grid[0][i] > 0) {
x = i / c_space->config.y_size;
y = i % c_space->config.y_size;
add_node(node_list, x, y);
grid[0][i] = 0;
}
}
add_node(node_list, 450, 140);
add_node(node_list, 550, 140);
add_node(node_list, 600, 141);
add_node(node_list, 550, 141);
free(grid[0]);
free(grid);
free(sample_grid[0]);
free(sample_grid);
roadmap = (carmen_roadmap_t *)calloc(1, sizeof(carmen_roadmap_t));
carmen_test_alloc(roadmap);
roadmap->nodes = node_list;
roadmap->goal_id = -1;
roadmap->c_space = c_space;
roadmap->avoid_people = 1;
return roadmap;
}
void
resample(std::vector<particle_datmo_t> &particle_set_t)
{
/*** LOW VARIANCE RESAMPLER ALGORITHM ***/
// EDUARDO'S VERSION:
// static double *cumulative_sum = NULL;
// static particle_datmo_t *temp_particles = NULL;
// static int num_particles = particle_set_t.size();
//
// particle_datmo_t *aux;
// particle_datmo_t *copy_particle_set_t = NULL;
// int i, which_particle;
// double weight_sum;
// double position, step_size;
//
// /* Allocate memory necessary for resampling */
// cumulative_sum = (double *)calloc(num_particles, sizeof(double));
// carmen_test_alloc(cumulative_sum);
// temp_particles = (particle_datmo_t*)malloc(sizeof(particle_datmo_t) * num_particles);
// carmen_test_alloc(temp_particles);
// copy_particle_set_t = (particle_datmo_t*)malloc(sizeof(particle_datmo_t) * num_particles);
// carmen_test_alloc(copy_particle_set_t);
//
// int j = 0;
// for (std::vector<particle_datmo_t>::iterator it = particle_set_t.begin(); it != particle_set_t.end(); it++)
// {
// copy_particle_set_t[j].pose.x = it->pose.x;
// copy_particle_set_t[j].pose.y = it->pose.y;
// copy_particle_set_t[j].pose.theta = it->pose.theta;
// copy_particle_set_t[j].velocity = it->velocity;
// copy_particle_set_t[j].weight = it->weight;
// copy_particle_set_t[j].class_id = it->class_id;
// copy_particle_set_t[j].model_features = it->model_features;
// copy_particle_set_t[j].dist = it->dist;
// j++;
// }
//
// weight_sum = 0.0;
// for (i = 0; i < num_particles; i++)
// {
// /* Sum the weights of all of the particles */
// weight_sum += copy_particle_set_t[i].weight;
// cumulative_sum[i] = weight_sum;
// }
//
// /* choose random starting position for low-variance walk */
// position = carmen_uniform_random(0, weight_sum);
// step_size = weight_sum / (double) num_particles;
// which_particle = 0;
//
// /* draw num_particles random samples */
// for (i = 0; i < num_particles; i++)
// {
// position += step_size;
//
// if (position > weight_sum)
// {
// position -= weight_sum;
// which_particle = 0;
// }
//
// while(position > cumulative_sum[which_particle])
// which_particle++;
//
// temp_particles[i] = copy_particle_set_t[which_particle];
// }
//
// /* Switch particle pointers */
// aux = copy_particle_set_t;
// copy_particle_set_t = temp_particles;
// temp_particles = aux;
//
// int m = 0;
// for (std::vector<particle_datmo_t>::iterator it = particle_set_t.begin(); it != particle_set_t.end(); it++)
// {
// it->pose.x = copy_particle_set_t[m].pose.x;
// it->pose.y = copy_particle_set_t[m].pose.y;
// it->pose.theta = copy_particle_set_t[m].pose.theta;
// it->velocity = copy_particle_set_t[m].velocity;
// it->weight = copy_particle_set_t[m].weight;
// it->class_id = copy_particle_set_t[which_particle].class_id;
// it->model_features = copy_particle_set_t[which_particle].model_features;
// it->dist = copy_particle_set_t[which_particle].dist;
// m++;
// }
//
// free(temp_particles);
// free(cumulative_sum);
// free(copy_particle_set_t);
static double *cumulative_sum = NULL;
static int num_particles = particle_set_t.size();
particle_datmo_t *copy_particle_set_t = NULL;
int i, which_particle;
double weight_sum = 0.0;
double position, step_size;
/* Allocate memory necessary for resampling */
cumulative_sum = (double *)calloc(num_particles, sizeof(double));
carmen_test_alloc(cumulative_sum);
copy_particle_set_t = (particle_datmo_t*)malloc(sizeof(particle_datmo_t) * num_particles);
carmen_test_alloc(copy_particle_set_t);
int j = 0;
for (std::vector<particle_datmo_t>::iterator it = particle_set_t.begin(); it != particle_set_t.end(); it++)
{
// copy_particle_set_t[j].pose.x = it->pose.x;
// copy_particle_set_t[j].pose.y = it->pose.y;
// copy_particle_set_t[j].pose.theta = it->pose.theta;
// copy_particle_set_t[j].velocity = it->velocity;
// copy_particle_set_t[j].weight = it->weight;
// copy_particle_set_t[j].class_id = it->class_id;
// copy_particle_set_t[j].model_features = it->model_features;
// copy_particle_set_t[j].dist = it->dist;
copy_particle_set_t[j] = *it;
/* change log weights back into probabilities */
/* Sum the weights of all of the particles */
weight_sum += it->weight;
cumulative_sum[j] = weight_sum;
j++;
}
/* choose random starting position for low-variance walk */
position = carmen_uniform_random(0, weight_sum);
step_size = weight_sum / (double) num_particles;
which_particle = 0;
/* draw num_particles random samples */
for (i = 0; i < num_particles; i++)
{
position += step_size;
if (position > weight_sum)
{
position -= weight_sum;
which_particle = 0;
}
while (position > cumulative_sum[which_particle])
which_particle++;
particle_set_t[i] = copy_particle_set_t[which_particle];
// particle_set_t[i].pose.x = copy_particle_set_t[which_particle].pose.x;
// particle_set_t[i].pose.y = copy_particle_set_t[which_particle].pose.y;
// particle_set_t[i].pose.theta = copy_particle_set_t[which_particle].pose.theta;
// particle_set_t[i].velocity = copy_particle_set_t[which_particle].velocity;
// particle_set_t[i].weight = copy_particle_set_t[which_particle].weight;
// particle_set_t[i].class_id = copy_particle_set_t[which_particle].class_id;
// particle_set_t[i].model_features = copy_particle_set_t[which_particle].model_features;
// particle_set_t[i].dist = copy_particle_set_t[which_particle].dist;
}
free(cumulative_sum);
free(copy_particle_set_t);
}
