static double compute_cost(carmen_roadmap_vertex_t *node,
carmen_roadmap_vertex_t *parent_node,
carmen_map_p c_space)
{
carmen_bresenham_param_t params;
int x, y;
double cost;
carmen_get_bresenham_parameters(node->x, node->y, parent_node->x,
parent_node->y, ¶ms);
if (node->x < 0 || node->x >= c_space->config.x_size ||
parent_node->y < 0 || parent_node->y >= c_space->config.y_size)
return 1e6;
cost = 0;
do {
carmen_get_current_point(¶ms, &x, &y);
if (c_space->map[x][y] > 1e5 || c_space->map[x][y] < 0) {
return 1e6;
break;
}
cost += 1;
} while (carmen_get_next_point(¶ms));
return hypot(node->x-parent_node->x, node->y-parent_node->y);
}
static int points_are_visible(carmen_traj_point_t *p1,
carmen_traj_point_t *p2,
carmen_map_t *c_space)
{
carmen_bresenham_param_t params;
int x, y;
carmen_map_point_t mp1, mp2;
carmen_trajectory_to_map(p1, &mp1, c_space);
carmen_trajectory_to_map(p2, &mp2, c_space);
carmen_get_bresenham_parameters(mp1.x, mp1.y, mp2.x, mp2.y, ¶ms);
if (mp1.x < 0 || mp1.x >= c_space->config.x_size ||
mp1.y < 0 || mp1.y >= c_space->config.y_size)
return 0;
if (mp2.x < 0 || mp2.x >= c_space->config.x_size ||
mp2.y < 0 || mp2.y >= c_space->config.y_size)
return 0;
do {
carmen_get_current_point(¶ms, &x, &y);
if (c_space->map[x][y] > .01 || c_space->map[x][y] < 0)
return 0;
} while (carmen_get_next_point(¶ms));
return 1;
}
int carmen_roadmap_is_visible(carmen_roadmap_vertex_t *node,
carmen_world_point_t *position,
carmen_roadmap_t *roadmap)
{
carmen_bresenham_param_t params;
int x, y;
carmen_map_point_t map_pt;
carmen_map_p c_space;
c_space = roadmap->c_space;
carmen_world_to_map(position, &map_pt);
carmen_get_bresenham_parameters(node->x, node->y, map_pt.x,
map_pt.y, ¶ms);
if (node->x < 0 || node->x >= c_space->config.x_size ||
map_pt.y < 0 || map_pt.y >= c_space->config.y_size)
return 0;
do {
carmen_get_current_point(¶ms, &x, &y);
if (c_space->map[x][y] > 1e5 || c_space->map[x][y] < 0)
return 0;
} while (carmen_get_next_point(¶ms));
return 1;
}
void
trace_laser(int x_1, int y_1, int x_2, int y_2, carmen_map_p true_map,
carmen_map_p modify_map)
{
carmen_bresenham_param_t params;
int X, Y;
double true_map_value;
double modified_map_value;
carmen_get_bresenham_parameters(x_1, y_1, x_2, y_2, ¶ms);
do {
carmen_get_current_point(¶ms, &X, &Y);
if (!is_in_map(X, Y, modify_map))
break;
true_map_value = true_map->map[X][Y];
modified_map_value = modify_map->map[X][Y];
if (!is_empty(modified_map_value) && is_empty(true_map_value))
add_clear_point(X, Y, true_map, modify_map);
} while (carmen_get_next_point(¶ms));
}
void carmen_mapper_update_evidence_grid_general(evidence_grid *grid,
double laser_x, double laser_y,
double laser_theta, int num_readings,
float *laser_range,
float *laser_angle,
double angular_resolution,
double first_beam_angle)
{
int i;
int x1int, y1int, x2int, y2int, current_x, current_y, x_diff, y_diff;
double d, theta, delta_theta, p_filled, temp_laser_x, temp_laser_y;
carmen_bresenham_param_t b_params;
double new_prob;
double correction_angle = 0;
if(grid->first) {
grid->start_x = laser_x / grid->resolution;
grid->start_y = laser_y / grid->resolution;
grid->start_theta = laser_theta;
grid->first = 0;
}
correction_angle = grid->theta_offset - grid->start_theta;
laser_theta = laser_theta + correction_angle;
temp_laser_x = laser_x * cos(correction_angle) -
laser_y * sin(correction_angle);
temp_laser_y = laser_x * sin(correction_angle) +
laser_y * cos(correction_angle);
laser_x = grid->size_x / 2.0 + (temp_laser_x / grid->resolution -
grid->start_x);
laser_y = grid->size_y / 2.0 + (temp_laser_y / grid->resolution -
grid->start_y);
x1int = (int)floor(laser_x);
y1int = (int)floor(laser_y);
theta = laser_theta + first_beam_angle;
delta_theta = angular_resolution;
for(i = 0; i < num_readings; i++) {
if(laser_range[i] < LASER_RANGE_LIMIT) {
theta = laser_theta + laser_angle[i];
x2int = (int)(laser_x + (laser_range[i] + grid->wall_thickness) *
cos(theta) / grid->resolution);
y2int = (int)(laser_y + (laser_range[i] + grid->wall_thickness) *
sin(theta) / grid->resolution);
carmen_get_bresenham_parameters(x1int, y1int, x2int, y2int, &b_params);
do {
carmen_get_current_point(&b_params, ¤t_x, ¤t_y);
if(current_x >= 0 && current_x < grid->size_x &&
current_y >= 0 && current_y < grid->size_y) {
/* Calculate distance from laser */
x_diff = abs(current_x - x1int);
y_diff = abs(current_y - y1int);
if(x_diff >= grid->distance_table_size ||
y_diff >= grid->distance_table_size)
d = 1e6;
else
d = grid->distance_table[x_diff][y_diff];
if(d < laser_range[i]) { /* Free observation */
if(d < grid->max_sure_range)
p_filled = grid->emp_evidence;
else if(d < grid->max_range)
p_filled = grid->emp_evidence +
(d - grid->max_sure_range) / grid->max_range *
(grid->prior_occ - grid->emp_evidence);
else
break;
}
else { /* Filled observation */
if(d < grid->max_sure_range)
p_filled = grid->occ_evidence;
else if(d < grid->max_range)
p_filled = grid->occ_evidence +
(d - grid->max_sure_range) / grid->max_range *
(grid->prior_occ - grid->occ_evidence);
else
break;
}
if(grid->prob[current_x][current_y] == -1)
grid->prob[current_x][current_y] = grid->prior_occ;
/* Adjust the map */
new_prob = 1 - 1 / (1 + (1 - grid->prior_occ) / grid->prior_occ *
p_filled / (1 - p_filled) *
grid->prob[current_x][current_y] /
(1 - grid->prob[current_x][current_y]));
grid->prob[current_x][current_y] = new_prob;
}
} while(carmen_get_next_point(&b_params));
}
}
}
