Semana 18: Adición de ruido y pérdidas al LiDAR semántico

LiDAR semántico

Debido a que el LiDAR semántico no introduce ni ruido ni pérdidas, a diferencia del LiDAR estándar, no se consiguen datos del todo realistas. Para solucionar esto, se ha decidido añadir ruido y pérdidas manualmente en el código o3d-semanticLidarViz-manual-segmentation.py.

o3d-semanticLidarViz-manual-segmentation-noiseDrop.py

Para añadir ruido gaussiano y pérdidas, se han implementado dos funciones y se ha tenido que modificar la función lidar_callback:

Modificaciones lidar_callback

def lidar_callback(lidar_data, point_cloud, frame, 
                                noise_std=0.1, 
                                attenuation_coefficient=0.1, 
                                output_dir = 'dataset/lidar'):

    data = np.copy(np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4')))
    data = np.reshape(data, (int(data.shape[0] / 6), 6))  # Ahora cada fila tiene 6 valores

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

    # Extraer las coordenadas XYZ y los valores de intensidad
    points = data[:, :3]

    # Extraer etiquetas semánticas
    semantic_tags = data[:, 5].view(np.uint32)  # Ver datos como enteros
    
    # Calculamos la distancia de cada punto al sensor (suponiendo que el sensor está en el origen)
    distances = np.linalg.norm(points, axis=1)  # Distancia euclidiana

    # Calculamos la intensidad para cada punto utilizando la fórmula I = e^(-a * d)
    intensities = np.exp(-attenuation_coefficient * distances)

    # Aplicar ruido a los puntos
    points = add_noise_to_lidar(points, noise_std)

    # Aplicar pérdidas de puntos según las reglas del LiDAR
    points, semantic_tags = drop_points(points, semantic_tags, intensities)

    # Asignar colores según etiquetas
    colors = np.array([get_color_from_semantic(tag) for tag in semantic_tags]) / 255.0  # Normalizar a [0,1]

    # Asignar los datos modificados a la nube de puntos
    point_cloud.points = o3d.utility.Vector3dVector(points)  # Nube de puntos (Nx3).
    point_cloud.colors = o3d.utility.Vector3dVector(colors) # Colores RGB normalizados (Nx3)

    # Guardar las etiquetas semánticas en el campo "normals"
    point_cloud.normals = o3d.utility.Vector3dVector(np.c_[semantic_tags, semantic_tags, semantic_tags])

    # 📂 Guardar el point cloud cada 20 frames
    # Crear la carpeta de salida si no existe
    if not os.path.exists(output_dir):
        os.makedirs(output_dir)

    if frame % 20 == 0:
        filename = os.path.join(output_dir, f"lidar_points_{frame:04d}.ply")
        print(f"Guardando archivo {filename}...")
        o3d.io.write_point_cloud(filename, point_cloud)

Se añadieron dos parámetros de entrada:

• noise_std: float - Desviación estándar del ruido Gaussiano que se aplica a las coordenadas XYZ de cada punto de la nube de puntos. Este parámetro introduce variabilidad en las mediciones, simulando errores de medición típicos en sensores del mundo real.
• attenuation_coefficient: float - Coeficiente de atenuación utilizado para calcular la intensidad de cada punto en función de su distancia desde el sensor.

Cálculo de la Intensidad: Debido a que en el LiDAR semántico no calcula la intensidad de los puntos por defecto, se ha decidido calcularla manualmente utilizando la siguiente fórmula, que se usa para el LiDAR estándar: I = e^(-a * d), donde:

• I es la intensidad.
• a es el coeficiente de atenuación.
• d es la distancia desde el sensor al punto.

Primero, calculamos la distancia de cada punto desde el sensor usando la norma euclidiana de sus coordenadas con: np.linalg.norm(points, axis=1) Luego, aplicamos la fórmula de atenuación a esas distancias para calcular la intensidad de cada punto. Esto simula la pérdida de señal a medida que los puntos están más alejados del sensor.

Función add_noise_to_lidar

def add_noise_to_lidar(points, std_dev):
    noise = np.random.normal(0, std_dev, points.shape)  # Ruido Gaussiano
    noisy_points = points + noise
    return noisy_points

Esta función recibe los siguientes parámetros de entrada:

• points: np.array (N, 3) - Coordenadas XYZ del LiDAR semántico
• std_dev: float - Desviación estándar del ruido Y devuelve:
• noisy_points: np.array (N, 3) - Nube de puntos con el ruido añadido

Para añadir el ruido, primero se genera un array del mismo tamaño que la cantidad de puntos que se detectan en el sensor LiDar, con muestras aleatorias extraídas de una distribución normal (Gaussiana). Estos valores aleatorios se suman a las coordenadas de los puntos originales, lo que introduce el ruido en los puntos del LiDAR.

Función drop_points

    num_points = points.shape[0]

    # Máscara de eliminación aleatoria
    mask_drop_random = np.random.rand(num_points) < drop_rate

    # Restaurar puntos con alta intensidad (> intensity_limit)
    mask_keep_high_intensity = intensities > intensity_limit
    mask_drop_random[mask_keep_high_intensity] = False  # No eliminamos estos puntos

    # Verificar cuántos puntos tienen intensidad muy baja
    num_low_intensity = np.sum(intensities < low_intensity_threshold)
    print(f"Cantidad de puntos con intensidad menor a {low_intensity_threshold}: {num_low_intensity}")

    # Máscara para eliminar puntos con intensidad baja (por debajo del umbral)
    mask_drop_low_intensity = (intensities < low_intensity_threshold) & (np.random.rand(num_points) < zero_intensity_drop)
    
    # Combinamos todas las eliminaciones
    final_mask = ~mask_drop_random & ~mask_drop_low_intensity

    return points[final_mask],semantic_tags[final_mask]

Esta función recibe los siguientes parámetros de entrada:

• points: np.array (N, 3) - Coordenadas XYZ del LiDAR (sin información adicional como colores o etiquetas).
• semantic_tags: np.array (N,) - Etiquetas semánticas de cada punto.
• intensities: np.array (N,) - Intensidades de los puntos.
• drop_rate: float - Probabilidad de eliminar un punto de forma aleatoria.
• intensity_limit: float - Umbral por encima del cual no se eliminan puntos (protección de puntos con alta intensidad).
• zero_intensity_drop: float - Probabilidad de eliminar puntos con intensidad cero.
• low_intensity_threshold: float - Umbral bajo de intensidad a partir del cual se eliminan los puntos.

Y devuelve: -np.array (M, 3) - Nube de puntos con menos puntos -np.array (M,) - Etiquetas semánticas filtradas

El objetivo de esta función es simular las pérdidas de puntos con la misma lógica que el LiDAR estándar:

• Eliminación Aleatoria: se crea una máscara de eliminación aleaotoria, donde cada punto tiene una probabilidad (drop_rate) de ser eliminado. A cada punto de la nube se le asigna un número aleatorio del 0 al 1, y aquellos que tengan un número inferior a la máscara serán descartados.

• Preservación de Puntos con Alta Intensidad: se crea una máscara que recupera los puntos que tengan una intensidad mayor al umbral establecido. Esto se realiza para simular un entorno real, ya que estos puntos generalmente son los más cercanos y mejor reflejados, y por lo tanto son menos propensos a perderse.

• Eliminación de Puntos con Baja Intensidad: se aplica un umbral de baja intensidad para eliminar de manera aleatoria los puntos con una intensidad inferior a este, ya que normalmente son puntos muy alejados o peor reflejados, y por lo tanto son más propensos a perderse. Según la documentación de CARLA, el LiDAR estándar aplica esto para puntos con intensidad 0, pero como no se detectaban puntos con esa intensidad tras usar la fórmula, se decidió eliminar los puntos con baja intensidad.

• Combinación de Máscaras: Las máscaras de eliminación aleatoria y de eliminación por baja intensidad se combinan en una máscara final (final_mask) que identifica los puntos que deben ser retenidos. Es decir, aquellos puntos que no cumplan con las condiciones de eliminación permanecen en la nube de puntos. Dicha máscara se aplica al array de puntos y de etiquetas semánticas.

Comparación del LiDAR semántico con ruido y pérdidas

Problemas

Se planteó la posibilidad de aplicar las funciones de ruido y pérdidas de puntos antes de la etiquetación de los puntos,con el objetivo de simular de manera más realista el comportamiento del LiDAR, y observar comó se generan errores en la etiquetación. Sin embargo, se encontró que esto no es posible debido a que el LiDAR semántico de CARLA proporciona las etiquetas semánticas directamente con los puntos. Esto implica que las etiquetas de los objetos ya están asignadas antes de que podamos aplicar las modificaciones. Por lo que se hace imposible aplicar ruido o pérdidas antes de la etiquetación sin modificar el código fuente de CARLA.