Objetivo

El objetivo de este ejercicio es implementar la lógica que permite a un dron reconocer los rostros de personas perdidas y guardar su ubicación para realizar una posterior maniobra de rescate.

Obtención de las coordenadas

Las coordenadas que se nos dan son del bote donde se encuentra el dron y las coordenadas de dónde se perdió el contacto con el avión por última vez. Estas coordenadas están dadas en el sistema de GPS Degrees, Minutes, Seconds y nosotros lo necesitamos en UTM, asi que con esta página Convert Geographic Units la convertimos y obtenemos lo siguiente:

Una vez obtenemos las coordenadas en UTM lo que hacemos es restar las coordenadas donde estarían las personas con las coordenadas del bote donde se encuentra el dron, obteniendo las coordenadas en gazebo de la zona de búsqueda:

Máquina de estados

Para encontrar las posibles víctima he implementado una máquina de estados sencilla, donde se hace lo siguiente:

1. Despegar el dron con:

HAL.takeoff(altitude)

despegue.webm

2. Ir donde se encuentra la zona del accidente con:

HAL.set_cmd_pos(target_pos[0], target_pos[1], altitude, 0.0)

3. Hacer una espiral en búsqueda de personas
4. Regresar al bote y aterrizar con:

HAL.set_cmd_pos(boat_pos[0], boat_pos[1], 2.0, 0.0)
# Compruebo que llego al bote
HAL.land()

aterrizaje.webm

Ahora, solo queda explicar el estado 3, que es buscar a las personas haciendo una espiral y usando un detector de caras.

Realización de la espiral

Para buscar a las personas, primero había pensado en usar una espiral mas cuadrada, es decir, que vaya por coordendas, pero no entendia por que si la siguiente coordenada era sumar '1' y '2' a 'x' e 'y' en realidad se sumaba '1' y '1', asi que decidí dejarlo y optar por una espiral simple que vaya aumentando la velocidad y que el ángulo de giro de mantenga constante.

Esto lo logré usando lo siguiente:

i = 0.1 # Velocidad inicial
...
HAL.set_cmd_mix(i, 0.0, altitude, 0.4)  # Movimiento en espiral
i += 0.001  # Incrementa la velocidad gradualmente

espiral.webm

Para saber que tengo que terminar de buscar a personas tenía dos opciones, usar el tiempo para simular que se acababa la batería o decir que ya he recorrido cierto radio. Yo he decidio usar el radio, asi que sabiendo donde empiezo (que son las coordenadas que se nos dan), voy viendo en cada iteración a que distancia estoy. Cuando llego a 18 metros de distancia, decido volver al bote. En cada iteracción también tengo que revisar si hay personas y transmitir su ubicación.

Detección de las personas con Haar Cascades de Opencv

Esta parte fue la más complicada, ya que no solo basta con aplicar el detector de caras a la imagen. El problema quee tiene este detector es que sólo te detecta la cara si esta se encuentra orientada de forma normal. Para solucionar esto, en cada iteracción roto la imagen por el centro en 45, 90, 135, 180, 225, 270, 315 y 360 grados, comprobando en cada giro si encuentro alguna persona. En el caso de encontrar a alguien con la imagen girada, por ejemplo, a 225 grados, no puedo dibujar el cuadrado donde encontré la persona, ya que la posición de la imagen rotada con la original no concuerda, por ello tengo que rotar el punto donde encontré a la persona el ángulo que giré la imagen con la siguiente función:

def rotate(center, point, angle):
    angle_rad = math.radians(angle)
    temp_point = [point[0] - center[0], point[1] - center[1]] 
    
    temp_x = temp_point[0] * math.cos(angle_rad) - temp_point[1] * math.sin(angle_rad)
    temp_y = temp_point[0] * math.sin(angle_rad) + temp_point[1] * math.cos(angle_rad)
    
    new_point = [temp_x + center[0], temp_y + center[1]]
    return new_point[0], new_point[1]

Con el punto rotado, ya puedo dibujar el cuadrado en la imagen original:

Solo queda guardar la ubicación de las personas que voy encontrando y enviarlas al equipo de rescate, pero sin repetir a la persona, es decir, una vez encontrada, sólo informo una vez aunque me la vuelva a encontrar. Para esto lo que hago es crear una lista donde voy guardando las coordenadas de donde se encuentra el dron al momento de encontrar una persona. La siguiente persona que encuentro, reviso sus coordenadas y veo si ya no la había encontrado antes, si no esta guardado, mando su ubicación.

El dron, como se va moviendo, puedo llegar a ver a la misma persona pero desde ubicaciones diferentes. Para solucionar esto, he usado un rango de tolerancia en el que, si mi ubicación esta dentro de este rango pues la considero la misma persona. Este no es perfecto, ya que en algunas ocasiones puede llegar a identificar dos veces la misma persona porque se ve desde puntos muy alejados, pero prefiero que se repita la misma persona a que llegue a confundir una persona con otra.

Conclusión

En el mejor de los casos he logrado identificar a las 6 personas sólo una vez y en coordenadas muy cercanas a la realidad:

https://github.com/user-attachments/assets/8d5453ae-a924-4aa4-a1e3-ab22ab5ddfdd

En el segundo mejor ha identificado a 7 personas, ya que a una se le ve desde dos ubicaciones muy alejadas:

https://github.com/user-attachments/assets/815b658b-519a-436f-9b1a-afefed263d53

Y en el peor caso he llegado a identificar más de 10 personas, sin modificar nada del código ya que el mismo dron va acumulando errores de posición y de velocidad a medida que avanza, haciendo que se detecte a una misma persona varias veces desde posiciones contrarias.