root / trunk / code / projects / mapping / matlab / sensor_map.m @ 1586
History | View | Annotate | Download (3.34 KB)
1 | 1134 | justin | |
---|---|---|---|
2 | 1149 | justin | %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 | 1134 | justin | |
6 | %Input comes from 'filename' where each line contains 8 space-separated 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 | 1149 | justin | %All output measurements are in millimeters |
15 | 1134 | justin | |
16 | 1149 | justin | function arena = sensor_map(filename) |
17 | 1134 | justin | |
18 | 1210 | justin | %aviobj = avifile('test.avi'); |
19 | %aviobj.Quality = 80; |
||
20 | %aviobj.Compression = 'None'; |
||
21 | |||
22 | 1134 | justin | %Vectors from center of bot to IR sensors |
23 | Bot_IR1 = [4.5,3.5]; |
||
24 | Bot_IR2 = [7.5,0]; |
||
25 | Bot_IR3 = [4.5,-3.5]; |
||
26 | Bot_IR4 = [-4,-3.5]; |
||
27 | Bot_IR5 = [-4,3.5]; |
||
28 | Bot_IR = [Bot_IR1; Bot_IR2; Bot_IR3; Bot_IR4; Bot_IR5]; |
||
29 | |||
30 | %Unit vectors with the same angle that the IR sensors are tilted at |
||
31 | Angle_IR1 = [.78,.63]; |
||
32 | Angle_IR2 = [1,0]; |
||
33 | Angle_IR3 = [.78,-.63]; |
||
34 | Angle_IR4 = [0,-1]; |
||
35 | Angle_IR5 = [0,1]; |
||
36 | Angle_IR = [Angle_IR1; Angle_IR2; Angle_IR3; Angle_IR4; Angle_IR5]; |
||
37 | |||
38 | %Define sensor model. |
||
39 | |||
40 | 1148 | justin | [sensor_model, udata, vdata] = model_sensor; |
41 | 1134 | justin | |
42 | 1210 | justin | %Define map area |
43 | map_width = 1000; |
||
44 | map_height = 1000; |
||
45 | %xdata = [-1, 1] * map_width/2; |
||
46 | %ydata = [-1, 1] * map_height/2; |
||
47 | xdata = [-200, 800]; |
||
48 | ydata = [-400, 300]; |
||
49 | 1134 | justin | arena = ones(map_width,map_height); |
50 | |||
51 | %For now, read input in from a file |
||
52 | Data = dlmread(filename, ' '); |
||
53 | |||
54 | points = []; |
||
55 | %loop through each line of file |
||
56 | 1210 | justin | for i=25:size(Data, 1) |
57 | 1134 | justin | display(i); |
58 | row = Data(i, :); |
||
59 | x = row(1); |
||
60 | y = row(2); |
||
61 | theta = row(3); |
||
62 | 1210 | justin | |
63 | row(4:8) |
||
64 | 1134 | justin | for ir=1:5 |
65 | 1210 | justin | if (row(4:8) == [-1 -1 -1 -1 -1]) |
66 | break; |
||
67 | end |
||
68 | reading = row(ir+3); |
||
69 | distance = reading * 0.1; %Scale by 0.1 since range is in mm. |
||
70 | if reading ~= -1 |
||
71 | k_val = floor(distance); |
||
72 | 1134 | justin | sensor_k = sensor_model(:,:,k_val); |
73 | |||
74 | %Find vector of object relative to center of bot |
||
75 | ObjectVector = distance * 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 | else |
||
82 | sensor_k = sensor_model(:,:,71); |
||
83 | |||
84 | %Use the maximum length to clear an empty area. |
||
85 | ObjectVector = 80 * Angle_IR(ir, :); |
||
86 | ObjectVector = ObjectVector + Bot_IR(ir, :); |
||
87 | |||
88 | %rotate by theta |
||
89 | objTheta = atan2(ObjectVector(2), ObjectVector(1)); |
||
90 | objTheta = objTheta + theta; |
||
91 | end |
||
92 | |||
93 | dx = x + Bot_IR(ir,1); |
||
94 | dy = y + Bot_IR(ir,2); |
||
95 | |||
96 | translation_mat = [1, 0, 0; |
||
97 | 0, 1, 0; |
||
98 | dx,dy, 1]; |
||
99 | |||
100 | rotation_mat = [cos(objTheta), sin(objTheta), 0; |
||
101 | -sin(objTheta), cos(objTheta), 0 |
||
102 | 0, 0, 1]; |
||
103 | |||
104 | transformation_mat = rotation_mat * translation_mat; |
||
105 | 1210 | justin | transformation = maketform('affine', transformation_mat); |
106 | |||
107 | %Big time waster 1: TODO Speedups. |
||
108 | sensor_k = imtransform(sensor_k, transformation,... |
||
109 | 1134 | justin | 'UData', udata, 'Vdata', vdata,... |
110 | 'XData', xdata, 'YData', ydata,... |
||
111 | 'Size', [map_width, map_height], 'FillValues', 1); |
||
112 | 1210 | justin | |
113 | %Big time waster 2: TODO Speedups. |
||
114 | 1134 | justin | arena = arena.*sensor_k; |
115 | 1210 | justin | |
116 | %For when you want to show the image every round: |
||
117 | image(arena.*20); |
||
118 | drawnow; |
||
119 | |||
120 | %frame = getframe; |
||
121 | %aviobj = addframe(aviobj,frame.cdata); |
||
122 | 1134 | justin | end |
123 | end |
||
124 | |||
125 | 1210 | justin | %aviobj = close(aviobj); |
126 |