-
Notifications
You must be signed in to change notification settings - Fork 0
/
PinholeCamera.m_save
204 lines (161 loc) · 6.68 KB
/
PinholeCamera.m_save
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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
%% Modelado de la cámara PinHole
close all
clc
clear all
% En coordenadas homogéneas
C = [0,0,0,1]';C1 = [0,0,1,1];
PlanoProyeccion = [0 0 -1 1]';
% Coordenadas del Centro de visión (cámara)
C = [0,0,0,1];
C1 = [0,0,1,1];
plot3([C(1) C1(1)],[C(2) C1(2)],[C(3) C1(3)]);
scatter3(C(1),C(2),C(3),'rx','LineWidth',4);
hold on;
% Plano de proyección Z = 1;
[PX,PY] = meshgrid(linspace(-1,1,50),linspace(-1,1,50));
PZ = -(PlanoProyeccion(1)*PX + PlanoProyeccion(2)*PY + PlanoProyeccion(4))/PlanoProyeccion(3);
surf(PX,PY,PZ,0.5*ones(size(PZ)),'EdgeAlpha',0,'FaceAlpha',0.1,'FaceColor','b');
% Para un punto P
P = [0.5 0.5 2 1]';
% Su proyección vendrá dada según la distancia f del punto C al plano
fx = 1;
fy = 1;
K = [fx 0 0 0; 0 fy 0 0; 0 0 1 0];
Pproyectado = (K*P)';
Pproyectado = Pproyectado/Pproyectado(3);
scatter3(P(1),P(2),P(3),'gx','LineWidth',4);
scatter3(Pproyectado(1),Pproyectado(2),Pproyectado(3),'gx','LineWidth',4);
plot3([P(1) C(1)],[P(2) C(2)],[P(3) C(3)],'g')
axis equal
hold off
% Ahora para una figura cualquiera
N = 100;
t = linspace(5,25,N);
X = 10*cos(t)./(2*t);
Y = 10*sin(t)./(2*t);
Z = 2*sin(1.05*X)./X;
espiral = [X;Y;Z;ones(size(X))]';
plot3(espiral(:,1),espiral(:,2),espiral(:,3),'LineWidth',2);
grid on
hold on
% Modelo cámara
plot3([C(1) C1(1)],[C(2) C1(2)],[C(3) C1(3)]);
scatter3(C(1),C(2),C(3),'rx','LineWidth',4);
surf(PX,PY,PZ,0.5*ones(size(PZ)),'EdgeAlpha',0,'FaceAlpha',0.1,'FaceColor','g');
% Dibujamos las líneas de proyección
for i=1:5:length(espiral)
A = espiral(i,:);
plot3([A(1) C(1)],[A(2) C(2)],[A(3) C(3)],'-.b','LineWidth',0.5);
end
% Calculamos con K los puntos proyectados
Espiral_proyectada = (K*espiral')';
Espiral_proyectada = Espiral_proyectada./repmat(Espiral_proyectada(:,3),[1,3]);
plot3(Espiral_proyectada(:,1),Espiral_proyectada(:,2),Espiral_proyectada(:,3),'r','LineWidth',2);
hold off
axis equal
%% Ahora vamos a meter un offset a la cámara para que concuerde con los píxeles de la imagen proyectada
% Supongamos que el centro de la proyección de la cámara está en:
Pcentro = [0.4 -0.5 1];
px = Pcentro(1);
py = Pcentro(2);
% Ahora K
K = [fx 0 px 0; 0 fy py 0; 0 0 1 0];
% Figura
plot3(espiral(:,1),espiral(:,2),espiral(:,3),'LineWidth',2);
grid on
hold on
% Modelo cámara
plot3([C(1) C1(1)],[C(2) C1(2)],[C(3) C1(3)]);
scatter3(C(1),C(2),C(3),'rx','LineWidth',4);
surf(PX,PY,PZ,0.5*ones(size(PZ)),'EdgeAlpha',0,'FaceAlpha',0.1,'FaceColor','g');
Espiral_proyectada2 = (K*espiral')';
Espiral_proyectada2 = Espiral_proyectada2./repmat(Espiral_proyectada2(:,3),[1,3]);
plot3(Espiral_proyectada2(:,1),Espiral_proyectada2(:,2),Espiral_proyectada2(:,3),'r','LineWidth',2);
hold off
axis equal
%% Ahora movamos la cámara a otro sitio y comprobemos la proyección y el modelo
% de parámetros intrínsecos junto con el de extrínsecos.
close all
% Supongamos que la cámara se ha movido a otra posición /traslación
T = [0.5 0.5 0.5]';
% Podemos aplicarlo a la matriz de calibración invirtiendo la traslación
TrMat = [eye(3) T; 0 0 0 1];
% Aplicamos la transformación al modelo de la cámara
C_tras = TrMat*C';
C1_tras = TrMat*C1';
% Nótese que para mover el plano se aplica el inverso de la transformada de la matriz de transformación para un plano DUALIDAD
Plano_tras = (TrMat')\PlanoProyeccion;
PZ_tras = -(Plano_tras(1)*PX + Plano_tras(2)*PY + Plano_tras(4))/Plano_tras(3);
plot3([C(1) C1(1)],[C(2) C1(2)],[C(3) C1(3)],'r');
hold on
scatter3(C(1),C(2),C(3),'rx','LineWidth',4);
surf(PX,PY,PZ,0.5*ones(size(PZ)),'EdgeAlpha',0,'FaceAlpha',0.2,'FaceColor','r');
plot3([C_tras(1) C1_tras(1)],[C_tras(2) C1_tras(2)],[C_tras(3) C1_tras(3)],'b');
scatter3(C_tras(1),C_tras(2),C_tras(3),'bx','LineWidth',4);
surf(PX,PY,PZ_tras,0.5*ones(size(PZ_tras)),'EdgeAlpha',0,'FaceAlpha',0.2,'FaceColor','b');
% Ahora hay que aplicar la transformación inversa al objeto antes de
% transformarlo
% K volvemos a K sin movimiento
px = 0;
py = 0;
K = [fx 0 px 0; 0 fy py 0; 0 0 1 0;0 0 0 1];
% Invertimos la transformación
invTrMat = inv(TrMat);
% La nueva matriz de modelado de la proyección es:
% Primero trasladamos el objeto y luego le aplicamos el modelo
P = K*invTrMat;
plot3(espiral(:,1),espiral(:,2),espiral(:,3),'LineWidth',2);
% Se crea la proyección
Espiral_proyectada3 = (P*espiral')';
% Se transforman a coordenadas de la imagen
Espiral_proyectada3(:,1:3) = Espiral_proyectada3(:,1:3)./repmat(Espiral_proyectada3(:,3),[1,3]);
% Se transforman las coordenadas de la imagen a las del plano moviendo la
% imagen del plano Z a la nueva posición siguiendo el movimiento de la
% cámara
Espiral_proyectada3 = (TrMat*Espiral_proyectada3')';
plot3(Espiral_proyectada3(:,1),Espiral_proyectada3(:,2),Espiral_proyectada3(:,3),'r','LineWidth',2);
hold off
axis equal
%% Probemos ahora a mover la cámara libremente sobre el espacio.
close all
rots = [pi/6 0 0]';
T = [-0.2 -0.2 0.2]';
Rt = [cos(rots(3)) sin(rots(3)) 0 ; -sin(rots(3)) cos(rots(3)) 0 ; 0 0 1] * ...
[cos(rots(2)) 0 sin(rots(2)) ;0 1 0 ; -sin(rots(2)) 0 cos(rots(2))] * ...
[1 0 0 ; 0 cos(rots(1)) sin(rots(1)) ; 0 -sin(rots(1)) cos(rots(1))];
% Primero rotamos y luego trasladamos
TrMat = [eye(3) T; 0 0 0 1]*[Rt zeros(3,1);0 0 0 1];
% K volvemos a K sin movimiento
px = 0;
py = 0;
K = [fx 0 px 0; 0 fy py 0; 0 0 1 0;0 0 0 1];
% Aplicamos la transformación al modelo de la cámara
C_tras = TrMat*C';
C1_tras = TrMat*C1';
% Nótese que para mover el plano se aplica el inverso de la transformada de la matriz de transformación para un plano DUALIDAD
Plano_tras = (TrMat')\PlanoProyeccion;
PZ_tras = -(Plano_tras(1)*PX + Plano_tras(2)*PY + Plano_tras(4))/Plano_tras(3);
plot3([C(1) C1(1)],[C(2) C1(2)],[C(3) C1(3)],'r');
hold on
scatter3(C(1),C(2),C(3),'rx','LineWidth',4);
surf(PX,PY,PZ,0.5*ones(size(PZ)),'EdgeAlpha',0,'FaceAlpha',0.2,'FaceColor','r');
plot3([C_tras(1) C1_tras(1)],[C_tras(2) C1_tras(2)],[C_tras(3) C1_tras(3)],'b');
scatter3(C_tras(1),C_tras(2),C_tras(3),'bx','LineWidth',4);
surf(PX,PY,PZ_tras,0.5*ones(size(PZ_tras)),'EdgeAlpha',0,'FaceAlpha',0.2,'FaceColor','b');
% Invertimos la transformación
invTrMat = [Rt' -Rt'*T; 0 0 0 1];
% La nueva matriz de modelado de la proyección es:
% Primero trasladamos el objeto y luego le aplicamos el modelo
P = K*invTrMat;
plot3(espiral(:,1),espiral(:,2),espiral(:,3),'LineWidth',2);
% Se crea la proyección
Espiral_proyectada3 = (P*espiral')';
% Se transforman a coordenadas de la imagen
Espiral_proyectada3(:,1:3) = Espiral_proyectada3(:,1:3)./repmat(Espiral_proyectada3(:,3),[1,3]);
% Se transforman las coordenadas de la imagen a las del plano moviendo la
% imagen del plano Z a la nueva posición siguiendo el movimiento de la
% cámara
Espiral_proyectada3 = (TrMat*Espiral_proyectada3')';
plot3(Espiral_proyectada3(:,1),Espiral_proyectada3(:,2),Espiral_proyectada3(:,3),'r','LineWidth',2);
hold off
axis equal