double
probability_with_pos( MAP2 map, logtools_lasersens2_data_t data,
logtools_rpos2_t rpos,
logtools_rmove2_t move, logtools_rmove2_t odo_move )
{
static logtools_rmove2_t nomove = {0.0, 0.0, 0.0};
int i;
logtools_vector2_t pt;
logtools_ivector2_t mvec;
double bprob, prob = 0.0;
for (i=0;i<data.laser.numvalues;i++) {
if (data.laser.val[i]<settings.local_map_max_range) {
pt = logtools_compute_laser_points( rpos,
data.laser.val[i],
nomove,
data.laser.angle[i] );
compute_rmap_pos_from_vec2( pt, map, &mvec );
bprob = log(get_map_val( mvec, map ));
} else {
bprob = log(settings.local_map_std_val);
}
prob += bprob;
}
prob += probability_between_moves( move, odo_move );
return( prob );
}
logtools_laser_coord2_t
compute_laser2d_coord( logtools_lasersens2_data_t lsens, int i )
{
double val;
logtools_laser_coord2_t coord = { {0.0,0.0},{0.0,0.0}, 0, 0 };
logtools_rpos2_t rpos, npos;
rpos = logtools_rpos2_with_movement2( lsens.estpos,
lsens.laser.offset );
npos.x = 0.0;
npos.y = 0.0;
npos.o = 0.0;
val = lsens.laser.val[i];
coord.relpt = logtools_compute_laser_points( npos, val,
lsens.laser.offset,
lsens.laser.angle[i] );
coord.abspt = logtools_compute_laser_points( rpos, val,
lsens.laser.offset,
lsens.laser.angle[i] );
return(coord);
}
void
create_local_map( MAP2 *map, logtools_lasersens2_data_t data,
logtools_rmove2_t movement )
{
int i, max_num_linepoints;
static logtools_rpos2_t npos = {0.0, 0.0, 0.0 };
static logtools_rmove2_t nmove = {0.0, 0.0, 0.0 };
logtools_rpos2_t rpos;
logtools_ivector2_t start, end;
logtools_vector2_t lpos;
static int first_time = TRUE;
static logtools_grid_line_t line;
logtools_rmove2_t offset;
if (first_time) {
max_num_linepoints = (int)
(2 * ( settings.local_map_max_range / settings.local_map_resolution ));
line.grid = (logtools_ivector2_t *) malloc( max_num_linepoints * sizeof(logtools_ivector2_t) );
first_time = FALSE;
}
offset.forward = movement.forward; // + data.laser.offset[i].forward;
offset.sideward = movement.sideward; // + data.laser.offset[i].sideward;
offset.rotation = movement.rotation; // + data.laser.offset[i].rotation;
rpos = logtools_rpos2_backwards_with_movement2( npos, offset );
compute_map_pos_from_rpos( rpos, map, &start );
for (i=0;i<data.laser.numvalues;i++) {
lpos = logtools_compute_laser_points( rpos,
data.laser.val[i],
nmove, // data.laser.offset[i],
data.laser.angle[i] );
if (compute_map_pos_from_vec2( lpos, map, &end )) {
if ( data.laser.val[i]<settings.local_map_max_range &&
end.x>=0 && end.x<map->mapsize.x &&
end.y>=0 && end.y<map->mapsize.y ) {
set_mapsumpoint( map, end.x, end.y );
set_maphitpoint( map, end.x, end.y, 1.0 );
}
}
}
}
