root / trunk / code / projects / mapping / matlab / sensor_map.m @ 1135
History | View | Annotate | Download (4.09 KB)
1 | 1134 | justin | |
---|---|---|---|
2 | %Function which creates a map of points seen by a robot |
||
3 | |||
4 | %Input comes from 'filename' where each line contains 8 space-separated 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 | 1135 | justin | sensor_model_width = 60; |
38 | sensor_model_height = 100; |
||
39 | 1134 | justin | 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 | 1135 | justin | map_width = 600; |
82 | map_height = 600; |
||
83 | 1134 | justin | 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) |