-
Notifications
You must be signed in to change notification settings - Fork 0
/
epipolar_lines.m
122 lines (96 loc) · 3.07 KB
/
epipolar_lines.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
clear;
%pi = 3.1415;
%%
%Parameters
Theta = 75;%degfrom x axis toward neg z axis(toward world)
Phi = -15;%deg from x axis toward y (up)
Theta=-(Theta)*pi/180;
Phi=(Phi+90)*pi/180;
R = [1 0 0;
0 1 0;
0 0 1];
r0 = [-200;0;-20];
f = 6;
%%
x_lm = f/tan(Theta);
y_lm = f/tan(Phi);
a1 = R(1,1)*x_lm + R(1,2)*y_lm+R(1,3)*f;
a2 = R(2,1)*x_lm + R(2,2)*y_lm+R(2,3)*f;
a3 = R(3,1)*x_lm + R(3,2)*y_lm+R(3,3)*f;
z_l = [-100 -10 -2 -1 -0.1 0.1 1 2 10 100]';
x_cm = f*(a1*z_l+f*r0(1))./(a3*z_l+f*r0(3));
y_cm = f*(a2*z_l+f*r0(2))./(a3*z_l+f*r0(3));
%%
%Plotting
x_axis = R*[100;0;0]+r0;
y_axis = R*[0;100;0]+r0;
z_axis = R*[0;0;100]+r0;
ray = -1000*[a1;a2;a3]+[r0(1);r0(2);r0(3)];
figure(1)
subplot(1,3,1)
plot([0 100],[0 0],'LineWidth',2,'color','b')
hold on
plot([0 0],[0 100],'LineWidth',2,'color','b')
plot([r0(1) x_axis(1,1)],[r0(3) x_axis(3,1)],'LineWidth',2,'color','r')
plot([r0(1) z_axis(1,1)],[r0(3) z_axis(3,1)],'LineWidth',2,'color','r')
plot([r0(1) ray(1)],[r0(3) ray(3)],'LineWidth',2,'color','g')
title('Bottom view')
xlabel('X')
ylabel('Z')
axis([-250 100 -1000 100])
plotCamera('Location',[0 0 0],'Orientation',[1 0 0; 0 0 1; 0 -1 0],'Opacity',0,'color','b','size',30);
subplot(1,3,2)
plot([0 100],[0 0],'LineWidth',2,'color','b')
hold on
plot([0 0],[0 100],'LineWidth',2,'color','b')
plot([r0(3) z_axis(3,1)],[r0(2) z_axis(2,1)],'LineWidth',2,'color','r')
plot([r0(3) y_axis(3,1)],[r0(2) y_axis(2,1)],'LineWidth',2,'color','r')
plot([r0(3) ray(3)],[r0(2) ray(2)],'LineWidth',2,'color','g')
title('Side view')
xlabel('Z')
ylabel('Y')
axis([-1000 100 -250 250])
plotCamera('Location',[0 0 0],'Orientation',[0 0 1; 0 1 0; -1 0 0],'Opacity',0,'color','b','size',30);
% plot3([0 5],[0 0],[0 0],'LineWidth',2,'color','b')
% hold on
% grid on
% plot3([0 0],[0 5],[0 0],'LineWidth',2,'color','b')
% plot3([0 0],[0 0],[0 5],'LineWidth',2,'color','b')
% plot3([r0(1) x_axis(1,1)],[r0(2) x_axis(2,1)],[r0(3) x_axis(3,1)],'LineWidth',2,'color','r')
% plot3([r0(1) y_axis(1,1)],[r0(2) y_axis(2,1)],[r0(3) y_axis(3,1)],'LineWidth',2,'color','r')
% plot3([r0(1) z_axis(1,1)],[r0(2) z_axis(2,1)],[r0(3) z_axis(3,1)],'LineWidth',2,'color','r')
% plot3([r0(1) ray(1)],[r0(2) ray(2)],[r0(3) ray(3)],'LineWidth',2,'color','g')
% xlabel('X')
% ylabel('Y')
% zlabel('Z')
% axis([-10 10 0 20 -15 5])
% title('Camera(blue) and Laser(red) positions')
subplot(1,3,3)
plot(x_cm,y_cm,'LineWidth',2,'color','b')
hold on
plot(0,0,'o','LineWidth',2,'color','b')
axis([-50 50 -50 50])
xlabel('x_c [mm]')
ylabel('y_c [mm]')
title('Camera image plane')
plot(x_lm,y_lm,'x')
axis([-2.2*10^-3*2304/2 2.2*10^-3*2304/2 -2.2*10^-3*1536/2 2.2*10^-3*1536/2])
%%
%How far is it even between the lines
Theta = 20;%degfrom x axis toward neg z axis(toward world)
Phi = 50;%deg from x axis toward y (up)
Theta=-(Theta)*pi/180;
Phi=(Phi+90)*pi/180;
R = [1 0 0;
0 1 0;
0 0 1];
r0 = [-10;0;0];
f = 6;
%%
x_lm = f/tan(Theta);
y_lm = f/tan(Phi);
a1 = R(1,1)*x_lm + R(1,2)*y_lm+R(1,3)*f;
a2 = R(2,1)*x_lm + R(2,2)*y_lm+R(2,3)*f;
a3 = R(3,1)*x_lm + R(3,2)*y_lm+R(3,3)*f;
z_l = [-10 -2 -1 -0.1 0.1 1 2 10]';
y_ = f*(a2*z_l+f*r0(2))./(a3*z_l+f*r0(3));