root / trunk / code / projects / mapping / matlab / sensor_transform_test.m @ 1135
History | View | Annotate | Download (936 Bytes)
1 | 1135 | justin | |
---|---|---|---|
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); |