El algoritmo ICP
Transcript of El algoritmo ICP
![Page 1: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/1.jpg)
VisiónSesión 6: Alineamiento 3D a partir de
nubes de puntos
Departamento CCIAhttp://www.jtech.ua.es/vision/2011/
![Page 2: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/2.jpg)
Hoy
• SfM (Structure from Motion)• Alineamiento 3D (Registration)• Descripción del Algoritmo ICP• Experimentación con ICP• Variantes del ICP• Cámara Kinect• Point Cloud Library (PCL)
• Referencias• Andreas Nüchter• Szymon Rusinkiewicz and Marc Levoy• Bogdan Rusu and Steve Cousins
2
![Page 3: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/3.jpg)
Objetivo: Reconstrucción de entornos 3D• Para reconstruir digitalmente un entorno sin
oclusiones es necesario visualizarlo desde diferentes puntos de vista
• Desde cada punto de vista, se obtiene una representación parcial del entorno (scan 3D)
• Para crear una representación completa y consistente del entorno, todos los scans deben alinearse en un único sistema de coordenadas
3
(R. Gvili)
![Page 4: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/4.jpg)
SfM (Structure from Motion)
• SfM = Alineamiento + Reconstrucción 3D
4
(A. Bartoli)
![Page 5: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/5.jpg)
El Problema del Alineamiento 3D
• Dados dos scans observados desde dos puntos de vista diferentes, en nuestro caso, dos nubes de puntos con su respectivo sistema de referencia local
• Se trata de encontrar la transformación rígida (rotación y traslación) entre los dos sistemas de referencia que alinea puntos correspondientes de las dos nubes (scan matching)
5
¿?
![Page 6: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/6.jpg)
¿Por qué es difícil el alineamiento 3D?
6
![Page 7: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/7.jpg)
Métodos de Scan Matching
• Matching basado en características:• Extraer características (features) distinguibles que
permitan ser identificadas en ambos scans• A partir del emparejamiento correspondiente entre
características de ambos scans, se calcula la transformación rígida que las alinea
• Matching como un problema de optimización:• Usar una función de coste que mide la calidad del
alineamiento entre puntos de ambos scans• Normalmente en un proceso iterativo, se determina la
transformación rígida que minimiza dicha función de coste
7
![Page 8: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/8.jpg)
Cálculo de la Transformación Rígida
• Solución en forma cerrada:• Aplicable cuando se conocen las correspondencias
entre los puntos• 4 métodos matemáticos
• Usando Descomposición en Valores Singulares de una matriz (SVD)
• Usando matrices ortonormales• Usando quaternions• Usando quaternions duales
• Solución iterativa:• Cuando las correspondencias entre los puntos son
desconocidas• Algoritmo ICP (Iterative Closest Points)
8
![Page 9: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/9.jpg)
Descripción del algoritmo ICP
• Sean dos conjuntos de puntos 3D, M (model set) y D (data set), |M| = Nm |D| = Nd
• se trata de encontrar la transformación rígida (R,t) que minimiza la siguiente función de coste:
• donde wi,j determinan las correspondencias de puntos :
• El algoritmo en cada paso de iteración, primero calcula las correspondencias de puntos usando el criterio del punto más cercano, y después calcula la transformación rígida que minimiza la función de coste para dichas correspondencias, utilizando uno de los métodos en forma cerrada
9
1 si mi empareja con dj
0 en otro caso wi,j = ⎨
![Page 10: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/10.jpg)
El algoritmo ICP
10
Repetir
for all dj ∈ D do encontrar el punto más cercano de dj en el conjunto Mend for
Calcular la transformación (R,t) que minimiza la función de coste
Actualizar el conjunto de datos D, aplicándole esta transformación
Calcular la diferencia del error cuadrático: ei = || Ei-1(R,t) - Ei(R,t) ||
hasta que ei < Umbral OR Max_iteraciones
![Page 11: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/11.jpg)
Calcular la Transformación (R,t) con ICP
• Generalmente se calcula primero la rotación R y después la traslación t
• Idea: en la solución correcta las 2 nubes de puntos deberían tener el mismo centroide
• Modificar las 2 nubes de puntos restándoles sus respectivos centroides
• De forma que el problema se reduce a minimizar la función de error:
11
M’ = {m’i = mi - cm} 1,..,ND’ = {d’i = di - dm} 1,..,N
![Page 12: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/12.jpg)
Calcular la Transformación (R,t) con ICP
• Calcular la rotación R usando una solución en forma cerrada (por ejemplo SVD)
• Calcular la traslación t = cm - Rcd
12
cm
cd
Rcd t
![Page 13: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/13.jpg)
Descomposición en Valores Singulares (SVD) de una matriz
• Calcular la matriz 3x3 de correlación H
donde
• Calcular la descomposición en valores singulares de la matriz H = U S Vt
• La matriz 3x3 de rotación es R = V Ut
13
![Page 14: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/14.jpg)
Ejemplo 2D
14
Data set (2057 puntos) Model set (1966 puntos)
Iteración: 1 R;t : 36.7º; (-194.3, 263.8)Error : 75.5
Iteración: 2 R;t : 5.9º; (-48.6, 11.8)Error : 36.7
Iteración: 3 R;t : 1.5º; (-9.4, -6.9)Error : 19.5
Iteración: 4 R;t : 1.4º; (-2.4, -2.8)Error : 14.5
Iteración: 5 R;t GLOBAL: 45.6º; (-241.4, 303.2)Error : 13.4
x
y
(0,0)
![Page 15: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/15.jpg)
Ejemplo 3D: Traslación X
15
Iteración: 1R : (0.15,0.50,0.0)t: : (171.3, -2.8, 0.0)Error : 186.12
Iteración: 2R : (-0.15,-0.50,0.0)t: : (28.7, 2.8, 0.0)Error : 30.67
Iteración: 3R global: (0.0,0.0,0.0)t: global : (200.0, 0.0, 0.0)Error : 0.0
![Page 16: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/16.jpg)
Ejemplo 3D:Rotación Yaw +Traslación X
16
Iteración: 1 R : (-7.10,-0.38,-0.17)t: : (160.4, -2.1, 2.8)Error : 218.24
Iteración: 2 R : (-2.77,0.24,0.11)t: : (38.5, -5.7, -4.8)Error : 51.29
Iteración: 4 R global: (-10.0,0.0,0.0)t: global : (199.7, 0.7, 0.0)Error : 0.85
Iteración: 3 R : (-0.13,0.14,0.04)t: : (1.0., 0.3, 0.2)Error : 2.79
![Page 17: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/17.jpg)
Ejemplo 3D: YawPitchRoll + XYZ
17
Iteración: 1 R : (-2.79,-7.27,-4.95)t: : (92.4, 61.8, 134.3)Error : 385.25
Iteración: 2 R : (-3.66,-4.59,-2.30)t: : (78.4, 18.5, 120.5)Error : 285.22
Iteración: 4 R : (0.64, -2.69, 0.02)t: : (-21.1, 11.6, 50.9)Error : 50.04
Iteración: 3 R : (-2.52, -6.22, -0.13)t: : (4.1, 2.4, 129.6)Error : 173.31
Iteración: 5 R global: (-10.00, -19.99, -10.00)t: global : (200.1, 99.9, 400.4)Error : 0.62
![Page 18: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/18.jpg)
Variantes del ICP
• El algoritmo ICP fue originalmente creado en 1991 de forma simultánea por Chen y Medioni y por Bels y McKay
• Desde entonces han aparecido múltiples variantes que afectan a los distintos pasos del algoritmo:• Seleccionar puntos en uno o ambos conjuntos• Emparejar (matching) puntos entre los dos conjuntos• Ponderar las correspondencias entre puntos• Eliminar correspondencias incorrectas (outliers)• Métrica de error asociado a las correspondencias• Minimizar el error
18
![Page 19: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/19.jpg)
Selección de puntos
• Todos los puntos disponibles
• Muestreo uniforme
• Muestreo aleatorio con diferentes muestras en cada iteración
• Con alto gradiente (color o intensidad)• Muestreo con la mayor distribución de normales
• Muestreo uniforme en cada sub-espacio angular de normales
• Facilita la convergencia en escenas con características pequeñas y dispersas
19
(Levoy)
![Page 20: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/20.jpg)
Emparejamiento de puntos
• Encontrar el punto más cercano en el otro conjunto (Closest Point)• Para acelerar la búsqueda, se utiliza un Kd-tree
• Encontrar la intersección del rayo en la dirección de la normal, con el conjunto modelo (Normal Shooting)
• Proyectar el punto sobre el conjunto modelo desde el punto de vista de la cámara del conjunto modelo (Reverse Calibration)
20
http://en.wikipedia.org/wiki/File:KDTree-animation.gif
(Levoy)
(R. Gvili)
![Page 21: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/21.jpg)
Ponderación de correspondencias
• Pesos constantes
• Asignar pesos en función de la distancia entre los puntos:
• Ponderar en función de la “compatibilidad” de normales:
• Ponderar en función del color
• Ponderación basada en la incertidumbre de la medición (ruido del sensor)• Por ejemplo, con la kinect las mediciones entre
[60cm..1,5m] son más fiables que mediciones > 3m
21
Weight = n1 . n2
![Page 22: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/22.jpg)
Eliminación de correspondencias (outliers)
• Eliminar correspondencias con distancia entre sus puntos superior a un determinado umbral
• Eliminar un determinado porcentaje (por ejemplo 10%) de los peores emparejamientos, según alguna métrica (por ejemplo distancia entre puntos)
• Eliminar emparejamientos cuya distancia entre sus puntos sea superior a un determinado múltiplo (por ejemplo, 2.5) de la desviación típica de las distancias
22
(R. Gvili)
![Page 23: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/23.jpg)
Eliminación de outliers
• Eliminar emparejamientos que no son consistentes con otros emparejamientos vecinos• Dos emparejamientos (p1,q1) y (p2,q2) son
inconsistentes sii
• Eliminar correspondencias que contienen puntos en los límites del conjunto destino• Aplicable cuando el solapamiento entre los conjuntos
no es completo
23
< Umbral
(R. Gvili)
(R. Gvili)
![Page 24: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/24.jpg)
Medición del error de emparejamiento
• Métricas punto-a-punto que consideran la suma de distancias al cuadrado entre puntos correspondientes• También pueden tener en cuenta la diferencia de
color
• Distancia punto-al-plano que considera la suma de distancias al cuadrado entre cada punto con el plano más cercano (perpendicular a la normal del punto destino)
24
(R. Gvili)
![Page 25: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/25.jpg)
Minimización del error
• Generar repetidamente emparejamientos usando la transformación actual, y encontrar una nueva transformación que minimiza la métrica de error
• Minimización iterativa combinada con extrapolación en el espacio de las transformaciones para acelerar la convergencia
• Realizar la minimización iterativa con distintas perturbaciones en las condiciones iniciales, y seleccionar el mejor resultado
• Realizar la minimización iterativa usando distintos subconjuntos de puntos seleccionados aleatoriamente, y elegir el resultado óptimo usando una métrica robusta (least median of squares)
25
![Page 26: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/26.jpg)
La cámara Kinect
• Comercializada desde Noviembre de 2010• Desarrollada por Primesense y utilizada por
Microsoft para la consola Xbox 360• Reconoce gestos, comandos de voz, objetos e
imágenes• Se ha extendido su uso a la comunidad científica• APIs
• OpenNI (Primesense)• SDK (Microsoft)• OpenKinect (comunidad abierta)
26
![Page 27: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/27.jpg)
Sensores de la cámara Kinect
• Cámara RGB (1)• 32 bits de color• 640x480 pixels• 57º x 47º campo visual• 30 Hz
• Sensor de profundidad• proyector infrarrojo (2)• cámara de infrarrojos (3)
• 16 bits de profundidad• 320x240• 30 Hz
• 1,2 a 3,5 metros• Motor de inclinación (4)
• Tilt (hacia arriba o abajo) hasta 27º• Micrófonos multiarray (5)
• 4 canales a 16 bit de audio• 16 kHz
27
(1)(2) (3)
(4)(5)
![Page 28: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/28.jpg)
Cálculo de la profundidad
• Se proyecta un patrón de puntos con un láser infrarrojo
• A partir de la luz infrarroja que llega rebotada a la cámara de infrarrojos se obtiene la profundidad a la que se encuentra cada pixel• El patrón de puntos está memorizado para una profundidad
conocida. Al poner objetos delante, el patrón aparece distorsionado y así se puede calcular la profundidad
• Como hay menos puntos en el patrón de IR que el mapa de profundidad, algunas partes de este mapa son interpoladas
• http://www.freepatentsonline.com/20100118123.pdf• http://www.youtube.com/watch?v=IHsPb5Yv5G8
28
(J. Kjaer)
![Page 29: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/29.jpg)
Capacidades de la kinect
• Ventajas• Bajo coste (100€)• Proporciona 30 fps con resolución 640x480• Es capaz de detectar profundidad en zonas sin
textura • Puede funcionar a oscuras
• Inconvenientes• Alcance limitado (50cm - 4m)• Falla en zonas que reflejan mal la luz infrarroja• No funciona en entornos de exterior• Suele fallar en los bordes de los objetos
29
![Page 30: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/30.jpg)
PCL (Point Cloud Library)
• Librería de procesamiento de nubes de puntos 3D• Software de libre distribución (licencia BSD),
desarrollado por la empresa Willow Garage• Incluye algoritmos de
• Filtrado• Extracción de características• Reconstrucción de superficies• Alineamiento• Ajustes de modelo• Segmentación• Visualización
• Implementada en C++• Plataformas de desarrollo Linux, MacOS, Windows
y Android• Soporta de forma nativa las interfaces 3D de
OpenNI
30
![Page 31: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/31.jpg)
Estructuras básicas en PCL
• Tipo PointT• Existen distintos tipos de puntos predefinidos:
• PointXYZ, PointXYZRGB, PointNormal, etc.• Se pueden añadir tipos definidos por el usuario• http://pointclouds.org/documentation/tutorials/
adding_custom_ptype.php#adding-custom-ptype
• Tipo PointCloud• Permite almacenar una nube de puntos
• int width• int height• std::vector<PointT> points
• Nube de puntos organizada (height = número de filas) o desorganizada (height = 1)
• Ejemplo:pcl::PointCloud<pcl::PointXYZ> cloud;std::vector<pcl::PointXYZ> data = cloud.points;// Nube organizadacloud.width = 640; cloud.height = 480;// Nube desorganizadacloud.width = 307200; cloud.height = 1;
• http://pointclouds.org/documentation/tutorials/basic_structures.php#basic-structures
31
![Page 32: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/32.jpg)
Librerías de PCL • libpcl_filters
• Filtros de datos: reducción de la resolución (downsampling), eliminación de outliers, etc.
• libpcl_features• Extracción de características 3D: normales de
superficies, puntos frontera, descriptores SIFT, NARF, etc
• libpcl_io• Entrada y Salida: escribir y leer ficheros en formato
PCD (Point Cloud Data)
• libpcl_segmentation• Segmentación: extracción de clusters, ajustes de
modelo mediante métodos de consenso, etc
• libpcl_surface• Reconstrucción de superficies
• libpcl_registration• ICP
• libpcl_keypoints• Extracción de puntos clave que puede utilizarse para
decidir dónde extraer descriptores de características
• libpcl_range_image• Dar soporte a mapas de profundidad creados a partir
de nubes de puntos
32
![Page 33: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/33.jpg)
Procesamiento pipeline en PCL
1. Crear el objeto de procesamiento (filtro, estimador de características, segmentación, etc)
2. Utilizar setInputCloud para pasar la nube de puntos de entrada al módulo de procesamiento
3. Actualizar algunos parámetros4. Invocar al correspondiente módulo de
procesamiento (compute, filter, segment, etc) para obtener el resultado
33
(R.B. Rusu)
![Page 34: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/34.jpg)
Proyecto KinectFusion
• Un proyecto para la kinect que fusiona el mundo real y el virtual
• Generar modelos 3D en tiempo real: objetos, personas y habitaciones enteras
• Motor de física realista que permite escanear objetos para ser manipulados con realismo
34
http://www.youtube.com/watch?v=quGhaggn3cQ
![Page 35: El algoritmo ICP](https://reader034.fdocuments.us/reader034/viewer/2022052300/58a1a6501a28ab9b4d8c4716/html5/thumbnails/35.jpg)
Referencias
• 3D Robotic Mapping: The Simultaneous Localization and Mapping Problem with Six Degrees of Freedom. Andreas Nüchter (Springer 2009).: cap. 4
• Efficient Variants of the ICP Algorithm. Rusinkiewicz and Levoy. (3DIM 2001)
• 3D is here: Point Cloud Library (PCL). Radu Bogdan Rusu and Steve Cousins (ICRA 2011)
• http://pointclouds.org• http://en.wikipedia.org/wiki/Kinect
35