-
Notifications
You must be signed in to change notification settings - Fork 0
/
reconstruccion3D.py
162 lines (137 loc) · 6.88 KB
/
reconstruccion3D.py
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
import cv2
import os
import pcl
import numpy as np
from vista import *
import matplotlib.pyplot as plot
from mpl_toolkits.mplot3d import Axes3D
class Rescontruccion3D(object):
"""docstring for rescontruccion3D"""
vistas = []
camaras = []
pts3D= []
K = None
R = None
t = None
E = None
F = None
FMask = None
d = None
ruta = None
matcher = None
vistas_procesadas = []
matches_utilizados = []
def __init__(self, ruta, K, d):
self.matcher = cv2.BFMatcher(cv2.NORM_L1, True)
self.K = K
self.d = d
self.ruta = ruta
directorio = os.walk(self.ruta)
for root, dirs, files in directorio:
for fichero in files:
(nombreFichero, extension) = os.path.splitext(fichero)
if(extension == ".png" or extension == ".JPG"):
self.vistas.append(Vista(self.ruta+nombreFichero+extension))
self.camaras.append(np.hstack((np.eye(3), np.zeros((3, 1)))))
def inicializacion(self):
self.pts3D = self.construir_vista3D(0,1)
def construir_vista3D(self,indice_vista_uno,indice_vista_dos):
"""la inicializacion se realiza con las dos primera imagenes, apartir de ahi se
deberia ir incrementando la nube 3D a partir de las demas vistas"""
self.vistas_procesadas.append(self.vistas[indice_vista_uno])
self.vistas_procesadas.append(self.vistas[indice_vista_dos])
self.vistas[indice_vista_uno].buscar_feature(self.vistas[indice_vista_dos],self.matcher ) #la primera inicializacion
self.obtener_matriz_fundamental(self.vistas[indice_vista_uno].features[self.vistas[indice_vista_dos]].match_puntos_uno,self.vistas[indice_vista_uno].features[self.vistas[indice_vista_dos]].match_puntos_dos)
self.obtener_matriz_esencial()
puntos_homogenios_uno,puntos_homogenios_dos,self.matches_utilizados = self.vistas[indice_vista_uno].features[self.vistas[indice_vista_dos]].homogeneizar_puntos(self.K,self.FMask)
camara = self.obtener_camaras(puntos_homogenios_uno,puntos_homogenios_dos)
self.camaras.append(camara)
X1 = self.triangular(puntos_homogenios_uno,puntos_homogenios_dos,camara)
#X2 = self.triangular_linealself.pts3Dmente(puntos_homogenios_uno,puntos_homogenios_dos,camara)
#X3D = np.concatenate((X1,X1))
return np.array(X1)
def obtener_matriz_fundamental(self,puntos_clave_uno,puntos_clave_dos):
self.F,self.FMask = cv2.findFundamentalMat(puntos_clave_uno,puntos_clave_dos,cv2.FM_RANSAC, 0.1, 0.99)
def obtener_matriz_esencial(self):
self.E = self.K.T.dot(self.F).dot(self.K)
def enfrente_ambas_camaras(self,first_points, second_points, rot, trans):
rot_inv = rot
for first, second in zip(first_points, second_points):
first_z = np.dot(rot[0, :] - second[0]*rot[2, :],trans) / np.dot(rot[0, :] - second[0]*rot[2, :],second)
first_3d_point = np.array([first[0] * first_z,second[0] * first_z, first_z])
second_3d_point = np.dot(rot.T, first_3d_point) - np.dot(rot.T,trans)
if first_3d_point[2] < 0 or second_3d_point[2] < 0:
return False
return True
def obtener_camaras(self,puntos_homogenios_uno,puntos_homogenios_dos):
U,s,V= np.linalg.svd(self.E,full_matrices=False)
W = np.array([0.0, -1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,1.0]).reshape(3, 3)
self.R = U.dot(W).dot(V) # U*W*V.T --> otra biblografia
self.t = U[:,2]
if not self.enfrente_ambas_camaras(puntos_homogenios_uno,puntos_homogenios_dos,self.R, self.t):
# Second choice: R = U * W * Vt, T = -u_3
self.t = - U[:, 2]
if not self.enfrente_ambas_camaras(puntos_homogenios_uno,puntos_homogenios_dos,self.R, self.t):
# Third choice: R = U * Wt * Vt, T = u_3
self.R = U.dot(W.T).dot(V)
self.t = U[:, 2]
if not self.enfrente_ambas_camaras(puntos_homogenios_uno,puntos_homogenios_dos, self.R, self.t):
# Fourth choice: R = U * Wt * Vt, T = -u_3
self.t = - U[:, 2]
#self.camaras.append(np.hstack((self.R, self.t.reshape(3, 1))))
return np.hstack((self.R, self.t.reshape(3, 1)))
def triangulacion_lineal(self,u1, P1, u2, P2):
A = np.array([u1[0]*P1[2, 0] - P1[0, 0], u1[0]*P1[2, 1] - P1[0, 1],
u1[0]*P1[2, 2] - P1[0, 2], u1[1]*P1[2, 0] - P1[1, 0],
u1[1]*P1[2, 1] - P1[1, 1], u1[1]*P1[2, 2] - P1[1, 2],
u2[0]*P2[2, 0] - P2[0, 0], u2[0]*P2[2, 1] - P2[0, 1],
u2[0]*P2[2, 2] - P2[0, 2], u2[1]*P2[2, 0] - P2[1, 0],
u2[1]*P2[2, 1] - P2[1, 1],
u2[1]*P2[2, 2] - P2[1, 2]]).reshape(4, 3)
B = np.array([-(u1[0]*P1[2, 3] - P1[0, 3]),
-(u1[1]*P1[2, 3] - P1[1, 3]),
-(u2[0]*P2[2, 3] - P2[0, 3]),
-(u2[1]*P2[2, 3] - P2[1, 3])]).reshape(4, 1)
ret, X = cv2.solve(A, B, flags=cv2.DECOMP_SVD)
return X.reshape(1, 3)
def triangular(self,puntos_homogenios_uno,puntos_homogenios_dos,camara):
puntos_homogenios_uno = puntos_homogenios_uno.reshape(-1, 3)[:, :2]
puntos_homogenios_dos = puntos_homogenios_dos.reshape(-1,3)[:,:2]
triangulacion = cv2.triangulatePoints(self.camaras[0],camara,puntos_homogenios_uno.T,puntos_homogenios_dos.T).T
X = triangulacion[:, :3]/np.repeat(triangulacion[:, 3], 3).reshape(-1, 3)
return X
def triangular_linealmente(self,puntos_homogenios_uno,puntos_homogenios_dos,camara):
X = []
for i in range(puntos_homogenios_uno.shape[0]):
uf = np.linalg.inv(self.K) * puntos_homogenios_uno[i]
us = np.linalg.inv(self.K) * puntos_homogenios_dos[i]
x = self.triangulacion_lineal(uf[0],self.camaras[0],us[0],camara)
X.append(x[0])
return X
def graficar_nube(self):
# plot with matplotlib
Ys = self.pts3D[:, 0]
Zs = self.pts3D[:, 1]
Xs = self.pts3D[:, 2]
# ------------------------->
fig = plot.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(Xs, Ys, Zs, c='r', marker='o')
#ax.set_aspect('equal')
ax.set_xlabel('Y')
ax.set_ylabel('Z')
ax.set_zlabel('X')
plot.show()
def salvar_nube(self,nombre):
cloud = pcl.PointCloud(np.array(self.pts3D, dtype=np.float32))
fil = cloud.make_statistical_outlier_filter()
fil.set_mean_k (50)
fil.set_std_dev_mul_thresh (1.0)
cloud = fil.filter()
cloud.to_file(nombre)
return cloud
def multiples_imagenes():
for vista_nueva in self.vistas:
if not vista_nueva in vistas_procesadas:
for vista_procesada in vistas_procesadas:
vista_procesada.buscar_feature(vista_nueva,self.matcher)