Assignment code for Georgia Tech CS 3630, 2014
giant commit of stuff before due date
+228
-92
Assignment5/dataset/log-1426087262.txt
Assignment5/matlab/log-1426087262.txt
Assignment5/dataset/log-1426087262.txt
Assignment5/matlab/log-1426087262.txt
Assignment5/dataset/pic-1426087272.jpg
Assignment5/matlab/pic-1426087272.jpg
Assignment5/dataset/pic-1426087272.jpg
Assignment5/matlab/pic-1426087272.jpg
Assignment5/dataset/pic-1426087275.jpg
Assignment5/matlab/pic-1426087275.jpg
Assignment5/dataset/pic-1426087275.jpg
Assignment5/matlab/pic-1426087275.jpg
Assignment5/dataset/pic-1426087277.jpg
Assignment5/matlab/pic-1426087277.jpg
Assignment5/dataset/pic-1426087277.jpg
Assignment5/matlab/pic-1426087277.jpg
Assignment5/dataset/pic-1426087280.jpg
Assignment5/matlab/pic-1426087280.jpg
Assignment5/dataset/pic-1426087280.jpg
Assignment5/matlab/pic-1426087280.jpg
Assignment5/dataset/pic-1426087283.jpg
Assignment5/matlab/pic-1426087283.jpg
Assignment5/dataset/pic-1426087283.jpg
Assignment5/matlab/pic-1426087283.jpg
Assignment5/dataset/pic-1426087285.jpg
Assignment5/matlab/pic-1426087285.jpg
Assignment5/dataset/pic-1426087285.jpg
Assignment5/matlab/pic-1426087285.jpg
Assignment5/dataset/pic-1426087287.jpg
Assignment5/matlab/pic-1426087287.jpg
Assignment5/dataset/pic-1426087287.jpg
Assignment5/matlab/pic-1426087287.jpg
Assignment5/dataset/pic-1426087289.jpg
Assignment5/matlab/pic-1426087289.jpg
Assignment5/dataset/pic-1426087289.jpg
Assignment5/matlab/pic-1426087289.jpg
Assignment5/dataset/pic-1426087292.jpg
Assignment5/matlab/pic-1426087292.jpg
Assignment5/dataset/pic-1426087292.jpg
Assignment5/matlab/pic-1426087292.jpg
Assignment5/dataset/pic-1426087294.jpg
Assignment5/matlab/pic-1426087294.jpg
Assignment5/dataset/pic-1426087294.jpg
Assignment5/matlab/pic-1426087294.jpg
Assignment5/dataset/pic-1426087296.jpg
Assignment5/matlab/pic-1426087296.jpg
Assignment5/dataset/pic-1426087296.jpg
Assignment5/matlab/pic-1426087296.jpg
Assignment5/dataset/pic-1426087299.jpg
Assignment5/matlab/pic-1426087299.jpg
Assignment5/dataset/pic-1426087299.jpg
Assignment5/matlab/pic-1426087299.jpg
Assignment5/dataset/pic-1426087301.jpg
Assignment5/matlab/pic-1426087301.jpg
Assignment5/dataset/pic-1426087301.jpg
Assignment5/matlab/pic-1426087301.jpg
Assignment5/matlab visual odo capture.PNG
Assignment5/matlab visual odo capture.PNG
This is a binary file and will not be displayed.
+60
-92
Assignment5/matlab/VisualOdo.m
+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
Assignment5/matlab/log-1427161855.txt
Assignment5/matlab/log-run3.txt
+84
Assignment5/matlab/log-run1.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
+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
Assignment5/matlab/pic-1427162650.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162652.jpg
Assignment5/matlab/pic-1427162652.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162653.jpg
Assignment5/matlab/pic-1427162653.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162655.jpg
Assignment5/matlab/pic-1427162655.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162657.jpg
Assignment5/matlab/pic-1427162657.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162658.jpg
Assignment5/matlab/pic-1427162658.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162660.jpg
Assignment5/matlab/pic-1427162660.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162661.jpg
Assignment5/matlab/pic-1427162661.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162663.jpg
Assignment5/matlab/pic-1427162663.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162665.jpg
Assignment5/matlab/pic-1427162665.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162666.jpg
Assignment5/matlab/pic-1427162666.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162668.jpg
Assignment5/matlab/pic-1427162668.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427162670.jpg
Assignment5/matlab/pic-1427162670.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306080.jpg
Assignment5/matlab/pic-1427306080.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306081.jpg
Assignment5/matlab/pic-1427306081.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306083.jpg
Assignment5/matlab/pic-1427306083.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306084.jpg
Assignment5/matlab/pic-1427306084.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306087.jpg
Assignment5/matlab/pic-1427306087.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306088.jpg
Assignment5/matlab/pic-1427306088.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306090.jpg
Assignment5/matlab/pic-1427306090.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306092.jpg
Assignment5/matlab/pic-1427306092.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306094.jpg
Assignment5/matlab/pic-1427306094.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306095.jpg
Assignment5/matlab/pic-1427306095.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306097.jpg
Assignment5/matlab/pic-1427306097.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306098.jpg
Assignment5/matlab/pic-1427306098.jpg
This is a binary file and will not be displayed.
Assignment5/matlab/pic-1427306101.jpg
Assignment5/matlab/pic-1427306101.jpg
This is a binary file and will not be displayed.