Revision 1135
Committing some test files, as well as an updated version of the sensor mapping code.
trunk/code/projects/mapping/matlab/sensor_transform_test.m  

1  
2 
sensor_model_width = 100; 

3 
sensor_model_height = 100; 

4  
5 
map_width = 800; 

6 
map_height = 800; 

7 
arena = ones(map_width,map_height); 

8  
9 
sample_sensor = sensor_model(:,:, 60); 

10  
11 
%sample data: 

12 
x = 200; 

13 
y = 100; 

14  
15 
dx = x; 

16 
dy = y; 

17  
18 
objTheta = pi; 

19 
%objTheta = 0; 

20  
21 
translation_mat = [1, 0, 0; 

22 
0, 1, 0; 

23 
dx, dy, 1]; 

24  
25 
rotation_mat = [cos(objTheta), sin(objTheta), 0; 

26 
sin(objTheta), cos(objTheta), 0 

27 
0, 0, 1]; 

28  
29 
udata = [1, 1] * sensor_model_width/2; 

30 
vdata = [1, 1] * sensor_model_height/2; 

31 
xdata = [1, 1] * map_width/2; 

32 
ydata = [1, 1] * map_height/2; 

33  
34 
transformation_mat = rotation_mat * translation_mat; 

35 
translation = maketform('affine', transformation_mat); 

36  
37 
sensor = imtransform(sample_sensor, translation,... 

38 
'UData', udata, 'Vdata', vdata,... 

39 
'XData', xdata, 'YData', ydata,... 

40 
'Size', [map_width, map_height], 'FillValues', 1); 

41  
42 
size(arena) 

43 
size(sensor) 

44  
45 
arena = arena.*sensor; 

46  
47 
image(arena.*20); 
trunk/code/projects/mapping/matlab/sensor_map.m  

34  34 
%Define sensor model. 
35  35  
36  36 
%Definitions: 
37 
sensor_model_width = 120;


38 
sensor_model_height = 400;


37 
sensor_model_width = 60;


38 
sensor_model_height = 100;


39  39 
sensor_bounds_x = 30; 
40  40 
sensor_bounds_y = 100; 
41  41 
sensor_width = 8; %cm 
...  ...  
78  78  
79  79  
80  80 
%Define map area (1100cm by 1100cm) 
81 
map_width = 1100;


82 
map_height = 1100;


81 
map_width = 600;


82 
map_height = 600;


83  83 
udata = [1, 1] * sensor_model_width/2; 
84  84 
vdata = [1, 1] * sensor_model_height/2; 
85  85 
xdata = [1, 1] * map_width/2; 
trunk/code/projects/mapping/matlab/sensor_test.m  

1  
2 
%Define sensor model. 

3  
4 
%Definitions: 

5 
sensor_model_width = 100; 

6 
sensor_model_height = 100; 

7 
sensor_bounds_x = 30; 

8 
sensor_bounds_y = 100; 

9 
sensor_width = 8; %cm 

10 
empty_weight = 0.1; 

11 
wall_weight = 2.0; 

12 
outside_weight = 1; 

13  
14 
%Space creation: 

15 
Sx = linspace(sensor_bounds_x, sensor_bounds_x, sensor_model_width); 

16 
Sy = linspace(sensor_bounds_y, sensor_bounds_y, sensor_model_height); 

17 
sensor_model = zeros(size(Sx,1), size(Sy,1), 71); 

18  
19 
%k ranges over distances (cm) 

20 
for k=10:80 

21 
for j = 1:sensor_model_height; 

22 
for i = 1:sensor_model_width; 

23 
slope = k / sensor_width; 

24 
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k  2) && Sy(j) > 0 

25 
sensor_model(i,j,k) = empty_weight; 

26 
elseif abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k + 2) && Sy(j) > 0 

27 
sensor_model(i,j,k) = wall_weight / abs((k  Sy(j))); 

28 
else 

29 
sensor_model(i,j,k) = outside_weight; 

30 
end 

31 
end 

32 
end 

33 
end 

34  
35 
%Define the sensor model for when no measurement is made (1) 

36 
for j = 1:sensor_model_height; 

37 
for i = 1:sensor_model_width; 

38 
slope = 80 / sensor_width; 

39 
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < 80 && Sy(j) > 0 

40 
sensor_model(i,j,k) = empty_weight; 

41 
else 

42 
sensor_model(i,j,k) = outside_weight; 

43 
end 

44 
end 

45 
end 

46  
47  
48 
Also available in: Unified diff