Revision 1211
Changes to the sensor modeling code. (Mostly from a better understanding of imtransform).
| model_sensor.m | ||
|---|---|---|
| 1 | 1 |
function [sensor_model, udata, vdata] = model_sensor |
| 2 | 2 |
|
| 3 | 3 |
%Definitions: |
| 4 |
sensor_model_width = 60;
|
|
| 4 |
sensor_model_width = 25;
|
|
| 5 | 5 |
sensor_model_height = 100; |
| 6 |
sensor_bounds_x = 30;
|
|
| 7 |
sensor_bounds_y = 100;
|
|
| 8 |
sensor_width = 8; %cm
|
|
| 9 |
empty_weight = 0.1;
|
|
| 6 |
sensor_bounds_x = 85; %cm
|
|
| 7 |
sensor_bounds_y = 15; %cm
|
|
| 8 |
sensor_width = 10; %cm
|
|
| 9 |
empty_weight = 0.4;
|
|
| 10 | 10 |
wall_weight = 2.0; |
| 11 | 11 |
outside_weight = 1; |
| 12 | 12 |
|
| 13 | 13 |
%Space creation: |
| 14 |
Sx = linspace(-sensor_bounds_x, sensor_bounds_x, sensor_model_width);
|
|
| 14 |
Sx = linspace(0, sensor_bounds_x, sensor_model_width);
|
|
| 15 | 15 |
Sy = linspace(-sensor_bounds_y, sensor_bounds_y, sensor_model_height); |
| 16 | 16 |
sensor_model = zeros(size(Sx,1), size(Sy,1), 71); |
| 17 | 17 |
|
| ... | ... | |
| 19 | 19 |
for k=10:80 |
| 20 | 20 |
for j = 1:sensor_model_height; |
| 21 | 21 |
for i = 1:sensor_model_width; |
| 22 |
slope = k / sensor_width;
|
|
| 23 |
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k - 2) && Sy(j) > 0
|
|
| 24 |
sensor_model(i,j,k) = empty_weight;
|
|
| 25 |
elseif abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k + 2) && Sy(j) > 0
|
|
| 26 |
sensor_model(i,j,k) = wall_weight / abs((k - Sy(j)));
|
|
| 22 |
slope = double(k) / double(sensor_width);
|
|
| 23 |
if abs(Sy(j)) < (Sx(i) / slope) && Sx(i) < (k - 2)
|
|
| 24 |
sensor_model(j,i,k) = empty_weight;
|
|
| 25 |
elseif abs(Sy(j)) < (Sx(i) / slope) && Sx(i) < (k + 2)
|
|
| 26 |
sensor_model(j,i,k) = wall_weight;
|
|
| 27 | 27 |
else |
| 28 |
sensor_model(i,j,k) = outside_weight;
|
|
| 28 |
sensor_model(j,i,k) = outside_weight;
|
|
| 29 | 29 |
end |
| 30 | 30 |
end |
| 31 | 31 |
end |
| 32 |
map(k,1,1) = 100.0; |
|
| 32 | 33 |
end |
| 33 | 34 |
|
| 34 | 35 |
%Define the sensor model for when no measurement is made (-1) |
| 36 |
slope = 80 / sensor_width; |
|
| 35 | 37 |
for j = 1:sensor_model_height; |
| 36 | 38 |
for i = 1:sensor_model_width; |
| 37 |
slope = 80 / sensor_width; |
|
| 38 |
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < 80 && Sy(j) > 0 |
|
| 39 |
sensor_model(i,j,k) = empty_weight; |
|
| 39 |
if abs(Sy(j)) < Sx(i) / slope && Sx(i) < 80 && Sx(i) > 0 |
|
| 40 |
sensor_model(j,i,71) = empty_weight; |
|
| 40 | 41 |
else |
| 41 |
sensor_model(i,j,k) = outside_weight;
|
|
| 42 |
sensor_model(j,i,71) = outside_weight;
|
|
| 42 | 43 |
end |
| 43 | 44 |
end |
| 44 | 45 |
end |
| 45 | 46 |
|
| 46 |
udata = [-1, 1] * sensor_model_width/2; |
|
| 47 |
vdata = [-1, 1] * sensor_model_height/2; |
|
| 47 |
image(sensor_model(:,:,70).*20); |
|
| 48 |
|
|
| 49 |
udata = [0, 2] * sensor_bounds_x; |
|
| 50 |
vdata = [-1, 1] * sensor_bounds_y; |
|
Also available in: Unified diff