ROBOPONG – Nuevo sistema de coordenadas

Tal como hemos explicado en una entrada anterior, necesitamos cambiar el sistema de ejes coordenados para referirlo todo a la mesa de ping pong, y para ello necesitamos los vectores del nuevo sistema de coordenadas. Tras pensarlo durante varios días pensamos que hacer un procedimiento automático para detectar los vectores y ajustarlo todo solo nos parece muy complicado y no merece la pena. Así que se nos ocurre que si fabricamos una especie de patrón de vectores y que manualmente seleccionemos esos vectores en las cámaras sería lo más fácil. Al fin y al cabo esta operación sólo hay que hacerla una vez al principio de instalarlo todo.

Para esta operación hacemos un programa que permita seleccionar los cuatro puntos de los vectores (el origen y los extremos de X, Y, Z) de forma manual en las dos cámaras. El programa permite seleccionar esos cuatro puntos en cada cámara y después calcula la matriz de transformación y la guarda en un fichero, para usarla luego en el juego.

El código del programa anterior es:

########################################################################
# Proyecto ROBOPONG
# Configuración de ejes coordenados en la mesa
# Usa los parámetros intrínsecos de las cámaras calculados previamente
# Muestra vídeos de las dos cámaras y una cruz que se mueve con las teclas
# Muestra las coordenadas del pixel y el valor del color
#
# 28/2/2023
########################################################################


from tkinter import *                   # Carga la librería tkinter
from PIL import Image                   # Carga el módulo imagen de la librería PIL
from PIL import ImageTk                 # Carga ImageTK de la librería PIL
import cv2
import numpy as np
import colorsys
import yaml
import os

# Variables globales
frame=[]
px=50
py=50
anchocam=0
altocam=0
puntoo=[[0,0],[0,0]]
puntovx=[[0,0],[0,0]]
puntovy=[[0,0],[0,0]]
puntovz=[[0,0],[0,0]]
Nimagen=0
fich_calib="calibra.yaml"
calibration_settings = {}


##########################################################################
# Carga parámetros de configuración desde el fichero yaml
def parse_calibration_settings_file(filename):
    
    global calibration_settings

    if not os.path.exists(filename):
        print('No existe el fichero:', filename)
        quit()
    
    print('Usando para calibración: ', filename)

    with open(filename) as f:
        calibration_settings = yaml.safe_load(f)

    # Comprueba si el fichero de configuración tiene datos correctos
    if 'camera0' not in calibration_settings.keys():
        print('El fichero de configuración YAML no tiene información correcta')
        quit()


###########################################################################
# Carga los parámetros intrínsecos de una cámara desde un fichero
def load_camera_intrinsics(camera_name):

    # Si no existe la carpeta es un error
    if not os.path.exists('camera_parameters'):
        print("No existe la carpeta de parámetros")
        exit()

    # Construye nombe de fichero y comprueba que existe
    filename = os.path.join('camera_parameters', camera_name + '_intrinsics.dat')
    if not os.path.isfile(filename):
        print("No existe el fichero de parámetros")
        exit()

    with open(filename) as fich:
        lines = [line.rstrip() for line in fich]

    camera_matrix=[[0 for x in range(3)] for y in range(3)]
    for ii in (1,2,3):
        line=lines[ii].split()
        for jj in range(3):
            camera_matrix[ii-1][jj]=float(line[jj])

    distortion_coefs=[[0 for x in range(5)]]
    line=lines[5].split()
    for jj in range(5):
        distortion_coefs[0][jj]=float(line[jj])
    return np.array(camera_matrix), np.array(distortion_coefs)


###########################################################################
# Carga los parámetros geométricos de una cámara desde un fichero
def load_camera_extrinsics(camera_name):

    # Si no existe la carpeta es un error
    if not os.path.exists('camera_parameters'):
        print("No existe la carpeta de parámetros")
        exit()

    # Construye nombe de fichero y comprueba que existe
    filename = os.path.join('camera_parameters', camera_name + '_rot_trans.dat')
    if not os.path.isfile(filename):
        print("No existe el fichero de parámetros")
        exit()

    with open(filename) as fich:
        lines = [line.rstrip() for line in fich]

    camera_R=[[0 for x in range(3)] for y in range(3)]
    for ii in (1,2,3):
        line=lines[ii].split()
        for jj in range(3):
            camera_R[ii-1][jj]=float(line[jj])

    camera_T=[[0] for x in range(3)]
    for ii in (5,6,7):
        camera_T[ii-5][0]=float(lines[ii])
    return np.array(camera_R), np.array(camera_T)


##############################################################################
# Dadas las matrices de proyección P1 y P2, y las coordenadas de los pixel 
# Entrada: P1, P2 matrices de proyección
#          point1, point2 = coordenadas del pixel en ambas cámaras
# Salida: Coordenadas 3D del punto respecto a la cámara 0
def DLT(P1, P2, point1, point2):

    A = [point1[1]*P1[2,:] - P1[1,:],
         P1[0,:] - point1[0]*P1[2,:],
         point2[1]*P2[2,:] - P2[1,:],
         P2[0,:] - point2[0]*P2[2,:]
        ]
    A = np.array(A).reshape((4,4))

    B = A.transpose() @ A
    U, s, Vh = np.linalg.svd(B, full_matrices = False)

    #print('Triangulated point: ')
    #print(Vh[3,0:3]/Vh[3,3])
    return Vh[3,0:3]/Vh[3,3]


############################################################################
# Convierte la matriz de rotación y vector de traslación en una matriz 
# de transformación homogénea
# Entrada: R = matriz de rotación (3x3)
#          t = vector de traslación (3x1)
# Salida: matriz de transformación homogénea (4x4)
def _make_homogeneous_rep_matrix(R, t):
    P = np.zeros((4,4))
    P[:3,:3] = R
    P[:3, 3] = t.reshape(3)
    P[3,3] = 1
    return P


############################################################################
# Obtiene la matriz de proyección de los datos de calibración de la cámara
def get_projection_matrix(cmtx, R, T):
    P = cmtx @ _make_homogeneous_rep_matrix(R, T)[:3,:]
    return P


#####################################################################
# Dibuja la ventana con todo su contenido
def redrawAll(canvas):
    global frame
    global gif1,gif2,root,Nimagen
    global cap0,cap1
    global anchocam,altocam
    
    canvas.delete(ALL)

    # Imagen de la cámara activa
    if Nimagen==0:
        ret, frame = cap0.read()
    else:
        ret, frame = cap1.read()
    frame= cv2.cvtColor(frame,cv2.COLOR_BGR2RGB)
    frame=Image.fromarray(frame)
    frame = ImageTk.PhotoImage(image=frame)
    canvas.create_image((0,0), image=frame, anchor='nw')

    # Cursor del pixel    
    canvas.create_line(px-5, py, px+5, py, fill="red")
    canvas.create_line(px, py-5, px, py+5, fill="red")

    # Nombre de cámara
    Cad="Cámara "+str(Nimagen)
    canvas.create_text(anchocam+30,10,fill="yellow",font="Arial 10",text=Cad)
    
    # Texto del pixel
    canvas.create_text(anchocam+20,25,fill="white",font="Times 10",text="Pixel")
    Cad="("+str(px)+","+str(py)+")"
    canvas.create_text(anchocam+65,25,fill="white",font="Times 10",text=Cad)

    # Punto O
    canvas.create_text(anchocam+30,65,fill="yellow",font="Times 10",text="Punto O")
    Cad=str(puntoo[Nimagen])
    canvas.create_text(anchocam+35,80,fill="white",font="Times 10",text=Cad)
    button = Button(root, text="Guardar pixel", command=ponpuntoo)
    canvas_widget = canvas.create_window(anchocam+50, 110, window=button)
    
    # Punto Vx
    canvas.create_text(anchocam+30,150,fill="yellow",font="Times 10",text="Punto Vx")
    Cad=str(puntovx[Nimagen])
    canvas.create_text(anchocam+35,165,fill="white",font="Times 10",text=Cad)
    button = Button(root, text="Guardar pixel", command=ponpuntovx)
    canvas_widget = canvas.create_window(anchocam+50, 195, window=button)
    
    # Punto Vy
    canvas.create_text(anchocam+30,235,fill="yellow",font="Times 10",text="Punto Vy")
    Cad=str(puntovy[Nimagen])
    canvas.create_text(anchocam+35,250,fill="white",font="Times 10",text=Cad)
    button = Button(root, text="Guardar pixel", command=ponpuntovy)
    canvas_widget = canvas.create_window(anchocam+50, 280, window=button)

    # Punto Vz
    canvas.create_text(anchocam+30,320,fill="yellow",font="Times 10",text="Punto Vz")
    Cad=str(puntovz[Nimagen])
    canvas.create_text(anchocam+35,335,fill="white",font="Times 10",text=Cad)
    button = Button(root, text="Guardar pixel", command=ponpuntovz)
    canvas_widget = canvas.create_window(anchocam+50, 365, window=button)

    # Cambiar imagen
    button = Button(root, text="Cambia cámara", command=cambiaimagen)
    canvas_widget = canvas.create_window(anchocam+50, 415, window=button)

    # Calcular puntos 3D
    button = Button(root, text="Calcula 3D", command=calcula3D)
    canvas_widget = canvas.create_window(anchocam+50, 460, window=button)


######################################################
# Graba posición de punto O
def ponpuntoo():
    puntoo[Nimagen]=[px,py]
    redrawAll(canvas) 

    
######################################################
# Graba posición de punto X
def ponpuntovx():
    puntovx[Nimagen]=[px,py]
    redrawAll(canvas) 

    
######################################################
# Graba posición de punto Y
def ponpuntovy():
    puntovy[Nimagen]=[px,py]
    redrawAll(canvas) 


######################################################
# Graba posición de punto Z
def ponpuntovz():
    puntovz[Nimagen]=[px,py]
    redrawAll(canvas)   


######################################################
# Cambia fuente de imagen
def cambiaimagen():
    global Nimagen
    Nimagen=1-Nimagen
    redrawAll(canvas)   


############################################################################
# Calcula la matriz de transformación entre ambos sistemas de coordenadas
# entre el de la cámara 0 y el de los puntos señalados
def calcula3D():
    global P0,P1

    vectoro=np.transpose(DLT(P0,P1,puntoo[0],puntoo[1]))
    vectorx=np.transpose(DLT(P0,P1,puntovx[0],puntovx[1])-DLT(P0,P1,puntoo[0],puntoo[1]))
    vectory=np.transpose(DLT(P0,P1,puntovy[0],puntovy[1])-DLT(P0,P1,puntoo[0],puntoo[1]))
    vectorz=np.transpose(DLT(P0,P1,puntovz[0],puntovz[1])-DLT(P0,P1,puntoo[0],puntoo[1]))
    matrizM=np.zeros([4,4],dtype=float)
    matrizM[:3,0]=vectorx
    matrizM[:3,1]=vectory
    matrizM[:3,2]=vectorz
    matrizM[:3,3]=vectoro
    matrizM[3,3]=1
    matrizM_1=np.linalg.inv(matrizM)
    print(matrizM_1)

    # Guarda la matriz de transformación en un fichero
    matrizm_filename = os.path.join('camera_parameters', 'matriz_m.dat')
    outf = open(matrizm_filename, 'w')
    outf.write('M:\n')
    for linea in matrizM_1:
        for elem in linea:
            outf.write(str(elem) + ' ')
        outf.write('\n')
    outf.close()

    
#####################################################################
# Evento que gestiona pulsaciones de teclas para mover el cursor
def keyPressed(event):
    global px,py
    global canvas
    global root
    
    if event.keysym=='Up':
      py-=1
    if (event.keysym=='Down'):
      py+=1
    if (event.keysym=='Left'):
      px-=1
    if (event.keysym=='Right'):
      px+=1
    redrawAll(canvas)      


#######################################################################
# Inicializa la ventana dibujando todo su contenido
def init(canvas):
    redrawAll(canvas)


#######################################################################
# Programa principal
def run():
    global gif1,gif2
    global canvas
    global root
    global cap0,cap1
    global P0,P1
    global anchocam,altocam

    # Abre configuración y carga los parámetros de las cámaras    
    parse_calibration_settings_file(fich_calib)
    anchocam= calibration_settings['frame_width']
    altocam= calibration_settings['frame_height']
    cmtx0,dist0=load_camera_intrinsics('camera0')
    cmtx1,dist1=load_camera_intrinsics('camera1')

    # Calcula las matrices de proyección de las cámaras
    R0,T0=load_camera_extrinsics('camera0')
    R1,T1=load_camera_extrinsics('camera1')
    P0 = get_projection_matrix(cmtx0, R0, T0)
    P1 = get_projection_matrix(cmtx1, R1, T1)

    # Abre las dos cámaras y configura su resolución
    cap0 = cv2.VideoCapture(calibration_settings['camera0'])
    cap1 = cv2.VideoCapture(calibration_settings['camera1'])
    cap0.set(3, anchocam)
    cap0.set(4, altocam)
    cap1.set(3, anchocam)
    cap1.set(4, altocam)

    # create the root and the canvas
    root = Tk()
    canvas = Canvas(root, width=anchocam+100, height=altocam, bg="black")
    canvas.pack()
    
    # Store canvas in root and in canvas itself for callbacks
    root.canvas = canvas.canvas = canvas

    # Set up canvas data and call init
    canvas.data = { }
    init(canvas)

    # set up events
    # root.bind("<Button-1>", mousePressed)
    root.bind("<Key>", keyPressed)
    # timerFired(canvas)

    # Bucle principal
    root.mainloop()  # Así espera el programa a cerrar la ventana
    cap0.release()
    cap1.release()

run()