VisiónSesión 6: Alineamiento 3D a partir de

nubes de puntos

Departamento CCIAhttp://www.jtech.ua.es/vision/2011/

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

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)

SfM (Structure from Motion)

• SfM = Alineamiento + Reconstrucción 3D

4

(A. Bartoli)

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

¿?

¿Por qué es difícil el alineamiento 3D?

6

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

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

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 = ⎨

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

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

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

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

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)

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

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

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

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

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)

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)

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

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)

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)

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)

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

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

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)

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)

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

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

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

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

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)

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

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