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)