Práctica P5: Localización con balizas visuales - ori1710/Robotica_servicio GitHub Wiki
Objetivo
El objetivo de este ejercicio es estimar la posición y la orientación (pose) de un robot en un espacio 2D mediante la detección y el análisis de marcadores visuales, específicamente AprilTags. Este proceso implica el uso de visión artificial para identificar las etiquetas en el entorno del robot y métodos matemáticos para derivar su pose relativa a las etiquetas detectadas.
El robot verde representa la posición real. El robot azul representa la posición obtenida mediante la odometría (con ruido). El robot rojo representa la posición estimada por el usuario.
Escenario
Para esta práctica, el escenario es una casa donde se disponen 8 AprilTags repartidos por la planta baja de esta. El robot usado es uno pequeño que dispone de una cámara con las siguientes características dadas:
size = image.shape
focal_length = size[1]
center = (size[1] / 2, size[0] / 2)
matrix_camera = np.array(
[focal_length, 0, center[0](/ori1710/Robotica_servicio/wiki/focal_length,-0,-center[0), [0, focal_length, center[1]], [0, 0, 1]],
dtype="double",
)
dist_coeffs = np.zeros((4, 1))
Los AprilTags están situadas a 0,8 metros de altura y tienen un tamaño de 0,3 x 0,3 metros (la parte negra mide 0,24 x 0,24 metros).
Movimiento del robot
Como en esta práctica lo importante es ubicarnos viendo los AprilTags, el movimiento del robot es simple: solo hacemos que avance y gire por un tiempo, formando un círculo. Si tuviéramos algún otro sensor, podríamos hacer un choca-gira para que el movimiento sea más aleatorio.
Obtención de la posición
Para obtener la posición se tienen que seguir los siguientes pasos:
- Detectar e identificar las balizas.
- Usar PnP (Perspective-n-Point) de OpenCV para estimar la posición relativa 3D entre la cámara y la baliza observada.
- Construir una matriz de transformación homogénea que representa la pose (posición y orientación) del tag en el mundo (mapa).
- Conseguimos la matriz de transformación del mapa a la cámara obteniendo posición y orientación del robot.
1. Detectar e identificar las balizas.
Este es el paso más sencillo donde solo tenemos que obtener la imagen, transformarla a escala de grises y usar detector.detect(gray), que, en caso de ver alguna baliza, nos devolverá los datos del AprilTag.
2. Usar PnP (Perspective-n-Point) de OpenCV para estimar la posición relativa 3D entre la cámara y la baliza observada.
Una vez detectado un tag, usamos solvePnP de OpenCV para estimar la pose de la cámara con respecto al tag:
success, rvec, tvec = cv2.solvePnP(object_points, image_points, matrix_camera, dist_coeffs, flags=cv2.SOLVEPNP_IPPE_SQUARE)
Donde object_points son las coordenadas 3D de las esquinas del tag (en el sistema local del tag):
# Parámetros del tag
tag_size = 0.24 # metros
half_size = tag_size / 2
object_points = np.array([
[-half_size, half_size, 0],
[ half_size, half_size, 0],
[ half_size, -half_size, 0],
[-half_size, -half_size, 0],
], dtype="double")
La variable image_points contiene las coordenadas 2D donde esas esquinas se ven en la imagen de la cámara:
ptA, ptB, ptC, ptD = r.corners
image_points = np.array([ptA, ptB, ptC, ptD], dtype="double")
Además, usamos cv2.SOLVEPNP_IPPE_SQUARE que es un algoritmo especializado para objetos cuadrados planos.
Con cv2.solvePnP obtenemos dos vectores: rvec y tvec, que representan rotación y traslación, respectivamente. Para construir la matriz RT de la cámara al tag, primero convertimos el vector de rotación a una matriz de 3x3 con cv2.Rodrigues(rvec) y nos aseguramos de que tvec tenga forma (3, 1). Con eso, construimos la matriz T_cam_tag.
3. Construir una matriz de transformación homogénea que representa la pose (posición y orientación) del tag en el mundo (mapa).
Esta matriz se construye con los datos del tag detectado:
tag_id = r.tag_id
tag_key = f"tag_{tag_id}"
if tag_key not in tags:
continue
x_tag, y_tag, z_tag, yaw_tag = tags[tag_key]["position"]
Ya con esto simplemente hacemos la matriz T_map_tag pero dejamos fija siempre la altura en "z" de 0.8, que es el dato que se nos da de los AprilTag:
T_map_tag = np.array([
[np.cos(yaw_tag), -np.sin(yaw_tag), 0, x_tag],
[np.sin(yaw_tag), np.cos(yaw_tag), 0, y_tag],
[0, 0, 1, 0.8],
[0, 0, 0, 1]
])
4. Conseguimos la matriz de transformación del mapa a la cámara obteniendo posición y orientación del robot.
La matriz que necesitamos es en base a lo siguiente:
T_map_cam = T_map_tag @ T_tag_cam
La matriz T_map_tag la conseguimos en el anterior punto pero la matriz de T_tag_cam tenemos que usar la primera matriz que conseguimos en el punto 2 pero la inversa, además hay que tener en cuenta que esta matriz debe multiplicarse por una matriz que llamamos "rotation_matrix" antes de aplicarla en la ecuación final. Esto se debe a que OpenCV y Gazebo utilizan sistemas de coordenadas distintos. En OpenCV, el eje X apunta hacia la derecha, el eje Y hacia abajo y el eje Z hacia adelante, todos relativos a la cámara. En cambio, en Gazebo, el eje X apunta al este, el Y al norte y el Z hacia arriba. Para poder convertir las coordenadas de OpenCV al sistema que usa Gazebo, es necesario aplicar dos rotaciones: primero una rotación de -90° alrededor del eje X, y luego otra de -90° alrededor del eje Z. Entonces tenemos lo siguiente:
# Calcular inversa T_tag_cam
T_tag_cam = np.linalg.inv(T_cam_tag)
x_axis_rotation_matrix = np.array([1, 0, 0, 0], [0, 0, 1, 0], [0, -1, 0, 0], [0, 0, 0, 1](/ori1710/Robotica_servicio/wiki/1,-0,-0,-0],-[0,-0,-1,-0],-[0,--1,-0,-0],-[0,-0,-0,-1))
z_axis_rotation_matrix = np.array([0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1](/ori1710/Robotica_servicio/wiki/0,-1,-0,-0],-[-1,-0,-0,-0],-[0,-0,-1,-0],-[0,-0,-0,-1))
rotation_matrix = z_axis_rotation_matrix @ x_axis_rotation_matrix
T_tag_cam = rotation_matrix @ T_tag_cam
T_map_cam = T_map_tag @ T_tag_cam
Finalmente, con T_map_cam ya podemos obtener la posición (x, y) del robot (o de la cámara, que varía muy poco) y su orientación, que para esta tenemos que aplicar una fórmula para sacarla desde la matriz de rotación, que esta puesto asi por si hubiera una inclinación inesperada (por ejemplo, si pasara por encima de algo), teniendo lo siguiente:
# Posición del robot en el mapa
camera_pos = T_map_cam[:3, 3]
# Orientación (yaw) del robot
R = T_map_cam[:3, :3] # world2cam
# Calcular theta
theta = np.arctan2(-R[2, 0], np.sqrt(R[0, 0]**2 + R[1, 0]**2))
# Calcular yaw con compensación
cos_theta = np.cos(theta)
yaw_est = np.arctan2(R[1, 0] / cos_theta, R[0, 0] / cos_theta) + np.pi / 2
Con esto ya podemos saber dónde se encuentra el robot y hacia dónde está mirando, siempre y cuando esté viendo un AprilTag. Si no, debemos confiar en la odometría del robot.
Conclusión
Con lo implementado no obtenemos una precisión del 100%, pero sí es bastante acertada, como se puede apreciar en los siguientes vídeos: