Cámara en coordenadas absolutas

Estos últimos días he dedicado tiempo a mejorar algunos aspectos del programa LiDAR Visualizer. He reestructurado el código para mejorar la escalabilidad de la aplicación y he añadido una nueva cámara para el modo de ejecución del sensor simulado en CARLA. Esta nueva cámara proporciona una vista en coordenadas absolutas que permite ver el entorno fijo mientras el vehiculo avanza tomando nuevas muestras.

Cámara en Coordenadas Abdolutas

Operaciones

Para concretar este modo de visualización, es necesario ubicar los puntos relativos captados por el sensor a la posición absoluta en el entorno de simulación. Para ello se precisa de la ubicación absoluta del vehículo y de sus ángulos de rotación. El simulador CARLA ofrece métodos para extraer la posición y angulos de rotación actuales de cualquier actor presente en el entorno

Teniendo en cuenta que se quiere obtener un entorno fijo, se relizará la rotación que tiene el vehiculo en cada instante a cada punto relativo a este y posteriormente se desplazará sumando la distancia que hay desde el vehículo al origen del entorno de simulación

Código

He creado una función para extraer la matriz de rotación del vehículo ya que CARLA solo ofrece un método para extraer los angulos respecto de los planos YZ, XZ, XY (pitch, yaw, roll). La matriz de rotación del vehículo será el producto de las matrices de rotación Rz, Ry, Rx

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)

    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

Se realizan las sigientes operaciones matriciales sobre cada punto

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)