Por qué falla ICP: inicialización, valores atípicos y geometría simétrica

ICP alterna la correspondencia de vecinos más cercanos y la estimación de transformación rígida. Un residuo menor no prueba que la pose recuperada sea correcta. Las formas 2D simples nos permiten aislar la inicialización, los valores atípicos y la simetría sin ruido del sensor.

Objetivo

Esta implementación minimiza las distancias punto a punto al cuadrado. En cada iteración, encuentra los vecinos objetivo para los puntos fuente transformados, mantiene las coincidencias dentro de un umbral de distancia y estima la rotación/traslación mediante SVD. Una corrección de determinante evita la reflexión. Se detiene después de un máximo de 100 iteraciones. El tutorial de Open3D ICP distingue entre objetivos punto a punto y punto a plano; aquí solo se implementa el primero.

Entradas y ejecución

Descargue el script y ejecute python3 engineering_labs.py. Se verificó con Python 3.12.3, NumPy 1.26.4 y Matplotlib 3.6.3. No se requiere Open3D, y esta no es una comparación de velocidad con su implementación.

El objetivo es una L formada por segmentos de 3 m y 2 m, con un total de 102 puntos y una esquina duplicada. Los puntos de origen se generan mediante la inversa de la rotación real de 20° y la traslación de (0.4, -0.3) m. No se añade ruido del sensor. La inicialización correcta es de 18° y (0.35, -0.25) m; la incorrecta es de 110° y (1.5, 1) m.

Leer tres tipos de evidencia

El residuo representa el RMSE sobre las distancias aceptadas entre vecinos más cercanos. La cobertura es la fracción de todos los puntos de origen dentro del umbral. La verdad fundamental sintética también permite errores de rotación y traslación.

Diagram 1 · Use the button to switch views

Incluso una buena inicialización deja un error: los puntos vecinos en segmentos muestreados regularmente permiten una solución local desplazada. Una cobertura de 1.000 por sí sola no lo detectaría. Los valores atípicos se encuentran en la fuente x=4–7 m, y=3 m. Descargue el objetivo, la entrada/salida de valores atípicos y el resumen.

Direcciones no observables en círculos y paredes

El círculo tiene un radio de 1 m y 120 puntos, con la identidad definida como la transformación verdadera. Su conjunto de puntos también coincide después de una rotación de 90°, por lo que un residuo prácticamente nulo coexiste con un error de pose de 90°. Más iteraciones no pueden proporcionar la información faltante.

Una pared presenta un problema relacionado, pero diferente. El error punto-plano mide el desplazamiento a lo largo de la normal a la pared y restringe de forma deficiente el movimiento tangencial. Las paredes finitas punto-punto pueden obtener información de los puntos finales y su ubicación, pero esto se debilita en un corredor largo sin puntos finales visibles. El círculo representa un experimento realizado; la discusión sobre la pared explica las restricciones direccionales del objetivo.

Un umbral menor no es una solución universal

El umbral de 0,5 m elimina estos valores atípicos, pero también puede eliminar correspondencias correctas cuando la inicialización se realiza lejos. Diferentes umbrales seleccionan diferentes puntos, por lo que los residuos por sí solos no son directamente comparables. Este código deja de actualizarse con menos de tres puntos aceptados; tres puntos no garantizan una buena geometría.

Entre las opciones prácticas se incluyen la inicialización de la odometría, la alineación de grueso a fino, el filtrado de valores atípicos y la observación de superficies con diferentes orientaciones. Mida sus efectos en los mismos intervalos y bajo las mismas condiciones de referencia.

Continuar con la evaluación SLAM

Este es un registro 2D de un solo fotograma, no una prueba de precisión de control 3D ni de robustez ante objetos en movimiento. La evaluación SLAM añade errores de trayectoria, alineación temporal e intervalos de fallo. Varíe primero solo el ángulo inicial para ver por qué la convergencia no se puede resumir con un único valor residual.

Ejecutar el mismo cálculo localmente

Extraiga el archivo ZIP de código fuente ejecutable completo, o guarde el punto de entrada del experimento, los kernels generados y el código fuente PID original en una carpeta. PID utiliza la biblioteca estándar de Python; Kalman e ICP requieren NumPy. Los comandos originales de generación de figuras siguen estando disponibles.

python3 -m pip install numpy==1.26.4
python3 experiment.py icp --parameters '{"case":"good_init","threshold":0.5}' > result.json

Prueba con bad_init, outliers y symmetric_ring. Compara los umbrales 10 y 0.5 para outliers. Verifica que el anillo pueda mantener un error de rotación de 90 grados incluso con un residuo cercano a cero.

python3 reproduce.py --experiment icp