def match(m, t, steps=1):
errs = t.z0s - m.z0
aes = np.sum(abs(errs), 0)
k = np.argmin(aes)
err = errs[:, k]
R = rot3(k * 2 * np.pi / angs)
aesn = aes[k] / la.norm(m.z0)
#if aesn > 0.4: return 1, ()
f0, j0, ij = m.gnw
dv = np.dot(ij, vecfeat(err))
dt = mktP(dv)
h = np.dot(la.inv(np.dot(dt, m.w)), np.dot(R, t.w))
err = aesn
f0, j0, ij = m.gno
for q in range(steps):
s = htrans(h, t.original)
err = afeat(s, wmax) - f0
dv = np.dot(ij, err)
dt = mktP(dv)
h = np.dot(la.inv(dt), h)
err = la.norm(err) / la.norm(f0)
return err, h
def __init__(self, contour):
self.original = contour
self.w = whitener(contour)
self.white = htrans(self.w, contour)
self.z0 = col(normalizeStart(spectralFeat(self.white, wmax)))
self.gno = GNModelP(contour, wmax)
self.gnw = GNModelP(self.white, wmax)
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:
p = poses[0]
T = cameraTransf(p)
cam.setData(pos=htrans(T, drawCam))
view.setData(data=img2tex(img))
m = T @ A
view.setTransform(QtGui.QMatrix4x4(*(m.flatten())))
axc.setTransform(QtGui.QMatrix4x4(*(T.flatten())))
#print(p)
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 showAxes(img,camera,scale=1):
p = camera
s = scale
augmented(img,p,ejeX*s, (0,0,255), 1)
augmented(img,p,ejeY*s, (0,210,0), 1)
augmented(img,p,ejeZ*s, (255,0,0), 1)
for axis,name,color in zip(htrans(p,np.eye(3)*s).astype(int), ['x','y','z'],[(0,0,255),(0,210,0),(255,0,0)]):
putText(img,name,axis,color,div=1)
def showCalib(K,image,dg=5,col=(128,128,128)):
HEIGHT, WIDTH = image.shape[:2]
cv.line(image,(0,HEIGHT//2),(WIDTH,HEIGHT//2),col,1,lineType)
cv.line(image,(WIDTH//2,0),(WIDTH//2,HEIGHT),col,1,lineType)
AW = WIDTH*1//100
for ang in range(-45,46,dg):
x,_ = htrans(K,row(np.sin(ang*np.pi/180),0))[0].astype(int)
x += HEIGHT//2 - WIDTH//2
cv.line(image,(WIDTH//2-AW,x),(WIDTH//2+AW,x),col,1,lineType)
x += -HEIGHT//2 + WIDTH//2
cv.line(image,(x,HEIGHT//2-AW),(x,HEIGHT//2+AW),col,1,lineType)
def applyImg(src, M, world, img, frame):
# calculamos dónde se proyectarán en la imagen esas esquinas
# usamos la matriz de cámara estimada
dst = htrans(M, world)
# calculamos la transformación
# igual que findHomography pero solo con 4 correspondencias
H = cv.getPerspectiveTransform(src.astype(np.float32),
dst.astype(np.float32))
# la aplicamos encima de la imagen de cámara
cv.warpPerspective(img, H, size, frame, 0, cv.BORDER_TRANSPARENT)
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 cameraOutline2(M, sc = 0.3):
K,R,C = sepcam(M)
# formamos una transformación 3D para mover la cámara en el origen a la posición de M
rt = jr(jc(R, -R @ col(C)),
row(0,0,0,1))
x = 1;
y = x;
z = 0.99;
ps =[x, 0, z,
(-x), 0, z,
0, 0, z,
0, 1.3*y,z,
0, (-y), z,
x, (-y), z,
x, y, z,
(-x), y, z,
(-x), (-y), z,
x, (-y), z,
x, y, z,
0, y, z,
0, 0, z,
0, 0, 0,
1, 1, z,
0, 0, 0,
(-1), 1, z,
0, 0, 0,
(-1), (-1), z,
0, 0, 0,
(1), (-1), z,
0, 0, 0,
0, 0, (2*x)]
ps = np.array(ps).reshape(-1,3)
return htrans(la.inv(rt), sc * ps)
def augmented(img,camera,thing,color,thick=1):
cv.drawContours(img, [htrans(camera,thing).astype(int)] , -1, color, thick, lineType)
for n, (key,frame) in enumerate(stream):
g = cv.cvtColor(frame,cv.COLOR_BGR2GRAY)
cs = extractContours(g, minarea=5, reduprec=2)
good = polygons(cs,6,3)
poses = []
for g in good:
p = bestPose(K,g,marker)
if p.rms < 2:
poses += [p.M]
for M in poses:
# capturamos el color de un punto cerca del marcador para borrarlo
# dibujando un cuadrado encima
x,y = htrans(M, (0.7,0.7,0) ).astype(int)
b,g,r = frame[y,x].astype(int)
cv.drawContours(frame,[htrans(M,square*1.1+(-0.05,-0.05,0)).astype(int)], -1, (int(b),int(g),int(r)) , -1, cv.LINE_AA)
# cv.drawContours(frame,[htrans(M,marker).astype(int)], -1, (0,0,0) , 3, cv.LINE_AA)
# Mostramos el sistema de referencia inducido por el marcador (es una utilidad de umucv)
showAxes(frame, M, scale=0.5)
# hacemos que se mueva el cubo
cosa = cube * (0.5,0.5,0.75 + 0.5*np.sin(n/100)) + (0.25,0.25,0)
cv.drawContours(frame, [ htrans(M, cosa).astype(int) ], -1, (0,128,0), 3, cv.LINE_AA)
cv.imshow('source',frame)
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:
# elegimos la homografía del primer marcador detectado
c, H = ok[0]
# la adaptamos igual que antes
T = desp([100, 100]) @ scale([100, 100]) @ H
# calculamos la transformación inversa
IH = np.linalg.inv(T)
# y nos llevamos las líneas del mundo real a la imagen
thoriz = htrans(IH, horiz.reshape(-1, 2)).reshape(-1, 2, 2)
tvert = htrans(IH, vert.reshape(-1, 2)).reshape(-1, 2, 2)
cv.polylines(frame, thoriz.astype(int), False, (0, 255, 0), 1,
cv.LINE_AA)
cv.polylines(frame, tvert.astype(int), False, (0, 255, 0), 1,
cv.LINE_AA)
cv.imshow('source', frame)
for key, frame in stream:
key = cv.waitKey(1) & 0xFF
if key == 27: break
g = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
cs = extractContours(g, minarea=5, reduprec=2)
good = polygons(cs, 6, 3)
poses = []
for g in good:
pos = bestPose(K, g, marker)
if pos.rms < 2:
poses += [pos.M]
cv.drawContours(frame, [htrans(M, marker).astype(int) for M in poses], -1,
(0, 255, 255), 1, lineType)
cv.drawContours(frame, [htrans(M, cube / 2).astype(int) for M in poses],
-1, (0, 255, 0), 1, lineType)
cv.drawContours(frame, [htrans(M, persona / 3).astype(int) for M in poses],
-1, (255, 0, 0), 3, lineType)
# Direccion del muñeco
if not reverse:
persona = persona + [0, incrementoMov, 0]
if persona[0][1] >= 1:
reverse = True
else:
persona = persona - [0, incrementoMov, 0]
if persona[0][1] <= 0:
def getCam(K, h, c, p3d):
view = htrans(h, c)
P = Pose(K, view, p3d)
return P.rms, P.M, P.R, P.C, P.view
def errorMarker(c):
H, _ = cv.findHomography(c, marker)
err = abs(htrans(H, c) - marker).sum()
return err, H
# si encontramos varios posibles marcadores nos quedamos solo con los
# que tengan un error de reproyección menor de 2 pixels
# Si aparece un polígono por casualidad con 6 lados, es muy difícil que
# sea consistente con una posible imagen del marcador.
# Este critero elimina casi todos los falsos positivos
poses = []
for g in good:
rms, M = bestPose(K,g,marker)
if rms < 2:
poses += [M]
# guardamos las matrices de cámara encontradas
# (tantas como marcadores se detecten, lo normal
# es que en la escena haya una o ninguna).
# es mejor usar el teclado cambiar la información que se muestra
if debug:
# dibujamos todos los contornos en morado
cv.drawContours(frame,[c.astype(int) for c in cs], -1, (255,255,0), 1, lineType)
# los polígonos de 6 lados (posibles marcadores) en rojo
cv.drawContours(frame,[c.astype(int) for c in good], -1, (0,0,255), 3, lineType)
# la reproyección del marcador con la cámaras estimada en amarillo
cv.drawContours(frame,[htrans(M,marker).astype(int) for M in poses], -1, (0,255,255), 1, lineType)
# mostramos un objeto 3D virtual en verde
cv.drawContours(frame,[ htrans(M,cube).astype(int) for M in poses ], -1, (0,128,0), 3, lineType)
cv.imshow('source',frame)
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 whiten(c):
return htrans(whitener(c), c)
# (esta versión devuelve un objeto, no una tupla)
def bestPose(K, view, model):
poses = [Pose(K, v.astype(float), model) for v in rots(view)]
return sorted(poses, key=lambda p: p.rms)[0]
for key, frame in stream:
g = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
cs = extractContours(g, minarea=5, reduprec=2)
good = polygons(cs, 6, 3)
poses = []
for g in good:
p = bestPose(K, g, marker)
if p.rms < 2:
poses += [p.M]
# rellenamos el marcador (podríamos intentar "borrarlo")
cv.drawContours(frame, [htrans(M, marker).astype(int) for M in poses], -1,
(0, 128, 255), -1, lineType)
# mostramos un objeto 3D virtual en verde
cv.drawContours(frame, [htrans(M, cube).astype(int) for M in poses], -1,
(0, 128, 0), 3, lineType)
cv.imshow('source', frame)
def __init__(self, contour):
self.original = contour
self.w = whitener(contour)
self.white = htrans(self.w, contour)
f0 = normalizeStart(spectralFeat(self.white, wmax))
self.z0s = col(f0) * rotator
def move(T, ref):
uc = np.zeros(ref.shape[:-1] + (1, ))
ref3D = np.append(ref, uc, axis=-1)
return htrans(T, ref3D)
def rmsreproj(view, model, transf):
err = view - htrans(transf,model)
return np.sqrt(np.mean(err.flatten()**2))
poses += [p]
#print(Me)
cosa = cube + 0
# todos los contornos
#cv.drawContours(frame,[c.astype(int) for c in cs], -1, (255,255,0), 1, lineType)
# posibles marcadores
#cv.drawContours(frame,[c.astype(int) for c in good], -1, (0,0,255), 3, lineType)
# reproyección del marcador con las cámaras estimadas
#cv.drawContours(frame,[htrans(p.M,marker).astype(int) for p in poses], -1, (0,255,255), 1, lineType)
# mostramos un objeto 3D virtual
cv.drawContours(frame,[htrans(p.M,cosa).astype(int) for p in poses], -1, (0,128,0), 3, lineType)
# proyectamos una "textura" (imagen) donde deseemos
for p in poses[:1]:
# los extremos de la "imagen virtual" que vamos a proyectar
h,w = imvirt.shape[:2]
src = np.array([[0,0],[0,h],[w,h],[w,0]]).astype(np.float32)
# dónde queremos ponerla en el sistema de referencia del marcador
world = np.array([[0.25,0.5,0],[.75,0.5,0],[.75,0.5,1],[0.25,0.5,1]])
# dónde se verá en la imagen de cámara
dst = htrans(p.M, world).astype(np.float32)
# calculamos la transformación
#H, _ = cv.findHomography(src,dst)
for M in poses:
# vamos a poner verticalmente la imagen imvirt cargada más arriba
# las coordenadas de sus 4 esquinas
# (se pueden sacar del bucle de captura)
h, w = imvirt.shape[:2]
src = np.array([[0, 0], [0, h], [w, h], [w, 0]])
# decidimos dónde queremos poner esas esquinas en el sistema de referencia del marcador
# (si no cambian se puede sacar del bucle de captura)
world = np.array([[0.25, 0.5, 0], [.75, 0.5, 0], [.75, 0.5, 1],
[0.25, 0.5, 1]])
# calculamos dónde se proyectarán en la imagen esas esquinas
# usamos la matriz de cámara estimada
dst = htrans(M, world)
# calculamos la transformación
#H, _ = cv.findHomography(src,dst)
# igual que findHomography pero solo con 4 correspondencias
H = cv.getPerspectiveTransform(src.astype(np.float32),
dst.astype(np.float32))
# la aplicamos encima de la imagen de cámara
cv.warpPerspective(imvirt, H, size, frame, 0, cv.BORDER_TRANSPARENT)
# tenemos también la distancia la marcador
# print(np.linalg.norm(p.C))
cv.imshow('source', frame)
def GNModelP(x, wmax):
f = lambda v: afeat(htrans(mktP(v), x), wmax)
return ICModel(f, 8)
