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

2 
%Function which creates a map of points seen by a robot 
3  
4 
%Input comes from 'filename' where each line contains 8 spaceseparated values 
5 
%Each line contains: x, y, theta, IR1, IR2, IR3, IR4, IR5 in that order 
6 
%Example line of input file: 
7 
%17 23 .45 1 42 1 1 1 
8 
%x and y are centimeters, theta is in radians 
9 
%IR values are the values seen by the robot, where 1 means that nothing is seen 
10  
11 
%Output: graph of points seen by robot 
12 
%All output measurements are in centimeters 
13  
14 
%TODO 
15 
%find conversion factor between IR sensor values and centimeters (currently 1 is being used) 
16 
function arena = 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 
%Definitions: 
37 
sensor_model_width = 120; 
38 
sensor_model_height = 400; 
39 
sensor_bounds_x = 30; 
40 
sensor_bounds_y = 100; 
41 
sensor_width = 8; %cm 
42 
empty_weight = 0.1; 
43 
wall_weight = 2.0; 
44 
outside_weight = 1; 
45  
46 
%Space creation: 
47 
Sx = linspace(sensor_bounds_x, sensor_bounds_x, sensor_model_width); 
48 
Sy = linspace(sensor_bounds_y, sensor_bounds_y, sensor_model_height); 
49 
sensor_model = zeros(size(Sx,1), size(Sy,1), 71); 
50  
51 
%k ranges over distances (cm) 
52 
for k=10:80 
53 
for j = 1:sensor_model_height; 
54 
for i = 1:sensor_model_width; 
55 
slope = k / sensor_width; 
56 
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k  2) && Sy(j) > 0 
57 
sensor_model(i,j,k) = empty_weight; 
58 
elseif abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k + 2) && Sy(j) > 0 
59 
sensor_model(i,j,k) = wall_weight / abs((k  Sy(j))); 
60 
else 
61 
sensor_model(i,j,k) = outside_weight; 
62 
end 
63 
end 
64 
end 
65 
end 
66  
67 
%Define the sensor model for when no measurement is made (1) 
68 
for j = 1:sensor_model_height; 
69 
for i = 1:sensor_model_width; 
70 
slope = 80 / sensor_width; 
71 
if abs(Sx(i)) < Sy(j) / slope && Sy(j) < 80 && Sy(j) > 0 
72 
sensor_model(i,j,k) = empty_weight; 
73 
else 
74 
sensor_model(i,j,k) = outside_weight; 
75 
end 
76 
end 
77 
end 
78  
79  
80 
%Define map area (1100cm by 1100cm) 
81 
map_width = 1100; 
82 
map_height = 1100; 
83 
udata = [1, 1] * sensor_model_width/2; 
84 
vdata = [1, 1] * sensor_model_height/2; 
85 
xdata = [1, 1] * map_width/2; 
86 
ydata = [1, 1] * map_height/2; 
87 
arena = ones(map_width,map_height); 
88  
89 
%For now, read input in from a file 
90 
Data = dlmread(filename, ' '); 
91  
92 
points = []; 
93 
%loop through each line of file 
94 
for i=1:size(Data, 1) 
95 
display(i); 
96 
row = Data(i, :); 
97 
x = row(1); 
98 
y = row(2); 
99 
theta = row(3); 
100  
101 
%look at each IR sensor reading 
102 
for ir=1:5 
103 
distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm 
104 
if distance ~= 1 
105 
k_val = floor(distance / 10); 
106 
sensor_k = sensor_model(:,:,k_val); 
107 

108 
%Find vector of object relative to center of bot 
109 
ObjectVector = distance * Angle_IR(ir, :); 
110 
ObjectVector = ObjectVector + Bot_IR(ir, :); 
111 

112 
%rotate by theta 
113 
objTheta = atan2(ObjectVector(2), ObjectVector(1)); 
114 
objTheta = objTheta + theta; 
115 
else 
116 
sensor_k = sensor_model(:,:,71); 
117 

118 
%Use the maximum length to clear an empty area. 
119 
ObjectVector = 80 * Angle_IR(ir, :); 
120 
ObjectVector = ObjectVector + Bot_IR(ir, :); 
121 

122 
%rotate by theta 
123 
objTheta = atan2(ObjectVector(2), ObjectVector(1)); 
124 
objTheta = objTheta + theta; 
125 
end 
126  
127 
dx = x + Bot_IR(ir,1); 
128 
dy = y + Bot_IR(ir,2); 
129 

130 
translation_mat = [1, 0, 0; 
131 
0, 1, 0; 
132 
dx,dy, 1]; 
133  
134 
rotation_mat = [cos(objTheta), sin(objTheta), 0; 
135 
sin(objTheta), cos(objTheta), 0 
136 
0, 0, 1]; 
137  
138 
transformation_mat = rotation_mat * translation_mat; 
139 

140 
translation = maketform('affine', transformation_mat); 
141 

142 
sensor_k = imtransform(sensor_k, translation,... 
143 
'UData', udata, 'Vdata', vdata,... 
144 
'XData', xdata, 'YData', ydata,... 
145 
'Size', [map_width, map_height], 'FillValues', 1); 
146 

147 
arena = arena.*sensor_k; 
148 
end 
149 
end 
150  
151 
image(arena.*20) 
152 