root / trunk / code / projects / mapping / matlab / sensor_map.m @ 1149
History  View  Annotate  Download (2.97 KB)
1  

2 
%Function which creates an occupancy grid map giving the probability of of there being an object 
3 
%given a series of measurements over the probability of there not being an object 
4 
%given the measurements. 
5  
6 
%Input comes from 'filename' where each line contains 8 spaceseparated values 
7 
%Each line contains: x, y, theta, IR1, IR2, IR3, IR4, IR5 in that order 
8 
%Example line of input file: 
9 
%17 23 .45 1 42 1 1 1 
10 
%x and y are centimeters, theta is in radians 
11 
%IR values are the values seen by the robot, where 1 means that nothing is seen 
12  
13 
%Output: graph of points seen by robot 
14 
%All output measurements are in millimeters 
15  
16 
function arena = sensor_map(filename) 
17  
18 
%Vectors from center of bot to IR sensors 
19 
Bot_IR1 = [4.5,3.5]; 
20 
Bot_IR2 = [7.5,0]; 
21 
Bot_IR3 = [4.5,3.5]; 
22 
Bot_IR4 = [4,3.5]; 
23 
Bot_IR5 = [4,3.5]; 
24 
Bot_IR = [Bot_IR1; Bot_IR2; Bot_IR3; Bot_IR4; Bot_IR5]; 
25  
26 
%Unit vectors with the same angle that the IR sensors are tilted at 
27 
Angle_IR1 = [.78,.63]; 
28 
Angle_IR2 = [1,0]; 
29 
Angle_IR3 = [.78,.63]; 
30 
Angle_IR4 = [0,1]; 
31 
Angle_IR5 = [0,1]; 
32 
Angle_IR = [Angle_IR1; Angle_IR2; Angle_IR3; Angle_IR4; Angle_IR5]; 
33  
34 
%Define sensor model. 
35  
36 
[sensor_model, udata, vdata] = model_sensor; 
37  
38 
%Define map area (1100cm by 1100cm) 
39 
map_width = 600; 
40 
map_height = 600; 
41 
xdata = [1, 1] * map_width/2; 
42 
ydata = [1, 1] * map_height/2; 
43 
arena = ones(map_width,map_height); 
44  
45 
%For now, read input in from a file 
46 
Data = dlmread(filename, ' '); 
47  
48 
points = []; 
49 
%loop through each line of file 
50 
for i=1:size(Data, 1) 
51 
display(i); 
52 
row = Data(i, :); 
53 
x = row(1); 
54 
y = row(2); 
55 
theta = row(3); 
56  
57 
%look at each IR sensor reading 
58 
for ir=1:5 
59 
distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm 
60 
if distance ~= 1 
61 
k_val = floor(distance / 10); 
62 
sensor_k = sensor_model(:,:,k_val); 
63 

64 
%Find vector of object relative to center of bot 
65 
ObjectVector = distance * Angle_IR(ir, :); 
66 
ObjectVector = ObjectVector + Bot_IR(ir, :); 
67 

68 
%rotate by theta 
69 
objTheta = atan2(ObjectVector(2), ObjectVector(1)); 
70 
objTheta = objTheta + theta; 
71 
else 
72 
sensor_k = sensor_model(:,:,71); 
73 

74 
%Use the maximum length to clear an empty area. 
75 
ObjectVector = 80 * Angle_IR(ir, :); 
76 
ObjectVector = ObjectVector + Bot_IR(ir, :); 
77 

78 
%rotate by theta 
79 
objTheta = atan2(ObjectVector(2), ObjectVector(1)); 
80 
objTheta = objTheta + theta; 
81 
end 
82  
83 
dx = x + Bot_IR(ir,1); 
84 
dy = y + Bot_IR(ir,2); 
85 

86 
translation_mat = [1, 0, 0; 
87 
0, 1, 0; 
88 
dx,dy, 1]; 
89  
90 
rotation_mat = [cos(objTheta), sin(objTheta), 0; 
91 
sin(objTheta), cos(objTheta), 0 
92 
0, 0, 1]; 
93  
94 
transformation_mat = rotation_mat * translation_mat; 
95 

96 
translation = maketform('affine', transformation_mat); 
97 

98 
sensor_k = imtransform(sensor_k, translation,... 
99 
'UData', udata, 'Vdata', vdata,... 
100 
'XData', xdata, 'YData', ydata,... 
101 
'Size', [map_width, map_height], 'FillValues', 1); 
102 

103 
arena = arena.*sensor_k; 
104 
end 
105 
end 
106  
107 
image(arena.*20) 
108 