Construcción del cerebro

Construcción del Brain

Esta semana, el objetivo era construir un cerebro, brain(), que sea capaz de controlar la conducción de un vehículo, de visualizar mediante un sensor LiDAR establecido en el vehículo el entorno mediante nubes de puntos, y una cámara en que se visualice el automóvil, la visión que tendría su conductor real.

La anterior semana, conseguí crear un programa que pudiera teleoperar el coche mediante teclado y, además de visualizar el vehículo, poder generar datos LiDAR para visualizarlos en Lidar-Visualizer. Esta semana no he podido avanzar mucho en el desarrollo del brain() en cuanto a la visualización de datos del LiDAR, por errores en el código que no he logrado comprender o bien porque el simulador no soporta la ejecución de una ventana con la cámara del vehículo junto a la que muestra la nube de puntos. Esta próxima semana me dedicaré a solucionar los problemas de visualización del LiDAR y a acabar brain().

Visualización de nube de puntos LiDAR

Los avances de esta semana han sido, en cuanto a investigación para el desarrollo del brain, estudiar las funcionalidades del programa Lidar Visualizer que procesan los datos del sensor y las representa.

Para ello he aplicado la función lidar_callback, distinta a la que implementé la semana anterior, que además de recibe como parámetros de entrada los datos que genera el sensor y la nube de puntos, usa también la posición y rotación del sensor (vehicle_transform). Esta función convierte los datos “crudos” del LiDAR a arrays de puntos, ajusta el sistema de coordenadas del sensor al sistema global y mapea la intensidad a colores. Finalmente actualiza la nube de puntos para poder visualizarla usando la librería Open3D:

def lidar_callback(lidar_data, point_cloud, vehicle_transform):
    global initial_vehicle_location

    # Leer los datos del LiDAR y reorganizarlos en forma de puntos e intensidad
    data = np.copy(np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4')))
    data = np.reshape(data, (int(data.shape[0] / 4), 4))

    # Reflejar los datos en el eje X para alinearlos con el sistema de CARLA
    data[:, 0] = -data[:, 0]

    if absolute_view:
        # Obtener ubicación y rotación del vehículo
        vehicle_location = np.array([-vehicle_transform.location.x,
                                     vehicle_transform.location.y,
                                     vehicle_transform.location.z])
        
        pitch = vehicle_transform.rotation.pitch
        yaw = vehicle_transform.rotation.yaw
        roll = vehicle_transform.rotation.roll

        # Crear la matriz de rotación inversa usando la orientación del vehículo
        rotation_matrix = euler_to_rotation_matrix(pitch, yaw, roll).T  # Usamos la transpuesta como la inversa

        # Aplicar la rotación inversa y luego posicionar en el sistema global
        points_in_world = []
        for point in data[:, :3]:  # 'data' contiene los puntos LiDAR relativos
            # Aplicar la rotación inversa al punto
            rotated_point = rotation_matrix @ point
            # Posicionar el punto en el sistema global usando la posición inicial del vehículo
            global_point = rotated_point + vehicle_location
            points_in_world.append(global_point)

        # Convertir a formato numpy para visualización en Open3D
        points_to_display = np.array(points_in_world)

    else:
        # Usar coordenadas relativas sin transformación
        points_to_display = data[:, :3]

    # Mapear la intensidad de cada punto a un color para visualización
    intensity = data[:, -1]
    int_color = np.c_[
        np.interp(intensity, VID_RANGE, VIRIDIS[:, 0]),
        np.interp(intensity, VID_RANGE, VIRIDIS[:, 1]),
        np.interp(intensity, VID_RANGE, VIRIDIS[:, 2])
    ]

    # Actualizar el point cloud en Open3D
    point_cloud.points = o3d.utility.Vector3dVector(points_to_display)
    point_cloud.colors = o3d.utility.Vector3dVector(int_color)

Añadir también, que para que funcione nuestra visualización de la nube de puntos y en relación con la posición y rotación del vehículo, hay que aplicar la matriz de rotación, que convertirá los ángulos de rotación del vehículo en una matriz de rotación 3x3 para poder rotar los puntos en el espacio del sistema global del simulador:

def euler_to_rotation_matrix(pitch, yaw, roll):
    # Convertir los ángulos de grados a radianes
    pitch = math.radians(pitch)
    yaw = math.radians(yaw)
    roll = math.radians(roll)

    # Crear la matriz de rotación para cada eje
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])

    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])

    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])

    # Multiplicar en el orden Yaw -> Pitch -> Roll
    rotation_matrix = R_z @ R_y @ R_x
    return rotation_matrix

Con la implementación de estas funciones, he podido teleoperar un vehículo mientras se generaba una nube de puntos. No obstante, la visualización no ha sido exitosa y presenta errores a solucionar esta próxima semana. A continuación, una imagen de la vizualización conseguida:

Resumen

Esta semana se ha avanzado con la visualización de los datos del sensor LiDAR mientras se teleopera un vehículo en el simulador Carla. Para la construcción de brain(), junto a lo que se había avanzado la semana anterior, se han implementado las funciones anteriores para la parte del sensor LiDAR.

Para la próxima semana hay que acabar de construir el “cerebro” y seguir investigando para las próximas semanas cómo vamos a tratar los datos del LiDAR, con el fín de entrenar un algoritmo de Deep Learning.