Algoritmo Adaptativo para laExtracción del Modelo 3D deObjetos Poliédricos usando el

Sensor Swissranger

RRI I

Institut de Robòtica i Informàtica Industrial

Daniel Rodrigo

Agosto 2007

IRI-DT 05/2007

Technical Report

Algoritmo adaptativo basado en Percepcion Activa para la

extraccion del modelo 3D de objetos poliedricos

Daniel Rodrigo Lopez

19 de septiembre de 2007

1. Introduccion

En este escrito se pretende describir un metodo para extraer el modelo tridimensional de un objetopoliedrico, formado unicamente por planos. Mediante un brazo robotico Staubli se controla el sensor 3DSwissRangerTMSR3100, el cual retorna en cada captura las coordenadas 3D de la nube de puntos. Lapeculiaridad del algoritmo es que se trata de un metodo adaptativo, en el sentido que determina la posiciondesde la cual debe realizarse la proxima captura a partir de la informacion del objeto obtenida hasta elmomento. Esta decision se toma de forma que en cada iteracion se obtenga el maximo de informacionposible, reduciendo al maximo el desconocimiento del objeto. El algoritmo se descompone en tres etapasprincipales:

1. Captura y extraccion de planos

2. Promediado de diferentes vistas

3. Determinacion del siguiente punto de observacion

En las primeras secciones se describiran las caracterıstacas tanto del sensor 3D SwissRangerTM comodel brazo robotico Staubli. Las siguientes secciones tratan cada una de las etapas del metodo principal,discutiendo los algoritmos usados, las expresiones matematicas involucradas y algunos resultados par-ciales. A continuacion se ilustra el funcionamiento global del sistema en un caso concreto y se exponen laslimitaciones del mismo. Por ultimo se describen y discuten dos modificaciones substanciales del algoritmogeneral.

1

2

2. SwissRangerTMSR-3100

Como herramienta de captura usamos la SwissRangerTMSR-3100 cuyas caracterısticas se describenen [2] y [7]. Se trata de un sensor 3D que captura imagenes tridimensionales de alta definicion.

Figura 1: SwissRangerTMSR-3100

Este sensor basa su funcionamiento en el principio de tiempo de vuelo (Time-Of-Flight Principle)trabajando con luz infraroja modulada. Esta senal emitida es reflejada en el objeto, y recibida en cadauno de los pıxels del sensor. Mediante la comparacion de fase entre la senal enviada y recibida se calculala distancia hasta el punto de incidencia. De esta forma, a diferencia de las camaras convencionales,que capturan unicamente una imagen de intensidad, el sensor Swissranger obtiene ademas el mapa dedistancias completo. Concretamente, proporciona una matriz de 176x144 pıxels con las coordenadas 3Dde cada punto y la correspondiente matriz de intensidades.

Figura 2: Funcionamiento basado en TOF

3

3. Staubli

Para variar la posicion del sensor se ha utilizado un brazo robotico STAUBLI RX60. Este es unbrazo articulado con 6 grados de libertad, que tras su calibracion permite mover el sensor en coordenadascartesianas x, y, z y controlar la orientacion del mismo.

Figura 3: Brazo robotico Staubli TX40

Controlamos el brazo desde un PC estableciendo comunicacion vıa socket. Utilizamos dos funcionesprincipales, una de peticion de la posicion actual y otra de ejecucion de un movimiento hacia una posiciondeterminada.

La situacion del robot es proporcionada en la forma (x, y, z, Rx, Ry, Rz). Las tres primeras compo-nentes, (x, y, z), estan en milımetros y representan la posicion en coordenadas cartesianas. Las otras trescomponentes, (Rx, Ry, Rz), estan en grados y son los angulos de Euler que representan la orientacion.Concretamente estos angulos representan las rotaciones respecto a los ejes x, y, z que deben aplicarse (eneste orden) para transformar el sistema de coordenadas canonico en la orientacion actual1. En la figura4 se ilustra el caso concreto de la orientacion Rx = 20o, Ry = 10o, Rz = 30o.

Figura 4: Orientacion del Staubli segun los angulos de Euler

1Habitualmente los angulos de Euler hacen referencia a rotaciones segun los ejes z, x, z, y no a las actuales x, y, z

4

4. Segmentacion y Extraccion de Planos

A partir de la nube de puntos capturada mediante el sensor Swissranger se desea extraer los planosque conforman el objeto y las caracterısticas de los mismos. La figura 5 ilustra el proceso.

Figura 5: Ejemplo de segmentacion y extraccion de planos

Dividimos esta tarea en tres pasos:

1. Calculo de la normal en cada punto.

2. Segmentacion de la nube de puntos.

3. Determinacion de los planos ası como sus centroides y vectores normales.

4.1. Calculo normales

Para realizar el calculo de las normales seguimos el metodo descrito en [4]. Basicamente consiste entomar un cuadrado Wi,j centrado en cada punto ~pi,j (en nuestro caso hemos escogido N=7, es decir, uncuadrado 7x7) y sobre estos puntos ajustar un plano por mınimos cuadrados.

La ecuacion del plano se puede escribir como

~p · n = d

Segun esta notacion el error cometido es

ε =∑

k,l∈Wi,j

(~pT

k,l · n− d)2

= nT Qn− 2d~qT n + N2d2

dondeQ =

∑

k,l∈Wi,j

~pk,l~pTk,l

~qT =∑

k,l∈Wi,j

~pTk,l

5

Tomamos el siguiente lagrangiano

l(n, d, λ) = ε + λ(1− nT n

)

Derivando respecto a n y d, y substituyendo llegamos a:(

Q− ~q~qT

N2

)n = λn

De esta forma la solucion es un vector propio de la matriz anterior. Dado que por simple substitucionse puede comprobar que el error global coincide con el valor propio asociado al vector, basta encontrar elvector propio de valor propio mınimo de la matriz

R =

(Q− ~q~qT

N2

)

Figura 6: Resultado del calculo de las normales

4.2. Segmentacion

Realizaremos un particion de la nube de puntos en conjuntos, donde cada uno de estos sera candidatoa ser uno de los planos del objeto observado. Tomaremos la nube como un grafo donde cada punto es unvertice y tendremos arista entre los puntos ~pi,j y ~pk,l si se verifican las siguientes condiciones

1. Puntos Adjacentes: |i− k|+ |j − l| = 1

2. Puntos cercanos: ‖~pi,j − ~pk,l‖ ≤ UMBRAL DISTANCIA

3. Normales similares: cos−1∣∣∣nT

i,jnk,l

∣∣∣ ≤ UMBRAL CURVATURA

Unicamente debemos encontrar las componentes conexas del grafo descrito, que se puede realizarmediante una busqueda en anchura (BFS).

Tambien se ha implementado el algoritmo de segmentacion descrito en [6]. Dicho algoritmo tomacomo punto inicial la particion mas fina posible (cada punto es un subconjunto) e iterativamente vauniendo componentes. Define la variacion interna de una componente y la variacion externa entre doscomponentes. El criterio de union que se toma es que la variacion externa entre dos componentes seainferior a su variacion interna conjunta. Su funcionamiento se ilustra en la figura 7

6

Figura 7: Ilustracion del funcionamiento del algoritmo 2

Este algoritmo tiene la ventaja de capturar propiedades NO locales (cosa que no ocurre en el anterior)y se utiliza principalmente para segmentacion de imagen segun color. Sin embargo, su adaptacion al casode extraccion de planos ha producido unos resultados comparativamente peores a los del primer algoritmo,tal y como se observa en la figura 8.

Figura 8: Comparativa entre los dos algoritmos de segmentacion

7

4.3. Extraccion de planos

Como es de esperar no todas las componentes de la particion anterior corresponden a planos del objeto.Como criterio de validacion de un plano, se ha impuesto que la componente conexa pueda contener uncuadrado de lado fijado, en nuestro caso 10x10 pıxels.

Otra decision a priori coherente de validar los planos es en funcion de la cantidad de puntos que ledan soporte. A la practica este no es un buen criterio, ya que la interseccion de dos planos da lugar a unacomponente conexa con muchos puntos que le dan soporte, pero que obviamente no es un plano valido.

Para el calculo del centroide del plano promediamos las coordenadas de todos sus puntos. De for-ma analoga calculamos su normal, pero teniendo en cuenta que debe deshacerse la ambiguedad entre{+n,−n} para sumar todas las normales con el mismo signo y que el vector resultante debe normalizarse.Ademas escogeremos el sentido de dicho vector de forma que el producto escalar entre este y el vector deobservacion sea negativo, es decir, tomamos la orientacion hacia el exterior del objeto.

8

5. Combinacion de diferentes capturas

El paso inmediato posterior a la extraccion de planos es la comparacion con los planos ya detectadosanteriormente y la adjuncion de la nueva informacion. Para ello deberemos decidir cuales son los planosobservados por primera vez en la nueva vista y cuales ya han sido detectados en observaciones anteriores.Para promediar las diferentes observaciones de un plano usaremos el filtro de Kalman.

5.1. Identificacion planos

Para decidir la correspondencia entre planos necesitamos que los datos nuevos y antiguos esten en elmismo sistema de referencia. La solucion mas sencilla, es inmediatamente despues a la captura medianteSwissranger aplicar la transformacion pertinente a la nube de puntos, de forma que las nuevas coordenadasesten en el sistema WORLD de brazo robotico Staubli.

Usamos la siguiente notacion:

~pW = coordenadas del punto en el sistema de referencia WORLD~pS = coordenadas del punto en el sistema de referencia de Swissranger

r = posicion de la camara en el sistema WORLD(α, β, δ) = angulos de orientacion en el sistema WORLD

Tenemos la siguiente relacion:

~pW = r + (Gx(α) ·Gy(β) ·Gz(δ)) (~pS)

Destacamos que los angulos son α, β, δ y no sus opuestos; ya que si tenemos los puntos en las coorde-nadas de la camara, un giro de esta es equivalente a un giro de sentido opuesto de los puntos.

Determinaremos que un plano corresponde con otro ya observado si satisface

‖Centroidnew − Centroidold‖ ≤ UMBRAL DISTANCIA CENTROIDES

cos−1 (nnew · nold) ≤ UMBRAL ANGULO NORMALES

donde los umbrales son menos restrictivos que los del proceso de segmentacion.

5.2. Filtro de Kalman

El filtro de Kalman, descrito en [9] y [3], es un conjunto de ecuaciones matematicas que proveenuna solucion eficiente para la estimacion del estado de un proceso en el sentido de mınimos cuadrados.Tenemos dos ecuaciones fundamentales:

ECUACION DE ESTADO: ~xk = Fk~xk−1 + ~wk

ECUACION DE OBSERVACION: ~zk = Hk~xk + ~vk

donde

Fk = matriz de transicionHk = matriz de observacion~wk = ruido de transicion~vk = ruido de observacionPk = matriz de covarianza de ~xk

Aplicaremos el filtro de Kalman a cada uno de los planos de forma independiente, ya que tal y comose ha planteado el sistema, no estudiaremos interrelaciones entre planos (a excepcion de la extraccion de

9

las adjacencias entre planos). De esta forma el vector de estado consistira en el centroide y la normal delplano en el sistema WORLD. Dado que este no varıa, Fk y ~wk seran nulos.

La matriz de observacion Hk es basicamente la matriz de giro que cambia las coordenadas WORLDa coordenadas del sensor Swissranger. El ruido de observacion ~vk podemos suponerlo gaussiano de medianula y matriz de covarianza Rk diagonal. Esta matriz R sera invariable con el tiempo pues las carac-terısticas de la camara no varıan y predominara la componente cz ya que en esta componente el sensorSwissranger presenta mayor error.

~xTk =

(cx cy cz nx ny nz

)

Fk = 0, ~wk = ~0

H = Gz(−δ) ·Gy(−β) ·Gx(−α)

~v ∼ N(0,R)

R =

σ2cx

σ2cy 0

σ2cz

σ2nx

0 σ2ny

σ2nz

Dado que no hay variacion de estado, el proceso de estimacion es trivial.

xk|k−1 = xk−1|k−1

Pk|k−1 = Pk−1|k−1

Si denotamos la posicion de la camara en el sistema WORLD como ~rk, el proceso de correccion es elsiguiente:

zk = Hk ·(xk|k−1 −~rk

)

Sk = Hk · Pk|k−1 ·HTk + R

Kk = Pk|k−1 ·HTk · S−1

k

~xk|k = xk|k−1 + Kk · (~zk − zk)

Pk|k = (I−KkHk) · Pk|k−1

En el caso que el plano sea observado por primera vez debemos inicializar su matriz de covarianza.En este caso la varianza en las coordenadas de la camara coincidira con la matriz R. Ası:

R = E[~z · ~zT

]= E

[(H · (~x−~r)) · (H · (~x−~r))T

]= H ·P ·HT

P = HT ·R ·H

10

6. Determinacion de la siguiente posicion

La parte central del estudio es a partir de la informacion disponible hasta el momento, determinarcual debe ser la siguiente posicion de forma que la captura aporte el maximo de informacion posible.Para ello se ha escogido el algoritmo Mutual Information Gain descrito en [8]. Este consiste basicamenteen escoger un conjunto finito de movimientos y tomar aquel que maximiza la informacion mutua entreestado y observacion, es decir el que minimiza la entropıa estimada tras el movimiento.

6.1. Conjunto de movimientos

El sensor se mueve sobre una esfera con centro en el promedio de los centroides de los planos detectadosy con radio igual a la distancia actual entre el sensor y el anterior punto. Tomamos un conjunto de 8movimientos posibles tal y como se observa en la figura 9. El movimiento ~v1 sigue un meridiano, elmovimiento ~v2 es el ortogonal a este y ~v3, ~v4 son la suma y resta de los dos anteriores. Los otros cuatromovimientos son los opuestos de ~v1, ~v2, ~v3, ~v4.

Object(xo,yo,zo)

SwissRanger(xs,ys,zs)

v1v4

v3

v2

u

Figura 9: Conjunto de los 8 movimientos posibles

El metodo mas comodo para el calculo de las nuevas posiciones es normalizar el sistema de formaque la esfera anterior quede centrada en el (0, 0, 0) y tenga radio 1. Posteriormente calcular las nuevasposiciones sobre la esfera unitaria y trasladar los resultados al sistema global.

R =√

(xs − x0)2 + (ys − y0)2 + (zs − z0)2

x = (xs − x0)/R y = (ys − y0)/R z = (zs − z0)/R

~v1 =(zx, zy, z2 − 1)‖(zx, zy, z2 − 1)‖

~v2 = (x, y, z)× ~v1

~v3 =~v1 + ~v2

‖~v1 + ~v2‖~v4 =

~v1 − ~v2

‖~v1 − ~v2‖

~rnew = (x0, y0, z0) + R · (x, y, z) + λ~vi

‖(x, y, z) + λ~vi‖

11

Mediante el parametro λ controlamos la distancia entre capturas. Concretamente, si tomamos λ =tan(γ), nos estaremos desplazando un angulo γ respecto al centro de la esfera.

Ademas de la nueva posicion debemos determinar tambien la nueva orientacion, de manera que elsensor quede orientado hacia el promedio de los centroides. De hecho, solo debemos determinar el valorde las rotaciones respecto a los ejes x e y, ya que la ultima rotacion respecto al eje z no modifica el puntocentral observado. Debemos imponer que el vector de direccion ~u en el sistema WORLD despues de loscambios de coordenadas, coincida con el vector ~z = (0, 0, 1). Estas son las ecuaciones.

~u =~r0 −~rs

‖~r0 −~rs‖(Gz(δ) ·Gy(β) ·Gx(α)) ~u = ~z

(Gx(−α) ·Gy(−β) ·Gz(−δ))~z = ~u

− sin (β)

sin (α) cos (β)

cos (α) cos (β)

= ~u

β = − sin−1(ux)

α = cos−1

(uz

cos(β)

)

Como era de esperar el valor de δ no aparece en las anteriores ecuaciones, ya que la ultima rotacionsobre el eje z no varıa el vector de observacion. Hemos tomado dicho valor de forma que la direccion ~xdel sensor sea paralelo al plano XY del sistema global.

(Gx(−α) ·Gy(−β) ·Gz(−δ))~x =

cos (β) cos (δ)

sin (α) sin (β) cos (δ)− cos (α) sin (δ)

cos (α) sin (β) cos (δ) + sin (α) sin (δ)

cos (α) sin (β) cos (δ) + sin (α) sin (δ) = 0

δ = − tg−1

(− sin(β)tg(α)

)

12

6.2. Mutual Information Gain

Tomamos la entropıa diferencial como medida de incertidumbre. Se define la entropıa diferencial deuna variable aleatoria continua con funcion de densidad f(x), como H(X) =

∫f(x) log f(x)dx. En el

caso de variables multinormales con media nula y varianza P, se reduce a H(X) = 12 log [(2π)n|P|].

Dada la prediccion de estado ~xk+1|k y la prediccion de observacion zk+1 maximizaremos la informacionmutua I(X,Z).

I(X, Z) = H(X)−H(X|Z) =

=12

log[(2π)n|Pk|k−1|

]− 12

log[(2π)n|Pk|k|

]=

=12

log[ |Pk|k−1||Pk|k|

]=

=12

log[ |Pk|k−1||Pk|k−1 −Pk|k−1HT S−1HPk|k−1|

]

Como vemos, maximizar la informacion mutua, es equivalente a maximizar la disminucion de entropıaentre una captura y la siguiente, que es exactamente lo que queremos. Dado que Pk|k−1 no cambia segunla direccion de movimiento basta minimizar |Pk|k−1 −Pk|k−1HT S−1HPk|k−1|.

En realidad, como nuestro sistema trata los planos de forma independiente, la formula anterior equivaleal producto de los determinantes correspondientes a cada uno de los planos. De esta forma evaluamos laexpresion anterior para cada uno de los ocho movimientos y escogemos aquel que la hace mınima.

13

7. Mapa topologico

Para facilitar la representacion tridimensional del objeto, se ha decidido extraer de las vistas no sololos planos, si no tambien el mapa topologico del objeto. Por mapa topologico entendemos el grafo deadjacencias de los planos que forman el objeto, es decir, el grafo que tiene como vertices los planos delobjeto y en el cual dos vertices estan unidos por una arista, si los planos correspondientes son adjacentesen el objeto.

Para obtener este grafo, se han detectado las adjacencias en cada una de las vistas y posteriormentese han trasladado los resultados al mapa topologico global. Concretamente para cada par de planosque aparecen en la captura actual, se ha sumado o restado una unidad en el mapa topologico global,dependiendo si en la vista actual hay o no adjacencia. De esta forma, valores positivos en la matriz deadjacencias indican la existencia de arista y valores negativos indican lo contrario.

La idea basica utilizada para determinar adjacencias, es que tras la segmentacion debe haber unalto numero de puntos fronterizos cercanos a la recta resultante de la interseccion entre los dos planosimplicados. El algoritmo concreto es el siguiente: Para cada pareja de planos πi, πj calculamos la rectainterseccion ri,j . Sea ni,j,1 el numero de puntos fronterizos del plano πi cuya recta mas cercana es ri,j , yde forma analoga, ni,j,2 el numero de puntos fronterizos del plano πj cuya recta mas cercana es ri,j . Sise verifica

ni,j,1 ≥ UMBRAL ADJACENCIA

ni,j,2 ≥ UMBRAL ADJACENCIA

entonces se determina que los planos son adjacentes.

Figura 10: Ilustracion de la creacion del mapa topologico

14

8. Funcionamiento global

Para ilustrar el funcionamiento global del sistema, mostramos las 8 primeras iteraciones del algoritmo.En la figura 11 se muestra la sucesion de imagenes de intensidad y en la figura 12 sus respectivas seg-mentaciones. Entre una captura y la siguiente, la posicion se ha modificado 0.05 radianes (' 3o) respectoal promedio de los centroides.

Figura 11: Sucesion de imagenes de intensidad

Figura 12: Sucesion de imagenes segmentadas

Como resultado tenemos 7 planos. Sin embargo, uno de ellos presenta una matriz de covarianza convalores muy superiores al resto. Este plano proviene de la sobresegmentacion del plano 4.

15

Los 6 planos restantes son:

Plane 1: centroid = (0,8864, −0,6653, −0,1987) normal = (0,8326, 0,5483, −0,0780)Plane 2: centroid = (0,9664, −0,5653, −0,1861) normal = (0,5388, −0,8405, 0,0568)Plane 3: centroid = (1,0110, −0,4298, −0,2921) normal = (0,5773, −0,8154, −0,0420)Plane 4: centroid = (0,9528, −0,5280, −0,2867) normal = (0,8040, 0,4404, −0,3995)Plane 5: centroid = (1,0440, −0,4645, −0,2407) normal = (0,1476, −0,1226, −0,9814)Plane 6: centroid = (1,0070, −0,5843, −0,1002) normal = (0,0519, −0,1188, −0,9916)

El mapa topologico resultante es el siguiente:

−8 +8 −8 −9 −7 +4+8 −8 −8 +7 +7 +4−8 −8 −8 +5 +7 −4−9 +7 +5 −7 +2 −4−7 +7 +7 +2 −7 −4+4 +4 −4 −4 −4 −4

Figura 13: Planos finales tras el promediado de las 8 vistas

16

9. Limitaciones

La principal limitacion que condiciona el sistema es el rango de vision del sensor SwissRangerTMSR3100.Desde el punto de vista de la optica y calibracion del mismo, se requiere que el objeto observado se en-cuentre a una distancia mayor de 0.5 metros. Esto, a su vez, supone que el objeto en cuestion tenga unasdimensiones mınimas.

Dado que el brazo robotico Staubli se desplaza sobre una esfera centrada en el elemento observado, unaumento de su radio implica aumentar a su vez la amplitud de sus movimientos. Esto ultimo conlleva queuna gran parte de la superficie de la esfera de desplazamiento se encuentre fuera de los lımites alcanzablespor el brazo robotico. En la figura 14 se muestra este margen de movimiento.

Figura 14: Region de movimiento del brazo robotico

17

10. Mejoras del algoritmo

10.1. Modificacion 1:Matriz de covarianza del ruido de observacion

En los capıtulos 5 y 6 se usa la siguiente relacion entre estado y observacion:

~zk = Hk~xk + ~vk

~v ∼ N(0,R)

El ruido de observacion ~v se supone siempre con distribucion gaussiana; sin embargo existen variasalternativas al valor que debe tomar su matriz de covarianza R. En los capıtulos anteriores se tomo

R =

σ2cx

σ2cy 0

σ2cz

σ2nx

0 σ2ny

σ2nz

y los valores de σcx, σcy, σcz, σnx, σny, σnz se consideraron fijos. Concretamente se dio mayor ponderaciona σcz, ya que el sensor SwissRangerTM comete un mayor error en esta direccion.

Una alternativa a este criterio surge de la idea que los parametros de cada plano (centroide y normal),se calculan como el promedio de las diferentes mediciones obtenidas tras la segmentacion. En el casodel centroide, se promedian directamente las coordenadas de los puntos, mientras que en el caso de lanormal, se promedian las normales asociadas a cada punto del plano. En ambos casos se esta realizandoun promedio de kj

i valores, donde kji es el numero de puntos que forman el i-esimo plano en la j-esima

captura.Es coherente considerar como independientes los datos relativos a cada medicion2. Teniendo en cuenta

que la varianza del promedio de una muestra disminuye linealmente segun el tamano de la misma, debemostomar

Rji =

1kj

i

σ2cx

σ2cy 0

σ2cz

σ2nx

0 σ2ny

σ2nz

Este nuevo procedimiento supone un cambio conceptual significativo respecto al antiguo sistema; yaque estamos considerando que la estadıstica del error es diferente para cada plano. De hecho, no solovarıa en funcion del plano, si no que tambien varıa temporalmente a consecuencia de la modificacion dela posicion de observacion. Tal y como se ilustra en la figura 15, posiciones de observacion ortogonales alplano suponen una mayor cantidad de puntos que observaciones trasversales.

En lo que respecta al filtro de Kalman (capıtulo 5) podemos contar directamente el numero de puntosque hemos promediado tras el proceso de segmentacion y este es el valor de kj

i . Sin embargo, al imple-mentar el Mutual Information (capıtulo 6) necesitamos saber el numero de puntos de cada plano quese obtendrıan desde cierta posicion hipotetica, algo que en principio desconocemos, pero que podemosestimar a partir de las observaciones anteriores. Para ello almacenaremos el numero de veces que se haobservado cada plano, nvistasi y la estimacion del numero de puntos que se obtendrıan al observar el

2La consideracion de independencia en lo que refiere a las normales obviamente NO es cierta, ya que las ventanas decalculo se solapan para puntos cercanos. De esta forma dos normales cercanas comparten datos al ser calculadas y por lotanto se elimina la independencia. Sin embargo, dado que el tamano de la ventana es pequeno en relacion a la cantidad depuntos obtenidos tras la segmentacion, la aproximacion es adecuada.

18

Figura 15: Puntos capturados en dos planos de igual tamano pero con diferente inclinacion

plano perpendicularmente, pi. Denotaremos por pji al numero de puntos que forman el i-esimo plano en

la j-esima captura del mismo, ~uj al vector de observacion de la j-esima captura y ~ni al vector normal deli-esimo plano.

De esta forma, el numero de puntos aproximado de un plano que obtendremos al realizar la capturasera igual al numero de puntos que obtendrıamos observando perpendicularmente multiplicado por elproducto escalar entre el vector de observacion ~u y la normal del plano ~ni

3.

ki,j = pi (~uj · ~ni)

Para obtener la estimacion del numero de puntos que obtendrıamos al observar un plano perpendic-ularmente, algo que basicamente mide el tamano del plano, realizamos lo siguiente.

pi =nvistasi∑

j=1

pji

1~ni · ~uj

Para ilustrar el funcionamiento de este criterio presentamos en la figura 17 la evolucion de la entropıade los planos detectados en las capturas de la figura 16. Se observa claramente como los planos con mayornumero de puntos (planos 1 y 4) se inicializan con menor entropıa y su decrecimiento es mas rapido. Sitomamos como ejemplo el plano 4, observamos que aun siendo detectado posteriormente a los planos 2 y3, tras 5 iteraciones su entropıa ya es menor que la de estos dos.

3Tengase en cuenta que las posibles posiciones del sensor se encuentran sobre una esfera centrada en el objeto, con locual, la distancia entre plano y sensor podemos considerarla constante

19

Figura 16: Capturas sucesivas segun la modificacion 1

Figura 17: Evolucion de la entropia de cada plano segun la modificacion 1

20

10.2. Modificacion 2:Criterio de adjacencias

Este nuevo sistema, al igual que el anterior, presenta el siguiente inconveniente. Ambos generan unasecuencia de movimientos que conduce hacia posiciones que permiten reducir al maximo la entropıa,pero unicamente pretenden disminuir la entropıa de aquello que ya ha sido observado. Incluso en algunassituaciones ocurre que el sensor tiene una posicion privilegiada de observacion y permanece reiteradamenteen dicha zona, en vez de moverse hacia posiciones donde podrıa encontrar nuevos planos (ver el ejemplode la seccion 10.2.1). En realidad, lo que realmente interesa es, en pocas capturas, obtener la informaciondel objeto completo y no unicamente algunos de sus planos.

Para conseguir esto debemos movernos hacia posiciones desde las cuales puedan observarse planosnuevos. Decidir cuales son estas posiciones no es a priori evidente. En nuestro caso, dado que ademas delconjunto de planos disponemos del mapa topologico del objeto, haremos uso de este ultimo para elegirla direccion de movimiento. Concretamente, favorecemos los desplazamientos que comporten una mejorobservacion de planos con pocas adjacencias. La justificacion de esto, es que por cada plano no visto, elgrado de cualquier plano adjacente conocido sera una unidad inferior a su valor real. Por ello es logicopensar que planos conocidos con pocas adjacencias, son adjacentes a planos desconocidos. Esta situacionqueda reflejada en la figura 18.

Figura 18: Figuras poliedricas con el grado de cada cara

Concretamente se ha seguido el siguiente algoritmo. Se ha calculado el numero mınimo de adjacenciasentre todos los planos vistos. Despues se ha sumado el producto escalar entre el vector de observacion y elvector normal de cada uno de los planos que alcanza el mınimo de adjacencias. Exactamente, se ha sumadoel valor absoluto del producto escalar y solo se han tenido en cuenta los planos cuyo correspondienteproducto escalar es negativo, ya que en caso contrario el plano no es observado4.

Finalmente, se ha reescalado entre 0 y 1 tanto el valor anterior calculado, como la entropıa estimadasegun el mutual information. Se ha tomado como direccion final de desplazamiento la que minimiza lasiguiente expresion:

F · (1− V alorAdjacencias) + (1− F ) · V alorMutualInformation

De esta forma, escogiendo el parametro F en el intervalo [0, 1] podemos controlar la influencia decada uno de los dos valores anteriores. Segun el valor de este parametro el sistema se comportara de lasiguiente forma:

4Recuerdese que los planos estan orientados de forma que la normal tiene sentido hacia el exterior del objeto

21

F ' 0: Sistema dominado por mutual information. Tendencia a establecer con certeza los planosya observados.

F ' 1: Sistema dominado por criterio de adjacencias. Tendencia a descubrir planos no observadospor el momento.

Para ilustrar la influencia del parametro F en el funcionamiento global mostraremos dos ejemplos condistintos valores.

10.2.1. Ejemplo 1: F=0

En este ejemplo el sistema favorece la determinacion de los planos ya observados. De hecho, parte deuna posicion de observacion privilegiada y por ello permanece en el mismo entorno, sin explorar zonas deun elevado grado de desconocimiento. Situaciones como esta son las que se pretenden evitar al introducirel criterio de adjacencias.

Figura 19: Recorrido y sucesion de segmentaciones para el algoritmo original, F=0

22

10.2.2. Ejemplo 2: F=0.8

En este ejemplo se toman condiciones iniciales identicas al caso anterior pero se toma F = 0,8, deforma que el criterio de adjacencias tenga un peso significativo. A diferencia del caso anterior, el sistemaabandona la zona de observacion inicial y continua la exploracion.

En este caso concreto, el numero mınimo de adjacencias es 2 y hay 4 planos que alcanzan este valor(2, 3, 4 y 5). Como los vectores normales de los planos 3 y 5 tienen un angulo muy pequeno en relacional resto de parejas de planos, el criterio de adjacencias favorece la observacion ortogonal de estos planos;lo cual queda reflejado en la sucesion de capturas mostradas en la figura 20.

Figura 20: Recorrido y sucesion de segmentaciones para el algoritmo modificado, F=0.8

23

Referencias

[1] STAUBLI FAVERGES Manual de referencia VAL3, version 3.0 edition, 2003.

[2] SwissRanger SR-3000 Manual, v1.03 edition, october 2006.

[3] J. Andrade-Cetto. The Kalman filter. Technical Report IRI-DT-02-01, Institut de Robotica i In-formatica Industrial, Universitat Politecnica de Catalunya, March 2002.

[4] J. Andrade-Cetto and A. C. Kak. Object recognition. In J. G. Webster, editor, Wiley Encyclopedia ofElectrical and Electronics Engineering, supplement 1, pages 449–470. John Wiley & Sons, New York,2000.

[5] Thomas H. Cormen, Charles E. Leiserson, Ronald L. Rivest, and Clifford Stein. Introduction toAlgorithms, Second Edition. The MIT Press, September 2001.

[6] P. F. Felzenszwalb and D. P. Huttenlocher. Image segmentation using local variation. In CVPR ’98:Proceedings of the IEEE Computer Society Conference on Computer Vision and Pattern Recognition,page 98, Washington, DC, USA, 1998. IEEE Computer Society.

[7] T. Kahlmann and H. Ingensand. Calibration and improvements of the high resolution range-imagingcamera swissranger. In Proceedings of SPIE, Vol. 5665 No. 20, 2005.

[8] T. Vidal-Calleja, A.D. Davison, J. Andrade-Cetto, and D.W. Murray. Active control for single cameraSLAM. pages 1930–1936.

[9] Greg Welch and Gary Bishop. An introduction to the kalman filter. Technical report, Chapel Hill,NC, USA, 1995.