Assignment code for Georgia Tech CS 3630, 2014
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

giant commit of stuff before due date

james7132 d9669f58 43b150cf

+228 -92
Assignment5/dataset/log-1426087262.txt Assignment5/matlab/log-1426087262.txt
Assignment5/dataset/pic-1426087272.jpg Assignment5/matlab/pic-1426087272.jpg
Assignment5/dataset/pic-1426087275.jpg Assignment5/matlab/pic-1426087275.jpg
Assignment5/dataset/pic-1426087277.jpg Assignment5/matlab/pic-1426087277.jpg
Assignment5/dataset/pic-1426087280.jpg Assignment5/matlab/pic-1426087280.jpg
Assignment5/dataset/pic-1426087283.jpg Assignment5/matlab/pic-1426087283.jpg
Assignment5/dataset/pic-1426087285.jpg Assignment5/matlab/pic-1426087285.jpg
Assignment5/dataset/pic-1426087287.jpg Assignment5/matlab/pic-1426087287.jpg
Assignment5/dataset/pic-1426087289.jpg Assignment5/matlab/pic-1426087289.jpg
Assignment5/dataset/pic-1426087292.jpg Assignment5/matlab/pic-1426087292.jpg
Assignment5/dataset/pic-1426087294.jpg Assignment5/matlab/pic-1426087294.jpg
Assignment5/dataset/pic-1426087296.jpg Assignment5/matlab/pic-1426087296.jpg
Assignment5/dataset/pic-1426087299.jpg Assignment5/matlab/pic-1426087299.jpg
Assignment5/dataset/pic-1426087301.jpg Assignment5/matlab/pic-1426087301.jpg
Assignment5/matlab visual odo capture.PNG

This is a binary file and will not be displayed.

+60 -92
Assignment5/matlab/VisualOdo.m
··· 13 13 14 14 Camera = CentralCamera('name', 'robocam', 'default'); 15 15 16 - 17 - log = fopen('log-1427161855.txt'); 16 + log = fopen('log-run1.txt'); 18 17 data = textscan(log, '%d %d %s'); 18 + oldImage = 0; 19 + oldSurf = 0; 20 + oldCamera = 0; 19 21 imagesData = data{3}; 20 22 images = cell(0); 21 23 surfs = cell(0); 22 24 cameras = cell(0); 25 + matches = cell(0); 26 + solutions = cell(0); 23 27 x = zeros([1, 2]); 28 + x_hist = zeros(2); 24 29 for i = 1 : size(imagesData, 1) 25 - test = imagesData{i}; 26 - 30 + test = imagesData{i}; 27 31 v = [data{1}(i) 28 - data{2}(i) 29 - ]; 32 + data{2}(i) 33 + ]; 30 34 31 35 % get motor values (Left-Right wrpt Fluke) 32 36 uL=v(1); ··· 42 46 % calculate speed 43 47 u(1) = 0.04 * r.*((ScaledUL+ScaledUR)./2); 44 48 u(2) = 0.1875 * (r./L).*(ScaledUR-ScaledUL); 45 - 49 + 46 50 x = x + u; 51 + x_hist = [x_hist; x]; 47 52 if(~strcmp(test, '0')) 48 53 image = imread(test); 49 54 surf = isurf(image); 50 - camera = CentralCamera('image', image); 55 + camera = CentralCamera('image', image, 'default'); 56 + % figure 57 + % imshow(image); 58 + % hold on 59 + % plot(surf.u, surf.v, '*', 'color', 'red'); 60 + 61 + if(oldSurf ~= 0) 62 + match = oldSurf.match(surf, 'thresh', 4); 63 + % figure 64 + % idisp({oldImage, image}); 65 + % match.plot(); 66 + F = match.ransac(@fmatrix, 3e-2, 'verbose'); 67 + % figure 68 + % idisp({oldImage, image}); 69 + % match.inlier.plot('g'); 70 + %E = camera.E(oldCamera); 71 + E = transpose(K) * F * K; 72 + Q = [0 0 100]'; 73 + sol = camera.invE(E, Q); 74 + eulerR = tr2eul(sol(1:3, 1:3)) .* 180.0/pi; 75 + t = sol(1:3, 4); 76 + 77 + solutions = [solutions, {sol}]; 78 + matches = [matches, {match}]; 79 + end 80 + 81 + oldImage = image; 82 + oldSurf = surf; 83 + oldCamera = camera; 51 84 52 85 images = [images, {image}]; 53 86 surfs = [surfs; {surf}]; 54 87 cameras = [cameras, {camera}]; 55 88 end 56 89 end 57 - matches = cell(0); 58 - solutions = cell(0); 59 - for i = 1 : size(surfs, 1) - 1 60 - current = surfs{i}; 61 - next = surfs{i + 1}; 62 - match = current.match(next, 'thresh', 40); 63 - matches = [matches, {match}]; 64 - % figure 65 - % idisp(images(1, i:i+1)); 66 - % match.plot('w'); 67 - F = match.ransac(@fmatrix, 1e-1, 'verbose'); 68 - % E = K' * F * K; 69 - E = Camera.E(F); 70 - sol = Camera.invE(E); 71 - diff1 = sum(sum((sol(:,:,1) - inv(cameras{i}.T) * cameras{i + 1}.T).^2)); 72 - diff2 = sum(sum((sol(:,:,2) - inv(cameras{i}.T) * cameras{i + 1}.T).^2)); 73 - if(diff1 < diff2) 74 - sol = sol(:, :, 1); 75 - else 76 - sol = sol(:, :, 2); 77 - end 78 - solutions = [solutions, {sol}]; 79 - [R, t] = tr2rt(cameras{i}.T) 80 - fkine 81 - cameras{i}.plot_camera('color', 'r'); 82 - end 83 - 84 90 85 - % %% and a robot with noisy odometry 86 - % 87 - % V=diag([.1, .3*pi/180].^2); % default 0.1, 1.1 88 - % veh=GenericVehicle(V,'dt',0.1); 89 - % veh.x0 = [0,0, pi/2 * 2/3]; 90 - % driver = DeterministicPath('log-test (1).txt'); 91 - % veh.add_driver(driver); 92 - % 93 - % % Creating the map. It places landmarks according to 'A' matrix. 94 - % map = LandmarkMap(500, A, 5); 95 - % 96 - % % Creating the sensor. We firstly define the covariance of the sensor measurements 97 - % % which report distance and bearing angle 98 - % W = diag([0.1, 0.1*pi/180].^2); % default 0.1 1 99 - % 100 - % % and then use this to create an instance of the Sensor class. 101 - % sensor = GenericRangeBearingSensor(veh, map, W, 'animate'); 102 - % % Note that the sensor is mounted on the moving robot and observes the features 103 - % % in the world so it is connected to the already created Vehicle and Map objects. 104 - % 105 - % % Create the filter. First we need to determine the initial covariance of the 106 - % % vehicle, this is our uncertainty about its pose (x, y, theta) 107 - % P0 = diag([0.1, 0.1, 0.1 * pi/180].^2); % default .005 .005 .001s 108 - % 109 - % % Now we create an instance of the EKF filter class 110 - % ekf = GenericEKF(veh, V, P0, sensor, W, []); 111 - % % and connect it to the vehicle and the sensor and give estimates of the vehicle 112 - % % and sensor covariance (we never know this is practice). 113 - % 114 - % % Now we will run the filter for 1000 time steps. At each step the vehicle 115 - % % moves, reports its odometry and the sensor measurements and the filter updates 116 - % % its estimate of the vehicle's pose 117 - % ekf.run(569); %596 non-closure %870 closure 118 - % % all the results of the simulation are stored within the EKF object 119 - % 120 - % % First let's plot the map 121 - % clf; map.plot() 122 - % % and then overlay the path actually taken by the vehicle 123 - % veh.plot_xy('b'); 124 - % % and then overlay the path estimated by the filter 125 - % ekf.plot_xy('r'); 126 - % % which we see are pretty close 127 - % 128 - % % Now let's plot the error in estimating the pose 129 - % axis equal 130 - % figure 131 - % ekf.plot_error() 132 - % % and this is overlaid with the estimated covariance of the error. 133 - % 134 - % % Remember that the SLAM filter has not only estimated the robot's pose, it has 135 - % % simultaneously estimated the positions of the landmarks as well. How well did it 136 - % % do at that task? We will show the landmarks in the map again 137 - % figure 138 - % map.plot(); 139 - % % and this time overlay the estimated landmark (with a +) and the 3sigma 140 - % % uncertainty bounds as green ellipses 141 - % ekf.plot_map(3,'g'); 91 + currentTransform = [1,0,0,0; 92 + 0,1,0,0; 93 + 0,0,1,0; 94 + 0,0,0,1]; 95 + scale = [0.01,0,0,0; 96 + 0,0.01,0,0; 97 + 0,0,1,0; 98 + 0,0,0,1]; 99 + x = zeros(1); 100 + y = zeros(1); 101 + for i = 1 : size(solutions, 2) 102 + solutions{i} = solutions{i} * scale; 103 + end 104 + for i = 1 : size(solutions, 2) 105 + x = [x, currentTransform(1, 4)]; 106 + y = [y, currentTransform(2, 4)]; 107 + currentTransform = currentTransform * solutions{i}; 108 + end 109 + plot(x,y);
Assignment5/matlab/log-1427161855.txt Assignment5/matlab/log-run3.txt
+84
Assignment5/matlab/log-run1.txt
··· 1 + 1 1 0 2 + 1 1 0 3 + 1 1 0 4 + 1 1 0 5 + 1 1 0 6 + 1 1 0 7 + 1 1 0 8 + 1 1 0 9 + 1 1 0 10 + 1 1 0 11 + 0 0 0 12 + 0 0 pic-1427306080.jpg 13 + 0 1 0 14 + 0 1 0 15 + 0 0 0 16 + 0 0 pic-1427306081.jpg 17 + 0 1 0 18 + 0 1 0 19 + 0 0 0 20 + 0 0 pic-1427306083.jpg 21 + 0 1 0 22 + 0 1 0 23 + 0 0 0 24 + 0 0 pic-1427306084.jpg 25 + 1 1 0 26 + 1 1 0 27 + 1 1 0 28 + 1 1 0 29 + 1 1 0 30 + 1 1 0 31 + 1 1 0 32 + 1 1 0 33 + 1 1 0 34 + 1 1 0 35 + 0 0 0 36 + 0 0 pic-1427306087.jpg 37 + 1 0 0 38 + 1 0 0 39 + 0 0 0 40 + 0 0 pic-1427306088.jpg 41 + 1 0 0 42 + 1 0 0 43 + 0 0 0 44 + 0 0 pic-1427306090.jpg 45 + 1 0 0 46 + 1 0 0 47 + 0 0 0 48 + 0 0 pic-1427306092.jpg 49 + 1 1 0 50 + 1 1 0 51 + 1 1 0 52 + 1 1 0 53 + 1 1 0 54 + 1 1 0 55 + 1 1 0 56 + 1 1 0 57 + 1 1 0 58 + 1 1 0 59 + 0 0 0 60 + 0 0 pic-1427306094.jpg 61 + 0 1 0 62 + 0 1 0 63 + 0 0 0 64 + 0 0 pic-1427306095.jpg 65 + 0 1 0 66 + 0 1 0 67 + 0 0 0 68 + 0 0 pic-1427306097.jpg 69 + 0 1 0 70 + 0 1 0 71 + 0 0 0 72 + 0 0 pic-1427306098.jpg 73 + 1 1 0 74 + 1 1 0 75 + 1 1 0 76 + 1 1 0 77 + 1 1 0 78 + 1 1 0 79 + 1 1 0 80 + 1 1 0 81 + 1 1 0 82 + 1 1 0 83 + 0 0 0 84 + 0 0 pic-1427306101.jpg
+84
Assignment5/matlab/log-run2.txt
··· 1 + 1 1 0 2 + 1 1 0 3 + 1 1 0 4 + 1 1 0 5 + 1 1 0 6 + 1 1 0 7 + 1 1 0 8 + 1 1 0 9 + 1 1 0 10 + 1 1 0 11 + 0 0 0 12 + 0 0 pic-1427162650.jpg 13 + 0 1 0 14 + 0 1 0 15 + 0 0 0 16 + 0 0 pic-1427162652.jpg 17 + 0 1 0 18 + 0 1 0 19 + 0 0 0 20 + 0 0 pic-1427162653.jpg 21 + 0 1 0 22 + 0 1 0 23 + 0 0 0 24 + 0 0 pic-1427162655.jpg 25 + 1 1 0 26 + 1 1 0 27 + 1 1 0 28 + 1 1 0 29 + 1 1 0 30 + 1 1 0 31 + 1 1 0 32 + 1 1 0 33 + 1 1 0 34 + 1 1 0 35 + 0 0 0 36 + 0 0 pic-1427162657.jpg 37 + 1 0 0 38 + 1 0 0 39 + 0 0 0 40 + 0 0 pic-1427162658.jpg 41 + 1 0 0 42 + 1 0 0 43 + 0 0 0 44 + 0 0 pic-1427162660.jpg 45 + 1 0 0 46 + 1 0 0 47 + 0 0 0 48 + 0 0 pic-1427162661.jpg 49 + 1 1 0 50 + 1 1 0 51 + 1 1 0 52 + 1 1 0 53 + 1 1 0 54 + 1 1 0 55 + 1 1 0 56 + 1 1 0 57 + 1 1 0 58 + 1 1 0 59 + 0 0 0 60 + 0 0 pic-1427162663.jpg 61 + 0 1 0 62 + 0 1 0 63 + 0 0 0 64 + 0 0 pic-1427162665.jpg 65 + 0 1 0 66 + 0 1 0 67 + 0 0 0 68 + 0 0 pic-1427162666.jpg 69 + 0 1 0 70 + 0 1 0 71 + 0 0 0 72 + 0 0 pic-1427162668.jpg 73 + 1 1 0 74 + 1 1 0 75 + 1 1 0 76 + 1 1 0 77 + 1 1 0 78 + 1 1 0 79 + 1 1 0 80 + 1 1 0 81 + 1 1 0 82 + 1 1 0 83 + 0 0 0 84 + 0 0 pic-1427162670.jpg
Assignment5/matlab/pic-1427162650.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162652.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162653.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162655.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162657.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162658.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162660.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162661.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162663.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162665.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162666.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162668.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427162670.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306080.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306081.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306083.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306084.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306087.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306088.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306090.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306092.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306094.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306095.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306097.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306098.jpg

This is a binary file and will not be displayed.

Assignment5/matlab/pic-1427306101.jpg

This is a binary file and will not be displayed.