def __init__(
self,
models,
angs=30,
wmax=10,
verbose=False,
):
model, self.target, self.match = mkMatcher(angs, wmax)
self.mod = [model(m) for m in readModels(models)]
for k, m in enumerate(self.mod):
m.id = k
try:
locs = np.loadtxt(models + '/locations.txt').reshape(-1, 7)
if verbose:
print(locs)
T = []
for ax, ay, az, dx, dy, dz, s in locs:
t = rotation(vec(1, 0, 0), np.radians(ax), homog=True)
t = rotation(vec(0, 1, 0), np.radians(ay), homog=True) @ t
t = rotation(vec(0, 0, 1), np.radians(az), homog=True) @ t
t = scale(vec(s, s, s)) @ t
t = desp(vec(dx, dy, dz)) @ t
T.append(t)
except:
T = [np.eye(4) for _ in self.mod]
self.p3d = [
move(t, (m.original * [[1, -1]])) for t, m in zip(T, self.mod)
]
if verbose:
print(self.p3d)
def prepare_RA(w, size, fov, dist=20):
WIDTH, HEIGHT = size
# Ponemos el punto de vista de la visualización gráfica en el origen del sistema
# de referencia, con elevación y azimuth de modo que el eje x apunte hacia la
# derecha, el eje y hacia abajo y el eje z mirando hacia delante.
# Es la posición "inicial" de una cámara en el origen.
# (ponemos distancia > 0 que luego se compensa porque cero da problemas)
setCameraPosition(w, distance=DC, elevation=-90, azimuth=-90, fov=fov)
# Preparamos el objeto textura que contendrá la imagen de cámara en vivo. La vamos
# a situar centrada delante del punto de vista del visor gráfico, a la distancia
# justa para que ocupe toda la ventana, teniendo en cuenta el FOV. Es el fondo
# de la escena, delante pondremos los objetos virtuales.
W2 = WIDTH / 2
d = dist
s = (d + DC) / W2 * np.tan(np.radians(fov) / 2)
camera_image = gl.GLImageItem(data=np.zeros([100, 100, 4]))
transform(
scale((s, s, 1)) @ desp((-WIDTH // 2, -HEIGHT // 2, d)), camera_image)
w.addItem(camera_image)
def update(frame):
camera_image.setData(data=img2tex(frame.transpose(1, 0, 2)))
return update
def update():
key, img = next(stream)
g = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
cs = extractContours(g, minarea=5, reduprec=2)
good = polygons(cs, 6, 3)
poses = []
for c in good:
p = bestPose(K, c, marker)
if p.rms < 2:
poses += [p.M]
#cv.polylines(img,[c],True,(255,255,0),3)
if poses:
M = poses[0]
# hasta aquí todo es igual que antes.
# Tenemos la matriz de cámara M
# sacamos la tranformación que nos permite situar en 3D
# el esqueleto de la cámara, sus ejes, y la imagen que se ve en ella
T = cameraTransf(M)
transform(T, axc)
cam.setData(pos=htrans(T, drawCam))
view.setData(data=img2tex(img))
m = T @ A
transform(m, view)
# A partir de la matriz de cámara sacamos la homografía del plano
# (esto también se puede hace como en el capítulo anterior)
# La homografía del plano z=0 se puede extraer de la matriz de cámara
# simplemente quitando la 3 columna (la que corresponde a la coordenada z).
# Hay que escalar para que encaje con el tamaño de la escena 3D.
s = 1 / 50
S = scale((s, s))
HZ0 = desp((250, 250)) @ la.inv(M[:, [0, 1, 3]] @ S)
# rectificamos la imagen
rectif = cv.warpPerspective(img, HZ0, (500, 500))
# la situamos en la escena con la escala adecuada
world.setData(data=img2tex(rectif))
transform(scale((s, s, 1)) @ desp((-250, -250, 0)), world)
cv.imshow('input', img)
def errorEllipse(e, c, mindiam=20, minratio=0.2):
(cx, cy), (A, B), ang = e
if A < mindiam: return 2000
if B / A < minratio: return 1000
T = la.inv(desp((cx, cy)) @ rot3(np.radians(ang)) @ scale((A / 2, B / 2)))
cc = htrans(T, c)
r = np.sqrt((cc * cc).sum(axis=1))
return abs(r - 1).max()
def mkTexture(w, img, T):
W = img.shape[1]
if img.shape[2] == 3:
obj = gl.GLImageItem(data=img2tex(img.transpose(1, 0, 2)))
else:
obj = gl.GLImageItem(data=img2texA(img.transpose(1, 0, 2)))
w.addItem(obj)
S = scale((1 / W, 1 / W, 1))
D = desp((0, 0, -DC))
def update(H):
transform(D @ H @ T @ S, obj)
return update
def update():
key, frame = next(stream)
#p.setData(z=getframe())
imagegl.setData(data=img2tex(frame))
s = 1 / 50
transform(scale((s, s, 1)) @ desp((0, 0, 1)), imagegl)
def rots(c):
return [np.roll(c, k, axis=0) for k in range(len(c))]
def bestRot(c):
return min([(errorMarker(r), r) for r in rots(c)])
for key, frame in autoStream():
g = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
cs = extractContours(g, minarea=5)
good = polygons(cs, n=6, prec=3)
ok = [(c, H) for (err, H), c in map(bestRot, good) if err < 0.1]
if ok:
# cogemos la homografía de rectificación del primer marcador detectado
c, H = ok[0]
# La combinamos con un escalado y desplazamiento para que la imagen
# resultante quede de un tamaño adecuado
T = desp([100, 100]) @ scale([100, 100]) @ H
# rectificamos
rectif = cv.warpPerspective(frame, T, (500, 500))
cv.imshow('rectif', rectif)
K = Kfov(size, fov)
# y para generar función de opengl para visualización la imagen de cámara en vivo
update_view = prepare_RA(w, size, fov)
# Ya solo queda transformar la posición de los objetos virtuales con la rotación
# y desplazamiento que indica la matriz de cámara (pose) estimada.
# La proyección final la hace el motor gráfico.
# (En los ejemplos anteriores la hacíamos nosotros con htrans(M, obj) )
# Las funciones mkTexture y mkLine permiten generar cómodamente objetos virtuales.
# Como ejemplo ponemos cuatro paredes sobre los lados de un cuadrado unidad, centrado
# en el sistema de referencia que nos da el marcador. Ponemos también un "suelo" de color liso.
# Descomentando las líneas comentadas se muestran también el perfil del marcador y un cubo.
# para cambiar si queremos el alto y ancho de las paredes (deformando)
S = scale((1, 1, 1)) # (lo dejamos igual)
# Si hay objetos transparentes primero definimos los sólidos
# (aún así, si hay varios transparentes la escena puede quedar mal,
# deben renderizarse de atrás hacia delante)
objects = [
mkLine(w, cube / 2 + (1.25, 0.25, 0), color=(128, 255, 255, 1), width=2)
# , mkLine(w, marker + (0,0,0.05), color=(255,255,0,1), width=2)
# , mkTexture(w, np.zeros((500,500,3),np.uint8) + np.array([64,92,64]).astype(np.uint8), desp((0,0,.3)) )
,
mkTexture(w, bricks,
rx(90) @ S),
mkTexture(w, bricks,
rz(90) @ rx(90) @ S),
mkTexture(w, bricks,
def autoscale(cont):
(x1, y1), (x2, y2) = cont.min(0), cont.max(0)
s = max(x2 - x1, y2 - y1)
c = vec(x1 + x2, y1 + y2) / 2
h = np.dot(scale(1 / vec(s, s)), desp(-c))
return htrans(h, cont)
def whitener(cont):
(c, (s1, s2, a)) = mymoments(cont)
return np.dot(np.dot(rot3(a), scale(1 / vec(s1, s2))),
np.dot(rot3(-a), desp(-c)))