Project

General

Profile

Revision 1134

Added a heavily modified version of map.m called sensor_map, which first constructs a sensor model, then maps data according to the sensor model.
NEEDS to be converted to log-odds to speed up computation. There is still a transformation that needs to take place per datapoint per sensor, but
theres not much I think I can do about that.

View differences:

trunk/code/projects/mapping/matlab/sensor_map.m
1

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

  
trunk/code/projects/mapping/matlab/prob_map.m
35 35

  
36 36
points = [];
37 37

  
38
%Map area:
39
[X1,X2] = meshgrid(-100:5:100,-100:5:100);
40
X = [X1(:),X2(:)];
41
prob_map = zeros(size(X1,1),size(X2,2));
38
%Map area, defined in centimeters:
39
[X,Y] = meshgrid(-100:2:100,-100:2:100);
40
%V = [X1(:),X2(:)];
41
prob_map = zeros(size(X,1),size(Y,2));
42 42

  
43

  
43 44
%Set position and range error constant for testing FIXME!
44 45
range_error = 250;
45 46
pos_error = 500;
trunk/code/projects/mapping/matlab/Makefile
11 11
	 mex -g receive.c -L../../libwireless/lib -lwireless
12 12

  
13 13
clean:
14
	rm *.o 
14
	rm *.mexglx
15 15

  

Also available in: Unified diff