Project

General

Profile

Statistics
| Revision:

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 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
%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