Revision 1070
First attempt at a probability density mapper. Unifinished.
trunk/code/projects/mapping/matlab/prob_map.m | ||
---|---|---|
1 |
%Function which creates a heat map of wall location probabilities. |
|
2 |
|
|
3 |
%Input comes from 'filename' where each line contains 8 space-separated values |
|
4 |
%Each line contains: x, y, theta, IR1, IR2, IR3, IR4, IR5 in that order |
|
5 |
%Example line of input file: |
|
6 |
%17 23 .45 -1 42 -1 -1 -1 |
|
7 |
%x and y are centimeters, theta is in radians |
|
8 |
%IR values are the values seen by the robot, where -1 means that nothing is seen |
|
9 |
|
|
10 |
%Output: probability density map. |
|
11 |
%All output measurements are in centimeters |
|
12 |
|
|
13 |
%TODO |
|
14 |
%find conversion factor between IR sensor values and centimeters (currently 1 is being used) |
|
15 |
function map(filename) |
|
16 |
|
|
17 |
%Vectors from center of bot to IR sensors |
|
18 |
Bot_IR1 = [4.5,3.5]; |
|
19 |
Bot_IR2 = [7.5,0]; |
|
20 |
Bot_IR3 = [4.5,-3.5]; |
|
21 |
Bot_IR4 = [-4,-3.5]; |
|
22 |
Bot_IR5 = [-4,3.5]; |
|
23 |
Bot_IR = [Bot_IR1; Bot_IR2; Bot_IR3; Bot_IR4; Bot_IR5]; |
|
24 |
|
|
25 |
%Unit vectors with the same angle that the IR sensors are tilted at |
|
26 |
Angle_IR1 = [.78,.63]; |
|
27 |
Angle_IR2 = [1,0]; |
|
28 |
Angle_IR3 = [.78,-.63]; |
|
29 |
Angle_IR4 = [0,-1]; |
|
30 |
Angle_IR5 = [0,1]; |
|
31 |
Angle_IR = [Angle_IR1; Angle_IR2; Angle_IR3; Angle_IR4; Angle_IR5]; |
|
32 |
|
|
33 |
%For now, read input in from a file |
|
34 |
Data = dlmread(filename, ' '); |
|
35 |
|
|
36 |
points = []; |
|
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)); |
|
42 |
|
|
43 |
%Set position and range error constant for testing FIXME! |
|
44 |
range_error = 250; |
|
45 |
pos_error = 500; |
|
46 |
|
|
47 |
%loop through each line of file |
|
48 |
i = 1; ir = 1; |
|
49 |
%for i=1:size(Data, 1) |
|
50 |
row = Data(i, :); |
|
51 |
x = row(1); |
|
52 |
y = row(2); |
|
53 |
theta = row(3); |
|
54 |
%look at each IR sensor reading |
|
55 |
%for ir=1:5 |
|
56 |
%if row(ir+3) ~= -1 |
|
57 |
%Find vector of object relative to center of bot |
|
58 |
distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm |
|
59 |
ObjectVector = distance * Angle_IR(ir, :); |
|
60 |
ObjectVector = ObjectVector + Bot_IR(ir, :); |
|
61 |
|
|
62 |
%rotate by theta |
|
63 |
objR = sqrt(ObjectVector(1)^2 + ObjectVector(2)^2); |
|
64 |
objTheta = atan2(ObjectVector(2), ObjectVector(1)); |
|
65 |
objTheta = objTheta + theta; |
|
66 |
|
|
67 |
%add absolute position and shift into image |
|
68 |
objX = (objR * cos(objTheta)) + x + 1500; |
|
69 |
objY = (objR * sin(objTheta)) + y + 1500; |
|
70 |
ObjectVector = [objX, objY]; |
|
71 |
|
|
72 |
%Calculate distribution for this point: |
|
73 |
|
|
74 |
%Range variance in y direction, rotated by the object angle. |
|
75 |
rotation = [cos(objTheta),sin(objTheta); -sin(objTheta),cos(objTheta)]; |
|
76 |
range_error_cov = inv(rotation)*[0,0;0,range_error]*rotation; |
|
77 |
|
|
78 |
%Position variance in the robot direction |
|
79 |
%(TODO introduce error in position estimation perpendicular to the robot) |
|
80 |
rotation = [cos(theta),sin(theta); -sin(theta),cos(theta)]; |
|
81 |
position_error_cov = inv(rotation)*[0,0;0,pos_error]*rotation; |
|
82 |
|
|
83 |
covar = position_error_cov + range_error_cov; |
|
84 |
|
|
85 |
dist = reshape(mvnpdf(X,[x,y],covar), size(X1,1),size(X2,2)) |
|
86 |
surf(X1,X2,dist); |
|
87 |
colormap default; |
|
88 |
return; |
|
89 |
|
|
90 |
%Using the values of the pdf as an approximation for the prob. density map |
|
91 |
%(needs to change) update as: |
|
92 |
prob_map = prob_map.*(1-dist) + dist; |
|
93 |
%end |
|
94 |
%end |
|
95 |
%end |
|
96 |
|
|
97 |
%graph the probability map. |
|
98 |
surf(X1,X2,prob_map); |
|
99 |
|
|
100 |
|
Also available in: Unified diff