Actualización de la práctica Marker Based Visual Loc en Unibotics.
Este ejercicio lo he resuelto utilizando operaciones matriciales.
1. Preparación del entorno
Primero he cargado todas las librerías necesarias como pyapriltags y definido los parámetros de la cámara (matriz intrínseca y coeficientes de distorsión). Siguiendo la recomendacion.
Un dato importante a considerar en este punto es la posición de la cámara en el sistema de coordenadas del robot.
- 0.069 m en el eje X (hacia adelante).
- -0.047 m en el eje Y (hacia la derecha).
- 0.107 m en el eje Z (hacia arriba).
- rpy=(0, 0, -np.pi/2): Esto describe la rotación de la cámara respecto al robot. Los ángulos son (roll = 0, pitch = 0, yaw = -np.pi/2) en radianes. Esta es la rotación clave. Significa que la cámara está girada 90 grados hacia la derecha sobre el eje Z del robot.
En resumen, es la pieza de información vital que le dice al robot cómo su cámara está montada y orientada, permitiendo que las detecciones visuales sean traducidas a su propia posición en el espacio.
2. El Bucle Principal:
Captura de Datos: En cada ciclo, el robot toma una imagen de su cámara y obtiene su posición actual.
Detección de Marcadores (AprilTags): La imagen capturada es analizada para encontrar los marcadores. Si se detecta un marcador, el código identifica sus esquinas en la imagen.
Estimación de Pose por Visión: Si se encuentra un marcador, el robot ejecuta una serie de pasos para calcular su pose exacta en el mundo.
- solvePnP: Esto calcula la pose del marcador respecto a la cámara.
- Validación: El código verifica la precisión de la pose calculada a través de un error de reproyección. Si este error es bajo, la pose es fiable.
- Transformación de Coordenadas: A través de una cadena de multiplicaciones de matrices (
T_world2apriltag @ RT_tag_camara @ T_camera2robot), la pose del marcador vista por la cámara se traduce a una pose global del robot en el mapa del mundo. - Almacenamiento de la Corrección: La pose visual calculada y la lectura del odómetro en ese mismo momento se guardan. Esto será fundamental para el siguiente paso.
Odometría (Cuando no hay marcador visible)
- Cálculo del Movimiento Incremental: En lugar de fiarse de la posición absoluta del odómetro, el código calcula el cambio de movimiento que ha tenido el robot desde la última vez que vio un marcador.
- Actualización de la Pose: Este movimiento incremental se aplica a la última pose visual validada que se guardó. Así, el robot propaga su última posición “correcta” usando los datos del odómetro, mitigando la acumulación de errores.
- Inicio con Odometría: Si el robot nunca ha visto un marcador, no tiene una pose visual de referencia. En ese caso, la estimación de la pose se basa únicamente en el odómetro hasta que se encuentre el primer marcador.
Este código permite que un robot navegue de manera fiable. Asegurando que la estimación de la pose sea siempre lo más cercana a la realidad.
