Statistics
| Revision:

## root / trunk / code / projects / mapping / matlab / sensor_map.m @ 1135

 1 ```%Function which creates a map of points seen by a robot ``` ```%Input comes from 'filename' where each line contains 8 space-separated values ``` ```%Each line contains: x, y, theta, IR1, IR2, IR3, IR4, IR5 in that order ``` ```%Example line of input file: ``` ```%17 23 .45 -1 42 -1 -1 -1 ``` ```%x and y are centimeters, theta is in radians ``` ```%IR values are the values seen by the robot, where -1 means that nothing is seen ``` ```%Output: graph of points seen by robot ``` ```%All output measurements are in centimeters ``` ```%TODO ``` ```%find conversion factor between IR sensor values and centimeters (currently 1 is being used) ``` ```function arena = map(filename) ``` ```%Vectors from center of bot to IR sensors ``` ```Bot_IR1 = [4.5,3.5]; ``` ```Bot_IR2 = [7.5,0]; ``` ```Bot_IR3 = [4.5,-3.5]; ``` ```Bot_IR4 = [-4,-3.5]; ``` ```Bot_IR5 = [-4,3.5]; ``` ```Bot_IR = [Bot_IR1; Bot_IR2; Bot_IR3; Bot_IR4; Bot_IR5]; ``` ```%Unit vectors with the same angle that the IR sensors are tilted at ``` ```Angle_IR1 = [.78,.63]; ``` ```Angle_IR2 = [1,0]; ``` ```Angle_IR3 = [.78,-.63]; ``` ```Angle_IR4 = [0,-1]; ``` ```Angle_IR5 = [0,1]; ``` ```Angle_IR = [Angle_IR1; Angle_IR2; Angle_IR3; Angle_IR4; Angle_IR5]; ``` ```%Define sensor model. ``` ```%Definitions: ``` ```sensor_model_width = 60; ``` ```sensor_model_height = 100; ``` ```sensor_bounds_x = 30; ``` ```sensor_bounds_y = 100; ``` ```sensor_width = 8; %cm ``` ```empty_weight = 0.1; ``` ```wall_weight = 2.0; ``` ```outside_weight = 1; ``` ```%Space creation: ``` ```Sx = linspace(-sensor_bounds_x, sensor_bounds_x, sensor_model_width); ``` ```Sy = linspace(-sensor_bounds_y, sensor_bounds_y, sensor_model_height); ``` ```sensor_model = zeros(size(Sx,1), size(Sy,1), 71); ``` ```%k ranges over distances (cm) ``` ```for k=10:80 ``` ``` for j = 1:sensor_model_height; ``` ``` for i = 1:sensor_model_width; ``` ``` slope = k / sensor_width; ``` ``` if abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k - 2) && Sy(j) > 0 ``` ``` sensor_model(i,j,k) = empty_weight; ``` ``` elseif abs(Sx(i)) < Sy(j) / slope && Sy(j) < (k + 2) && Sy(j) > 0 ``` ``` sensor_model(i,j,k) = wall_weight / abs((k - Sy(j))); ``` ``` else ``` ``` sensor_model(i,j,k) = outside_weight; ``` ``` end ``` ``` end ``` ``` end ``` ```end ``` ```%Define the sensor model for when no measurement is made (-1) ``` ```for j = 1:sensor_model_height; ``` ``` for i = 1:sensor_model_width; ``` ``` slope = 80 / sensor_width; ``` ``` if abs(Sx(i)) < Sy(j) / slope && Sy(j) < 80 && Sy(j) > 0 ``` ``` sensor_model(i,j,k) = empty_weight; ``` ``` else ``` ``` sensor_model(i,j,k) = outside_weight; ``` ``` end ``` ``` end ``` ```end ``` ```%Define map area (1100cm by 1100cm) ``` ```map_width = 600; ``` ```map_height = 600; ``` ```udata = [-1, 1] * sensor_model_width/2; ``` ```vdata = [-1, 1] * sensor_model_height/2; ``` ```xdata = [-1, 1] * map_width/2; ``` ```ydata = [-1, 1] * map_height/2; ``` ```arena = ones(map_width,map_height); ``` ```%For now, read input in from a file ``` ```Data = dlmread(filename, ' '); ``` ```points = []; ``` ```%loop through each line of file ``` ```for i=1:size(Data, 1) ``` ``` display(i); ``` ``` row = Data(i, :); ``` ``` x = row(1); ``` ``` y = row(2); ``` ``` theta = row(3); ``` ``` %look at each IR sensor reading ``` ``` for ir=1:5 ``` ``` distance = row(ir+3) * 1; %TODO!!!!!!!!! replace 1 with a conversion factor to cm ``` ``` if distance ~= -1 ``` ``` k_val = floor(distance / 10); ``` ``` sensor_k = sensor_model(:,:,k_val); ``` ``` ``` ``` %Find vector of object relative to center of bot ``` ``` ObjectVector = distance * Angle_IR(ir, :); ``` ``` ObjectVector = ObjectVector + Bot_IR(ir, :); ``` ``` ``` ``` %rotate by theta ``` ``` objTheta = atan2(ObjectVector(2), ObjectVector(1)); ``` ``` objTheta = objTheta + theta; ``` ``` else ``` ``` sensor_k = sensor_model(:,:,71); ``` ``` ``` ``` %Use the maximum length to clear an empty area. ``` ``` ObjectVector = 80 * Angle_IR(ir, :); ``` ``` ObjectVector = ObjectVector + Bot_IR(ir, :); ``` ``` ``` ``` %rotate by theta ``` ``` objTheta = atan2(ObjectVector(2), ObjectVector(1)); ``` ``` objTheta = objTheta + theta; ``` ``` end ``` ``` dx = x + Bot_IR(ir,1); ``` ``` dy = y + Bot_IR(ir,2); ``` ``` ``` ``` translation_mat = [1, 0, 0; ``` ``` 0, 1, 0; ``` ``` dx,dy, 1]; ``` ``` rotation_mat = [cos(objTheta), sin(objTheta), 0; ``` ``` -sin(objTheta), cos(objTheta), 0 ``` ``` 0, 0, 1]; ``` ``` transformation_mat = rotation_mat * translation_mat; ``` ``` ``` ``` translation = maketform('affine', transformation_mat); ``` ``` ``` ``` sensor_k = imtransform(sensor_k, translation,... ``` ``` 'UData', udata, 'Vdata', vdata,... ``` ``` 'XData', xdata, 'YData', ydata,... ``` ``` 'Size', [map_width, map_height], 'FillValues', 1); ``` ``` ``` ``` arena = arena.*sensor_k; ``` ``` end ``` ```end ``` ```image(arena.*20) ```