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);
}
double
probability_with_move( MAP2 map,
logtools_lasersens2_data_t data,
logtools_rmove2_t move,
logtools_rmove2_t odo_move,
double *laserprob )
{
int i;
logtools_vector2_t pt;
logtools_ivector2_t mvec;
logtools_rpos2_t rpos;
logtools_rpos2_t npos = {0.0, 0.0, 0.0};
static logtools_rmove2_t nomove = {0.0, 0.0, 0.0};
double bprob, prob = 0.0;
rpos = logtools_rpos2_with_movement2( npos, move );
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;
}
*laserprob = prob;
if (settings.local_map_use_odometry != ODO_NOTHING)
prob += probability_between_moves( move, odo_move );
return( prob );
}
