root / trunk / code / projects / mapping / matlab / sensor_transform_test.m @ 1135
History | View | Annotate | Download (936 Bytes)
1 |
|
---|---|
2 |
sensor_model_width = 100; |
3 |
sensor_model_height = 100; |
4 |
|
5 |
map_width = 800; |
6 |
map_height = 800; |
7 |
arena = ones(map_width,map_height); |
8 |
|
9 |
sample_sensor = sensor_model(:,:, 60); |
10 |
|
11 |
%sample data: |
12 |
x = 200; |
13 |
y = 100; |
14 |
|
15 |
dx = x; |
16 |
dy = y; |
17 |
|
18 |
objTheta = pi; |
19 |
%objTheta = 0; |
20 |
|
21 |
translation_mat = [1, 0, 0; |
22 |
0, 1, 0; |
23 |
dx, dy, 1]; |
24 |
|
25 |
rotation_mat = [cos(objTheta), sin(objTheta), 0; |
26 |
-sin(objTheta), cos(objTheta), 0 |
27 |
0, 0, 1]; |
28 |
|
29 |
udata = [-1, 1] * sensor_model_width/2; |
30 |
vdata = [-1, 1] * sensor_model_height/2; |
31 |
xdata = [-1, 1] * map_width/2; |
32 |
ydata = [-1, 1] * map_height/2; |
33 |
|
34 |
transformation_mat = rotation_mat * translation_mat; |
35 |
translation = maketform('affine', transformation_mat); |
36 |
|
37 |
sensor = imtransform(sample_sensor, translation,... |
38 |
'UData', udata, 'Vdata', vdata,... |
39 |
'XData', xdata, 'YData', ydata,... |
40 |
'Size', [map_width, map_height], 'FillValues', 1); |
41 |
|
42 |
size(arena) |
43 |
size(sensor) |
44 |
|
45 |
arena = arena.*sensor; |
46 |
|
47 |
image(arena.*20); |