Màster: Tesi de Màster ESTUDIO DEL SEGUIMIENTO VISUAL DE ANATOMÍAS ORIENTADO A LA ROBÓTICA QUIRÚRGICA. Luis Alberto Morales Escobar

Transcripción

1 UNIVERSITAT POLITÈCNICA DE CATALUNYA Màster: AUTOMÀTICA I ROBÒTICA Tesi de Màster ESTUDIO DEL SEGUIMIENTO VISUAL DE ANATOMÍAS ORIENTADO A LA ROBÓTICA QUIRÚRGICA Luis Alberto Morales Escobar Directora: Alicia Casals Gelpí Curs Acadèmic 2012/2013 Noviembre 2012

2

3 Agradecimientos Agradezco ante todo a Dios por darme sus bendiciones y haberme guiado e iluminado durante toda mi vida, ayudándome a cumplir cada una de las metas que me he propuesto. A mis padres y hermanas que desde el inicio me apoyaron y me ayudaron en esta nueva etapa permitiéndome culminarla con éxito. Gracias a todos ellos por nunca dejarme solo ya que siempre a pesar de la distancia me acompañaron aconsejándome, dándome sus bendiciones y sus mejores deseos durante este tiempo. Gracias por estar siempre pendientes de mí y por el ejemplo que me han dado. A mis nuevos amigos, becarios también, que se convirtieron en un gran apoyo y de quienes he aprendido mucho, con quienes pasamos buenos momentos, compartiendo esta nueva experiencia. A mi directora de tesis, Alicia Casals y al profesor Josep Amat, quienes me dieron la oportunidad de trabajar bajo su tutela y me han brindado toda la ayuda necesaria para el desarrollo de este trabajo. Un agradecimiento muy especial a la Secretaría Nacional de Educación Superior, Ciencia, Tecnología e Innovación de la República del Ecuador (SENESCYT), que mediante la beca otorgada, me ha permitido continuar con mi preparación profesional dándome el apoyo y los recursos necesarios para el desarrollo y finalización de mis estudios a nivel de maestría.

4 PRESENTACIÓN El presente trabajo consta de 5 capítulos en los que se detalla información relevante sobre los conceptos y procedimientos realizados de forma clara para facilitar el entendimiento del mismo. El Capítulo 1, contiene la introducción al planteamiento del problema, haciendo un breve análisis de trabajos similares, en el ámbito de la visión por computador aplicada a cirugía realizados anteriormente, permitiendo conocer el estado del arte, así como los objetivos planteados y razones que han llevado a desarrollar este trabajo. El Capítulo 2, está enfocado en los Conceptos Generales empleados y que serán de gran utilidad para resolver el problema planteado, y serán de gran utilidad para el desarrollo de los algoritmos computacionales. En el Capítulo 3, se describe el algoritmo de procesamiento de imágenes desarrollado, así como el envío de datos al robot, explicando detalladamente las funciones utilizadas, para el tratamiento de la secuencia de imágenes adquirida. El Capítulo 4, muestra las diversas pruebas realizadas con la maqueta del corazón disponible en el laboratorio de robótica comprobando el funcionamiento del algoritmo programado, así como los resultados obtenidos por el funcionamiento del sistema constituido por hardware y software, y la discusión de los mismos. Por último, el Capítulo 5, referente a las conclusiones y trabajo futuro, se destaca los aspectos e ideas más importantes adquiridas en la realización de este trabajo, de igual modo, se entrega una visión sobre posibles mejoras que puedan implementarse para obtener resultados más óptimos.

5 Índice de Contenidos RESUMEN... 1 CAPITULO Introducción Objetivo del Trabajo Motivación Trabajos Relacionados Descripción del Trabajo... 6 CAPITULO Conceptos Generales Robótica Quirúrgica Robots Quirúrgicos Robot Pathfinder o Neurocirujano Robot PAKY- RCM o Urólogo Sistema robótico quirúrgico Da Vinci Sistema robótico quirúrgico Zeus Visión por Computador Modelo de la Cámara Ecuación Proyectiva Distorsión de la Lente Distorsión Radial Distorsión Tangencial Visión Estéreo Calibración de Cámaras Estéreo... 22

6 2.2.6 Rectificación de Imágenes Detección de Puntos Relevantes o de Interés Seguimiento de Puntos Relevantes (Tracking) Emparejamiento de puntos homólogos correspondientes a puntos relevantes de la escena Filtro de Kalman CAPÍTULO Desarrollo del Sistema de Visión Adquisición de Imágenes Calibración y Rectificación de Cámaras Detección de Puntos de Interés y Seguimiento (Tracking) Emparejamiento de Puntos Singulares Filtro de Kalman Calculo de la Posición 3D Transformación del sistema de coordenadas Comunicación con el Robot CAPÍTULO Experimentos y Resultados Calibración y rectificación de cámaras Selección y Emparejamiento de Puntos Singulares Calculo de la Posición 3D respecto a la cámara Calculo de la Posición 3D respecto al sistema de coordenadas del Robot Errores de Emparejamiento Interfaz de Usuario Adquisición y Filtrado de Datos... 68

7 4.8 Predicción de Datos CAPITULO CONCLUSIONES Y TRABAJO FUTURO Conclusiones Trabajos Futuros BIBLIOGRAFIA Índice de Figuras Figura 1. Esquema de Funcionamiento del Proceso... 6 Figura 2. Etapas del Proyecto Planteado... 7 Figura 3. Robot Quirúrgico y Consola de Mando Figura 4. Esquema de la Estructura de un Sistema de Teleoperación Figura 5. Robot Pathfinder Figura 6. Robot PAKY RCM Figura 7. Consola y Robot Esclavo del Sistema Quirúrgico Da Vinci Figura 8. Consola y Robot Esclavo del Sistema Quirúrgico Zeus Figura 9. Etapas Fundamentales del Procesamiento Digital de Imágenes Figura 10. Representación del Modelo de la Cámara Figura 11. Modelo de la Cámara Pin-Hole Figura 12. Modelo de Visión Estéreo Tipo Binocular Figura 13. Punto P Observado por los Planos Izquierdo y Derecho Figura 14. Plantilla de la Tabla de Ajedrez Figura 15. Calibración de Cámaras Figura 16. Tipos de Regiones Detectadas Figura 17. Regiones en Función de los Valores Propios de M... 26

8 Figura 18. Supuestos del Algoritmo de Lucas Kanade Figura 19. Cálculo de Flujo Óptico Piramidal Figura 20. Esquema de la Técnica Match Template Figura 21. Ciclo de Trabajo del Filtro de Kalman Figura 22. Diagrama de Flujo General del Procesamiento de Imágenes Figura 23. Logitech Hd Pro Webcam C Figura 24. Vista frontal de la disposición de las cámaras en modo estéreo Figura 25. Esquema del Algoritmo de Calibración, Rectificación y Eliminación de la Distorsión de Imágenes Figura 26. Imágenes de la Plantilla de Ajedrez Utilizadas Figura 27. Imágenes sin Rectificar Figura 28. Imágenes Rectificadas y sin Distorsión Figura 29. Diagrama de flujo de la detección de puntos de interés y seguimiento Figura 30. Puntos Relevantes detectados en la Escena Figura 31. Selección de Puntos de Interese en Pantalla Figura 32. Diagrama de Flujo Etapa de Emparejamiento de Puntos Figura 33. Filtro de Kalman Figura 34. Representación del Plano de Trabajo y la Orientación de la Normal Figura 35. Brazo Robótico ABB ARB Figura 36. Espacio de Trabajo y Dimensiones del Robot ABB ARB Figura 37. Diagrama de Flujo de la Comunicación con el Robot ABB ARB Figura 38. Visualización de la Calibración y Rectificación de Cámaras para Dos Vistas de la Escena Figura 39. Emparejamiento de puntos relevantes de una imagen estática con objetos cercanos (Entorno 1)... 59

9 Figura 40. Emparejamiento de puntos relevantes de una imagen estática con objetos lejanos (Entorno 2) Figura 41.a) y b) Emparejamiento de Puntos Relevantes Seleccionados en Video Figura 42. Disposición de los Elementos en la Escena Figura 43. Puntos Observados en cada uno de los Planos del Espacio 3D Figura 44. Coordenadas de los puntos seleccionados (Entorno 1) Figura 45. Puntos Observados en los plano XZ y YZ (Entorno 2) Figura 46. Coordenadas de los puntos seleccionados (Entorno 2) Figura 47. Efector Final Figura 48. Posiciones del Elemento Terminal Figura 49. Resultados de la Etapa de Emparejamiento Figura 50. Ventana 1 Adquisición y presentación de datos, y descripción de sus elementos en pantalla Figura 51. Ventana 2 Interfaz del Interprete y descripción de los elementos en pantalla. 68 Figura 52. Respuesta del Filtro de Kalman R= 1, Q = en X, Y y Z Figura 53. Respuesta del Filtro de Kalman R= 0.001, Q = en X, Y y Z Figura 54. Respuesta del Filtro de Kalman R= 0.1, Q = 0.1 en X, Y y Z Figura 55. Respuesta del Filtro de Kalman R= 0.1, Q = en X, Y y Z Figura 56. Respuesta del Filtro de Kalman R= 0.1, Q = en X, Y y Z Figura 57. Datos Muestreados para Cálculo del Error Figura 58. Comportamiento del Predictor en el Eje X Figura 59. Comportamiento del Predictor en el Eje Y Figura 60. Comportamiento del Predictor en el Eje Z Figura 61. Sistema de visión con el video de un corazón real latiendo... 78

10 RESUMEN El objetivo del presente trabajo es desarrollar un algoritmo de procesado de imágenes que permita conocer la posición y detectar el movimiento de una determinada superficie perteneciente a una anatomía del cuerpo humano, en este caso el corazón, con la finalidad de permitir a un operador o cirujano teleoperar un robot que sincronice sus movimientos con los movimientos naturales de la anatomía de interés de manera automática. Para este fin, se empleará la técnica de visión estéreo en la que se utilizarán dos cámaras que observan la misma escena y mediante triangulación permiten calcular la posición de la superficie de trabajo en el espacio tridimensional. La detección del movimiento de la misma se llevará a cabo primeramente identificando sus puntos relevantes para después, ser seguidos mediante la técnica conocida como tracking en una secuencia de imágenes tomadas por las cámaras. Sin embargo, el seguimiento de estos puntos relevantes es susceptible al ruido contenido en la secuencia de imágenes, por lo que se implementará el filtro de Kalman para atenuarlo. No obstante, la ejecución del algoritmo propuesto conllevará el retardo de tiempo necesario para realizar todas las operaciones matemáticas y funciones de procesamiento de imágenes, por lo cual es de suma importancia compensar este factor. Para ello se utilizará el mismo filtro de Kalman mencionado, a modo de predictor de un estado futuro. Una vez compensado el retardo de tiempo se obtendrán las coordenadas de la posición de la superficie de trabajo en el espacio, las cuales serán enviadas al robot para sincronizar su movimiento con el movimiento natural de la superficie de trabajo, evitando que el teleoperador tenga que realizar estos movimientos manualmente. Las pruebas del funcionamiento han sido realizadas especialmente en el ámbito de la cirugía cardíaca con la maqueta del corazón y el robot disponibles en el laboratorio de robótica. 1

11 CAPITULO 1 Introducción En la actualidad el uso de plataformas robóticas útiles para realizar cirugías mínimamente invasivas en anatomías del cuerpo humano se encuentra en constante desarrollo. Mediante la teleoperación de estos mecanismos robóticos se busca brindar a los cirujanos facilidad de maniobra sin que esto conlleve la pérdida de su destreza, facilitando la ejecución de acciones sobre la misma. Sin embargo muchas veces estos mecanismos se encuentran limitados por la pérdida de la realimentación táctil y por la presencia de perturbaciones generadas en su mayoría por el movimiento fisiológico natural, lo cual limita su uso en varios casos. Por tanto, se encuentran en desarrollo, métodos que permitan disminuir estos factores. En el ámbito de la cirugía cardíaca es preciso tomar en cuenta las perturbaciones mencionadas anteriormente, que pueden darse por los movimientos generados por los latidos del corazón y por la respiración, buscando sincronizar el manipulador automáticamente con estos movimientos para que el cirujano se enfoque en operaciones tales como la suturación e incisión ya que estas tareas requieren de gran precisión y concentración. Es por esta razón, que se debe aprovechar al máximo la información entregada por las cámaras, tratarla adecuadamente e interpretarla correctamente de modo que sea de gran utilidad durante el proceso o cirugía ya que dichas cámaras cumplen la función de ser los sensores que permiten conocer la posición y orientación de un punto, superficie u objeto de interés dentro del área de trabajo para su posterior manipulación o para la ejecución de una determinada tarea. 2

12 1.1 Objetivo del Trabajo Este proyecto se llevará acabo en el marco del procesado digital de imágenes, cuyo objetivo se basa en la detección de la posición de la superficie de la anatomía de interés, así como su movimiento en base a la identificación de determinados puntos relevantes (característicos) de la misma, permitiendo trabajar sobre el área de interés con el robot teleoperado, otorgándole así, un movimiento automático acorde a los movimientos propios de la superficie de trabajo, evitando que el operador realice estos movimientos en forma manual. Los puntos detectados serán identificados y seguidos (tracking) en las secuencias de imágenes tomadas por las dos cámaras que observan el área de trabajo desde dos posiciones distintas (visión estéreo), sin embargo, el seguimiento de dichos puntos es sensible al ruido presente en las imágenes, para lo cual será necesario implementar un filtro que lo atenúe y a su vez permita compensar el retardo de tiempo producto del procesado de cada una de las imágenes tomadas, específicamente de la anatomía del corazón humano. 1.2 Motivación La motivación de este trabajo surge de la importancia que han adquirido la robótica y la visión por computador; campos de investigación que en numerosas aplicaciones van de la mano. Una de ellas es la teleoperación o telemanipulación de robots aplicados en operaciones quirúrgicas, en los que la estación del operador, o cirujano (maestro), se encuentra en una ubicación, mientras que la estación del robot (esclavo) o del paciente, puede encontrarse a cierta distancia, desde unos metros a kilómetros. Para ello se han diseñado diversos tipos de robots que pueden asistir al cirujano en una operación. Por ejemplo, existen los que se aplican a la cirugía con intervenciones poco invasivas como son el caso de las operaciones laparoscópicas, donde se introducen por 3

13 pequeños orificios tres o más apéndices: una cámara óptica, una luz y un instrumento robotizado que puede llevar desde micropinzas hasta bisturís. Existen también los que se aplican a situaciones en los que se requiere gran fuerza y presión, por ejemplo cuando se requiere cortar huesos o en intervenciones en la cabeza u órganos importantes, sin afectar puntos vitales, recibiendo las instrucciones del cirujano y valiéndose de imágenes que permitan conocer la disposición tridimensional de la anatomía de los órganos del paciente. Es por ello que la visión por computador juega un papel muy importante ya que permite obtener información acerca de la forma, textura, y ubicación de las anotomías del paciente y en el caso de este trabajo, sus movimientos propios y desplazamiento en el espacio de interés ayudando al cirujano a concentrarse en la cirugía, sin tener que tomar en cuenta los movimientos naturales que los órganos puedan tener, ya que esto será visto por las cámaras y compensado automáticamente. Con este fin, se busca desarrollar técnicas y algoritmos implementados a nivel de software que hagan posible localizar la ubicación y desplazamiento de un determinado punto, superficie u objeto visto por una o varias cámaras, ya sea en el espacio bidimensional o tridimensional lo cual a nivel médico es sumamente importante cuando un operador o cirujano teleopere un mecanismo robótico posicionándolo y actuando sobre una zona deseada de manera precisa y segura. 1.3 Trabajos Relacionados Varios han sido los estudios que se han desarrollado en este campo, aplicando diversas técnicas como se ha visto en la cirugía de revascularización mínimamente invasiva [1], en la que se realiza una estabilización visual de una imagen en la que se encuentra una marca artificial que no es más que un punto de interés para el cirujano, y del que se detecta su desplazamiento 2D con la finalidad de sincronizar el robot (esclavo) con los latidos del corazón. Sin embargo, al solamente detectar un desplazamiento bidimensional se pierde información relevante referente a la profundidad del área de trabajo. 4

14 Las mismas marcas mencionadas anteriormente son utilizadas en [2] y se encuentran colocadas en la superficie del corazón, realizándose la predicción de dicho movimiento tomando en consideración el movimiento generado por los latidos y el movimiento generado debido a la respiración del paciente, sin embargo la comunidad médica ha establecido que las marcas colocadas para este tipo de trabajo no son prácticas por los problemas que involucra su fijación y el espacio limitado del que se dispone. Debido a esto en [3] se estudia el movimiento de características naturales que se encuentran en la superficie del corazón, analizando y estableciendo un modelo matemático con la finalidad de predecir el comportamiento de dicha superficie en un instante de tiempo. Sin embargo este procedimiento nuevamente se limita a un análisis del desplazamiento de estas marcas en dos dimensiones. Por tal razón, con la finalidad de obtener la profundidad de una región de interés de la superficie cardíaca en análisis, y por ende la reconstrucción de la misma, [4] propone el uso de los mapas de disparidad obtenidos de dos imágenes tomadas a modo de visión estéreo, con la finalidad de encontrar la relación entre puntos homólogos en cada cámara y así determinar su ubicación en el espacio 3D. Del mismo modo en [5], se propone una solución de la detección de desplazamiento de puntos de la superficie del corazón mediante visión de cámaras estéreo utilizando el método de flujo óptico que busca donde se ha movido un punto relevante en una secuencia de imágenes, siendo un método computacionalmente eficiente que a su vez requiere de condiciones de luz óptimas para evitar la pérdida de los puntos requeridos. Una alternativa al método anterior, es el seguimiento de regiones o puntos en tres dimensiones que se establece en [6], basado en una malla digital deformable con compensación de iluminación que disminuye los efectos de los cambios de luz arbitrarios. Este método de igual manera como en [4] y [5] se basa en la detección del desplazamiento 3D de puntos pertenecientes a la superficie del corazón, mediante visión estéreo. El método propuesto en este proyecto se basa en identificar y detectar automáticamente puntos relevantes o de interés de la superficie cardíaca determinada, de los cuales serán escogidos por el operador tres de ellos permitiendo determinar la orientación del plano 5

15 formado, así como la orientación y su punto medio, con la finalidad de poder sincronizar el movimiento del robot con el movimiento propio de la superficie de trabajo. Sin embargo el seguimiento o detección de movimiento de dichos puntos relevantes es susceptible a la presencia de ruido en la secuencia de imágenes tomadas por las cámaras, es por ello que [7] y [8] proponen la implementación del filtro de Kalman para tracking de objetos, el cual es un estimador predictor que atenúa el ruido y a la vez permite estimar donde se encontrarán los puntos de interés en la secuencia imágenes tomadas por las cámaras estéreo, así como una predicción de su ubicación en la siguiente imagen, compensando así el tiempo de cómputo empleado en la ejecución del algoritmo programado. 1.4 Descripción del Trabajo Como se ha mencionado anteriormente, el objetivo del presente trabajo es sincronizar los movimientos del robot con los de la anatomía de interés, en este caso el corazón, para ello es necesario procesar en tiempo real la secuencia de imágenes de la escena obtenidas mediante cámaras. El esquema de este proceso se muestra en la Figura 1. Robot Teleoperado Cámaras Estéreo Envío de Órdenes Adquisición de Imágenes Normal al plano Puntos Singulares Sincronización de movimiento Operador Corazón latiendo Procesamiento de Imágenes Figura 1. Esquema de Funcionamiento del Proceso 6

16 En este esquema se observa que un operador puede teleoperar un brazo robótico sobre una superficie de interés (corazón) que se encuentra en movimiento. Lo que se busca con este trabajo es sincronizar los movimientos del robot con los de dicha superficie de manera automática, esto a partir de las imágenes adquiridas por dos cámaras de video colocadas a modo de visión estéreo con la finalidad de detectar la posición en el espacio 3D. En este caso el computador se encargará de ejecutar los algoritmos de visión que permiten hacer el seguimiento de los movimientos de la superficie del corazón mediante la detección de sus puntos singulares, y filtrado de datos obtenidos para luego ser enviados al robot. Las etapas de las que se compone el sistema desarrollado se muestran en la Figura 2. Calibración y Rectificación de Cámaras Adquisición de Imágenes Detección de Puntos Singulares Tracking de Puntos Obtención del 3D (Posición y orientación) Filtrado y Predicción Señales (Filtro de Kalman) Emparejamiento de Puntos Homólogos Selección de Puntos Envío de datos de movimiento al Robot Sincronización de Movimientos Figura 2. Etapas del Proyecto Planteado A continuación se describe brevemente la función que cumple cada una de las etapas mostradas en la figura anterior. Calibración y Rectificación de Cámaras.- esta etapa es requerida debido a que los fabricantes entregan información poco exacta de las cámaras por lo que es necesario calcular sus parámetros internos tales como distancia focal y punto principal, datos que permiten hacer un cálculo de la posición del objeto con mejor precisión. 7

17 Adquisición de Imágenes.- realizada mediante un par de cámaras de video de similares características, colocadas a modo de visión estéreo que permitirán triangular y determinar la posición de la superficie de interés en 3D. Con estos parámetros se procede a la detección y seguimiento de puntos relevantes. Esta etapa es importante ya que los puntos relevantes son aquellos que poseen gran información dentro de una imagen y son fácilmente detectables en una secuencia, razón por la que en esta aplicación deberán escogerse 3 de ellos los cuales deben pertenecer a la superficie a la que se quiere sincronizar el movimiento del robot y de los cuales se debe hacer un seguimiento en tiempo real. En base a estos tres puntos se podrá conocer la posición del punto medio del plano y la orientación de la normal al mismo, para que el efector final del robot se oriente de manera opuesta. Emparejamiento de puntos relevantes.- esta etapa es requerida para el cálculo de la posición 3D de los puntos a ser seguidos, y su función es encontrar de la imagen de la cámara izquierda, sus homólogos en la cámara derecha para triangularlos y resolver el estéreo. Filtrado de datos.- la finalidad de filtrar los datos se da ya que las imágenes con objetos en movimiento son sensibles al ruido, por lo que se busca atenuarlo para poder realizar cálculos más precisos independientes de ese tipo de perturbaciones, además se ha desarrollado un visualizador que permite observar los datos en tiempo real, permitiendo conocer el comportamiento del sistema. A partir de esta etapa ya se puede calcular la posición en 3D del plano y su orientación. Envío de datos al robot.- una vez que se ha realizado todo el proceso de tratamiento de imágenes se envía la información de la posición y orientación de la superficie de interés detectada para que el robot sincronice su movimiento de manera continua. 8

18 CAPITULO 2 Conceptos Generales En esta sección se presentan los conceptos generales de los temas relacionados con el desarrollo de este trabajo, enfocándose en la robótica quirúrgica y algunos de los robots utilizados para este fin, y a continuación se estudia la teoría relacionada con la visión por computador necesaria para el desarrollo del algoritmo propuesto para cumplir con los objetivos planteados. 2.1 Robótica Quirúrgica Desde los primeros sistemas de cirugía asistida por robot, se ha ido progresando y en algunas especialidades quirúrgicas se ha demostrado la utilidad de la aplicación de la robótica dando lugar a la cirugía robotizada. Para ello, los robots pueden ser autónomos o teleoperados, siendo los primeros aquellos que necesitan de un programa diseñado para realizar ciertas actividades de forma automática, en cambio los robots teleoperados, operan en una estructura del tipo maestro-esclavo, en que el esclavo es guiado por las órdenes del cirujano, es decir, que es absolutamente dependiente del juicio, conocimiento, habilidad y la destreza del médico, pero que puede aportar asistencia para operar de forma mas eficiente. Estos robots constan de una estructura en general del tipo brazo articulado, capaz de imitar los movimientos de diversas articulaciones como las del hombro muñeca y manos, y son teleoperados a través de una consola de mando, la cual puede situarse en el mismo quirófano, o eventualmente en otro lugar, incluso a gran distancia. La Figura 3, muestra un ejemplo de robot quirúrgico teleoperado, mientras que la Figura 4, muestra el esquema de la estructura de un sistema de teleoperación. 9

19 Figura 3. Robot Quirúrgico y Consola de Mando Interfaces de Entrada Órdenes ROBOT Actuación Operador Bucle interno del operado r Interfaces de Salida Estado Sistema Sensorial Bucle interno del robot Entorno de trabajo Percepción Consola de Mando Comunicación a corta o gran distancia Estación del Robot Figura 4. Esquema de la Estructura de un Sistema de Teleoperación La teleoperación de robots en cirugía puede permitirle a un cirujano llevar a cabo procedimientos menos invasivos que en la cirugía tradicional, ya que el robot reduce los movimientos del cirujano (por ejemplo, moviendo media pulgada por cada pulgada que el cirujano mueve), lo cual disminuye algunos de los temblores y movimientos de la mano que, de lo contrario, podrían hacer la cirugía menos precisa. También permite compensar movimientos propios de los órganos del paciente como los producidos por los latidos del corazón, o la respiración, que pueden convertirse en una perturbación que afecte el área a 10

20 operar. Igualmente, los instrumentos robóticos pueden llegar a áreas del cuerpo de difícil acceso más fácilmente a través de incisiones quirúrgicas más pequeñas en comparación con la cirugía abierta común y la cirugía laparoscópica Robots Quirúrgicos La utilización de robots aplicados en el ámbito quirúrgico inicia a mediados de los años 80, con el uso del robot PUMA 560, un robot industrial utilizado para incrementar la precisión en aplicaciones como las biopsias cerebrales mediante punción [9]. A partir de esta aplicación, se han desarrollado diversos sistemas quirúrgicos que han permitido tener el día de hoy mecanismos robóticos dedicados a cirugía, algunos de los cuales por su importancia se describen a continuación: Robot Pathfinder o Neurocirujano Es un robot que permite ayudar al médico en cirugías donde no puede llegar la mano humana debido a su tamaño. La programación del brazo robótico Pathfinder se realiza a partir de una tomografía (un método para obtener imágenes radiográficas 3D) de la cabeza del paciente. Este robot es utilizado para casos como son: operaciones contra el cáncer cerebral, el síndrome de Parkinson, extracción de tumores y cirugía contra epilepsia. Figura 5. Robot Pathfinder 11

21 Robot PAKY- RCM o Urólogo Este robot fue utilizado por primera vez en el año 1998 para realizar el primer acceso renal percutáneo robotizado por teleoperación entre el Johns Hopkins Hospital de USA, y el Hospital Tor Vergara de la Universidad de Roma, Italia, obteniendo resultados satisfactorios, al ser un sistema teleoperado a gran distancia [10], [11]. Figura 6. Robot PAKY RCM Sistema robótico quirúrgico Da Vinci El robot quirúrgico Da Vinci es una plataforma robótica del tipo maestro-esclavo, diseñada para hacer más precisa y eficiente la cirugía laparoscópica que es una cirugía de acceso mínimamente invasivo, la cual se realiza mediante pequeñas incisiones. Este sistema quirúrgico proporciona al cirujano un control intuitivo, una amplia gama de movimientos, capacidad de manipulación de tejidos finos y visualización en 3D, característico de la cirugía abierta o convencional permitiéndole realizar el procedimiento con eficiencia, precisión, seguridad, mejor ergonomía y por ende menor estrés. Este sistema consta de una consola maestra que se encuentra ubicada a distancia de la mesa de operaciones, donde el cirujano ejecuta los movimientos que habrá de realizar el robot. Dado que este sistema es un robot tipo maestro-esclavo, el cirujano es el maestro y controla todas las acciones del esclavo que contiene los brazos robóticos. La consola sirve de interface o comunicación entre el cirujano y el robot (Figura 7). 12

22 El robot esclavo está constituido por cuatro brazos, uno de los cuales contiene el manipulador para la cámara y los otros llevan los manipuladores de instrumentos que reproducen los movimientos de las manos del cirujano realizados desde la consola de mando. Figura 7. Consola y Robot Esclavo del Sistema Quirúrgico Da Vinci Sistema robótico quirúrgico Zeus El sistema robótico Zeus esta constituido de 3 brazos robóticos colocados en la mesa de operaciones, un sistema de control computarizado y una consola ergonómica para el cirujano. Uno de los brazos robóticos se utiliza para posicionar el endoscopio que proporciona la visualización del campo operatorio, mientras que los otros dos manipulan los instrumentos quirúrgicos controlados por del cirujano. Son varias las diferencias del sistema quirúrgico Zeus y el Da Vinci, por ejemplo, en este último, el cirujano mira hacia un visor binocular que se caracteriza por poseer dos imágenes, que al integrarse conforman una señal de vídeo estereoscópica, permitiendo obtener una visión tridimensional de alta definición, que da al cirujano la sensación de inmersión en el campo quirúrgico, mientras en el sistema Zeus, el cirujano se encuentra frente a una pantalla que muestra una imagen en 2D, pero puede proporcionar una imagen en 3D no con el detalle que lo hace Da Vinci. A diferencia del sistema Da Vinci, en el sistema Zeus el brazo que soporta la cámara posee un mecanismo de control accionado 13

23 mediante la voz del cirujano. Finalmente, el sistema Zeus utiliza instrumental endoscópico convencional, mientras que el sistema Da Vinci utiliza instrumental articulado especial que permite transmitir y traducir los movimientos de las muñecas, las manos y los dedos del cirujano, lo que le proporciona una destreza mayor [9]. Figura 8. Consola y Robot Esclavo del Sistema Quirúrgico Zeus Los sistemas quirúrgicos Zeus y Da Vinci tienen gran cantidad de aplicaciones, ya sean para operaciones torácicas, como lobectomías pulmonares, cardíacas, operaciones laparoscópicas, etc. 2.2 Visión por Computador La visión por computador es un campo que incluye métodos para adquirir, procesar, analizar y entender imágenes tomadas del mundo real e interpretarlas en forma de algún tipo de datos, por ejemplo numéricos. Este campo busca desarrollar algoritmos de percepción para interpretar la escena según requiera la aplicación. Las aplicaciones de esta rama de la inteligencia artificial son varias, entre ellas están: La detección, segmentación, localización y reconocimiento de ciertos objetos en imágenes. 14

24 El registro de diferentes imágenes de una misma escena u objeto, es decir, hacer concordar un mismo objeto en diversas imágenes. El seguimiento de un objeto en una secuencia de imágenes. El mapeo de una escena para generar un modelo tridimensional de la misma; modelo el cual podría ser usado por un robot para navegar por la escena, entre otras. El tratamiento digital de imágenes comprende un amplio rango hardware, software y recursos teóricos, cuyas etapas se muestran en la Figura 9 [12]. Preprocesado Segmentación Representación y Descripción Escena Adquisición de Imagenes Modelos Reconocimiento e Interpretación Resultado Figura 9. Etapas Fundamentales del Procesamiento Digital de Imágenes La primera etapa del proceso es la adquisición de la imagen. Para ello se necesita un sensor de imágenes (cámara) y la posibilidad de digitalizar la señal producida por el sensor. Una vez obtenida la imagen digital se da paso al preprocesado de la misma. La función básica del preprocesamiento es de mejorar la imagen de forma que se aumenten las posibilidades de éxito en los procesos siguientes. Dentro de esta etapa se encuentran técnicas de eliminación de ruido y de aislamiento de regiones. La siguiente etapa es la de segmentación, la cual consiste en dividir una imagen de entrada en sus partes constituyentes u objetos. Una buena etapa de segmentación permitirá garantizar una solución satisfactoria al problema del procesamiento de imágenes. A la salida del proceso de segmentación se tienen los datos de los pixeles de interés de una región determinada. La representación se basa en la especificación de un método para describir los datos de forma que resalten los rasgos de interés. 15

25 La descripción consiste en extraer rasgos con alguna información cuantitativa de interés o que sean fundamentales para diferenciar una clase de objetos de otra. La etapa de reconocimiento es el proceso que asigna una etiqueta a un objeto basándose en la información que se tiene en base a un modelo o base de datos con los cuales pueda ser comparada. A medida que se disponga de una mayor cantidad de datos o el modelo sea más exacto, se podrá obtener un reconocimiento más fiable y robusto. La interpretación implica asignar significado a un conjunto de objetos reconocidos Modelo de la Cámara Fundamentalmente el proceso usado para producir imágenes no ha cambiado desde el comienzo de la fotografía. La luz de una escena observada es capturada por una cámara a través de una apertura frontal, mientras que el rayo de luz se proyecta en el plano de la imagen localizado la parte posterior de la cámara. Adicionalmente una lente es usada con la finalidad de concentrar los rayos provenientes desde los diferentes elementos de la escena. Este proceso se muestra en la Figura 10 [13]. f Z Objeto X x Plano de la imagen Lente Figura 10. Representación del Modelo de la Cámara 16

26 Como se observa en la grafica anterior, Z es la distancia entre el lente y el objeto observado, f es la distancia focal de la lente, X es la longitud del objeto y x es la ubicación del objeto en el plano de la imagen. Estas longitudes están relacionadas de la forma: El modelo anterior se puede simplificar asumiendo que todos los rayos provenientes de la escena llegan a un solo punto y que el plano de la imagen es trasladado una longitud igual a la distancia focal a lo largo del eje Z pudiendo observarse la nueva geometría del sistema en la que la imagen en el plano no es invertida. Este modelo simplificado es conocido como PIN-HOLE y se muestra en la Figura 11. x Q X y (x,y) (X, Y, Z) o f (c x,c y ) Z Y Plano de la imagen Figura 11. Modelo de la Cámara Pin-Hole Entonces, se puede fácilmente deducir la ecuación proyectiva: Considerando la línea (que va desde el punto focal o ) que es ortogonal al plano de la imagen, es necesario conocer en que pixel, esta línea lo perfora. Este punto es llamado punto principal. 17

27 Debido a que usualmente el punto principal no esta en el centro del plano de la imagen se introduce los parámetros y. El resultado es relativamente un modelo simple en el que un punto perteneciente a un espacio 3D y de coordenadas (X, Y, Z) proyectado en el plano de la imagen esta dado por: ( ) ( ) Ecuación Proyectiva Las ecuaciones mostradas anteriormente pueden ser escritas en forma de matriz a través de la introducción de coordenadas homogéneas en la que los puntos 2D son representados por un vector de tres elementos y los puntos 3D son representados por un vector de cuatro elementos. Entonces se establece la ecuación proyectiva: [ ] [ ] [ ] [ ] s es el factor de escalado entre la imagen real y la imagen en el plano, la primera matriz posee los valores de la cámara como distancia focal y coordenadas del punto principal conocidos como Parámetros Intrínsecos. La segunda matriz es una matriz de proyección que posee los valores conocidos como Parámetros Extrínsecos y que está compuesta de una matriz (3x3) de rotación y una matriz de traslación (3x1), que siempre y cuando el sistema de referencia del mundo sea igual al de la cámara, tendrá la forma presentada en la ecuación anterior, caso contrario la ecuación proyectiva será [13]: [ ] [ ] [ ] [ ] Distorsión de la Lente Idealmente se puede considerar que la lente de una cámara no introduce distorsiones en una imagen tomada, sin embargo, en la práctica y debido a su construcción, ninguna lente es 18

28 perfecta. Por lo tanto se han definido dos tipos de distorsiones que deben ser tomadas en cuenta al trabajar en el ámbito de la visión por computador, estas son: la radial y la tangencial Distorsión Radial Por lo general los lentes de las cámaras distorsionan la localización de los pixeles que se encuentran próximos a los bordes de la imagen. Este es un fenómeno conocido como efecto de Barril u Ojo de Pez, y se presenta en mayor medida en cámaras de bajo costo. La distorsión radial es 0 en el centro óptico de la imagen e incrementa a medida que se acerca hacia los bordes o la periferia de la imagen. Esta distorsión es pequeña y puede ser anulada a través de series Taylor alrededor de r = 0. Las coordenadas corregidas de los pixeles se dan por las expresiones: Distorsión Tangencial Esta distorsión se presenta debido a la construcción de los lentes, ya que la lente no se encuentra colocada de forma perfectamente paralela al plano de la imagen. La corrección para la distorsión tangencial se puede lograr a través de los coeficientes y, de la forma: ( ) En total son cinco coeficientes los que permiten eliminar la distorsión radial y tangencial generada por el lente de una cámara los cuales son almacenados en forma de un vector de longitud (5x1) [14]. 19

29 2.2.4 Visión Estéreo Esta disposición consta de dos cámaras idénticas separadas a lo largo del eje x una distancia b. Los planos de la imagen son coplanares en este modelo, por lo que las características de la escena son tomadas por las dos cámaras colocadas en diferente posición, Figura 12. Plano epipolar Objeto Ejes ópticos Líneas epipolares Plano Izquierdo Plano Derecho Punto focal izquierdo Línea Base Punto focal derecho Figura 12. Modelo de Visión Estéreo Tipo Binocular El desplazamiento entre las localizaciones de las características de la escena en las imágenes tomadas por las cámaras se conoce como disparidad, mientras que el plano que pasa a través de los puntos focales y el punto característico visto se conoce como plano epipolar La intersección del plano epipolar con el plano de la imagen se conocer como línea epipolar. Para el modelo mostrado en la figura cada punto característico en una imagen, se encontrará en la misma fila de la segunda imagen, aunque en la práctica esto no sea del todo ideal. 20

30 Eje óptico izquierdo Eje óptico derecho P (x,y,z) z Plano izquierdo L x l P l x r R P r Plano derecho f C l b C r Figura 13. Punto P Observado por los Planos Izquierdo y Derecho Observando la Figura 13, tomando en cuenta que el origen de coordenadas se encuentra en el punto focal izquierdo C l, y analizando las semejanzas entre triángulos se obtiene las ecuaciones: De donde se puede deducir: Mediante esta expresión se puede observar que es posible calcular la profundidad a la que se encuentra un objeto respecto de las cámaras, siempre y cuando se conozca la ubicación y del objeto en cada uno de los planos de las imágenes. También se puede observar que la disparidad es inversamente proporcional a la profundidad del objeto, por lo que a disparidades que tienden a cero, se puede entender que hay una gran profundidad, mientras que a disparidades amplias dicha profundidad será pequeña. Por lo tanto se puede concluir que la visión estéreo es útil y de gran precisión para objetos que se encuentran relativamente cercanos a las cámaras [17]. 21

31 2.2.5 Calibración de Cámaras Estéreo La calibración de cámaras es el proceso por el cual, los diferentes parámetros de la cámaras son obtenidos, ya que los entregados por los fabricantes no son lo suficientemente precisos para aplicaciones como por ejemplo la reconstrucción 3D. Para esto es necesario mostrar a una cámara fija, un conjunto de puntos de una escena, de los cuales su posición 3D es conocida, o también es posible tomar varias imágenes de un conjunto de puntos desde diferentes ubicaciones de la cámara [15]. La ventaja de este último, es que se pueden obtener los parámetros de las cámaras sin necesidad de conocer la posición de los puntos de la escena. El proceso de calibración se realiza en tres etapas, primero se transforma el sistema de coordenadas del mundo en el de la cámara (matriz extrínsecos), luego se realiza una corrección de la distorsión, para después obtener las coordenadas 2D de la imagen (matriz intrínsecos). Mediante el modelo se obtiene una estimación de la escena que se analiza, y se realiza mediante la matriz de proyección P, que, como se mencionó anteriormente, engloba los parámetros intrínsecos y extrínsecos. [ ] [ ] [ ] Para realizar este proceso se requiere utilizar lo que se conoce como la plantilla de la tabla de ajedrez (Figura 14), que consta de cuadrados de colores blanco y negro, cuyos vértices son fácilmente identificables. Entonces se puede asumir que la tabla esta en la posición Z=0 y que los ejes X y Y están alineados con la cuadricula. Figura 14. Plantilla de la Tabla de Ajedrez 22

32 Ésta plantilla, que debe tener un mínimo de 6 puntos, permite localizar los puntos y resolver las ecuaciones mediante las cuales se calculan los parámetros de la cámara. Ya que se ha asumido Z= 0, entonces: { { El sistema de ecuaciones presentado consta de 12 incógnitas. Por esta razón son necesarios 6 puntos como mínimo en la plantilla de calibración para cada cámara. De no ser así, no se podría resolver el sistema de ecuaciones, y por ende, los parámetros de calibración. De esta forma se consigue una aproximación inicial para cada captura de la plantilla de calibración que luego se refina mediante un método iterativo que minimiza el criterio de máxima probabilidad. Es decir, se trata de minimizar el sumatorio de la diferencia de las coordenadas de cada punto de las imágenes reales respecto a su proyección realizada teniendo en cuenta todos los parámetros de la calibración (extrínsecos, intrínsecos y distorsión). A partir de la matriz de proyección y los puntos encontrados se hace el cálculo de los parámetros intrínsecos y extrínsecos a partir del modelo Pin-Hole antes visto Rectificación de Imágenes La rectificación de imágenes, consiste en transformar las imágenes captadas por las cámaras estéreo, de modo que las líneas epipolares de dichas imágenes queden alineadas horizontalmente Para realizar esta transformación son necesarios los parámetros intrínsecos y extrínsecos de las cámaras a utilizar. De este modo se busca conseguir que un 23

33 punto P (visto en las imágenes izquierda y derecha, y respectivamente) tenga la misma posición en el eje Y en cada imagen y así poder determinar su profundidad mediante las formulas establecidas en la visión estéreo. (Figura 15). P(X,Y,Z) Plano Izquierdo p 1 Línea Epipolar p 2 Plano Derecho z z C i x Línea Base C d x y i y d Figura 15. Calibración de Cámaras Detección de Puntos Relevantes o de Interés En muchas aplicaciones así como en el presente trabajo, los puntos relevantes serán los que permitan la identificación o detección de movimiento de un objeto ya que estos poseen suficiente información para ser localizados en imágenes consecutivas, además de ser muy poco susceptibles a cambios de rotación y de escala. En esta etapa existen varios métodos que permiten identificar puntos relevantes en una escena como el detector de esquinas de Harris, SIFT y SURF entre otros. Sin embargo, en este proyecto se ha escogido trabajar con puntos esquina por la rapidez y facilidad de su localización respecto de SIFT y SURF que tienen un alto coste computacional en relación al detector de Harris, y no serán de utilidad en esta aplicación ya que se requiere trabajar con una secuencia de imágenes en tiempo real. Los puntos esquina formados dentro de una imagen ya sea por la estructura, forma o textura de los objetos o por la disposición de estos en la escena son puntos relevantes que poseen suficiente información para ser encontrados en una secuencia de imágenes, a su vez 24

34 son sencillos de detectar teniendo un coste computacional bajo. Por tal razón en este trabajo, para detectar los puntos de interés en la secuencia de imágenes obtenida, se ha empleado lo que se conoce como el detector de esquinas de Harris [18]. Una esquina se caracteriza por ser una región de la imagen con cambios de intensidad en diferentes direcciones. Éste será el principio básico de búsqueda de puntos de Harris. Filtrando la imagen con una ventana móvil, se obtienen tres tipos de región: Plana: Sin cambios de dirección. Borde: No hay cambios en la dirección del borde. Esquina: Presenta cambios significativos en todas direcciones. Figura 16. Tipos de Regiones Detectadas El detector de Harris, basa su funcionamiento en la obtención de la matriz de correlación M de la imagen en análisis, que se obtiene al calcular la segunda derivada de una imagen con una ventana más pequeña alrededor de cada pixel. La matriz de correlación se define como: Donde: [ ] Rescribiendo la matriz M de forma simplificada se tiene: 25

35 [ ] Los valores propios de la matriz M se calculan mediante: Donde: I: Matriz Identidad (2x2) De aquí se obtendrán dos valores propios λ 1 y λ 2 pertenecientes a la matriz M. A partir de estos valores se deducen los tres tipos de regiones antes descritas. 1. Si λ 1 0 y λ 2 0, entonces el pixel (x, y) no será relevante (plana). 2. Si λ 1 0 y λ 2 tiene un valor grande positivo, entonces existe un borde. 3. Si λ 1 y λ 2 tienen valores grandes positivos, entonces existe una esquina. λ 2 λ 1 0 λ 2 >> 0 Esquina λ 1 >> 0 λ 2 >> 0 λ 1 0 λ 2 0 λ 1 >> 0 λ 2 0 λ 1 Figura 17. Regiones en Función de los Valores Propios de M. Los valores de λ 1 y λ 2 reflejan los modos de variación de las direcciones principales de los gradientes. Por lo tanto, cuando en una región de la imagen estos valores son elevados, se deduce que existen dos direcciones diferentes de los gradientes, determinando así una esquina. El uso de los valores propios de la matriz de autocorrelación tiene la ventaja adicional de que estos son cantidades que son también invariantes a la rotación, lo cual es importante ya que al realizar el seguimiento de estos puntos, estos pueden rotar a medida que se desplazan. 26

36 La definición original de la técnica de Harris permite obtener muy buenos resultados cuando se trata de identificar puntos relevantes, sin embargo en [19] se establece que los mejores puntos útiles para el tracking son aquellos en los que el mínimo valor propio es suficientemente mayor que un umbral predefinido λ. Esto es: El valor de λ no debe ser mayor a 1, y por lo general en la práctica se establece entre 0.1 y 0.01 [9] Seguimiento de Puntos Relevantes (Tracking) El seguimiento de puntos consiste en localizarlos constantemente en una secuencia de imágenes tomadas por la cámara. Para cumplir este fin se han desarrollado varios métodos, uno de ellos es el conocido como flujo óptico, para el cual existen varios tipos de algoritmos que permiten hacer el seguimiento de objetos, uno de los cuales es propuesto por Lucas Kanade [20], que se ha escogido en este trabajo debido a los buenos resultados que entrega y porque su tiempo computacional es bajo. El algoritmo de Lucas Kanade basa su principio de funcionamiento en tres supuestos: 1. Brillo Constante. Un pixel de la imagen de un objeto en la escena no cambia en apariencia se mueva o no, entre imágenes sucesivas. Para imágenes a escala de gris esto significa que se asume que el brillo de un pixel no cambia al desplazarse. 2. Persistencia Temporal. El movimiento de una pequeña región de puntos cambia lentamente en el tiempo. Esto significa que los incrementos temporales son lo suficientemente rápidos para que el desplazamiento de la región de puntos entre imágenes sucesivas sea pequeño. 3. Coherencia Espacial. Los puntos vecinos de una superficie pertenecientes a una escena, tienen movimiento similar y se proyectan como puntos cercanos en el plano de la imagen. 27

37 Brillo Constante Persistencia Temporal I(x+u,y+v,t+1) = I(x,y,t) Coherencia Espacial Superficie Plano de la Imagen Figura 18. Supuestos del Algoritmo de Lucas Kanade Con estas condiciones, el método propone para calcular el flujo óptico, suponer que el entorno del punto en el que se está realizando el cálculo tiene el mismo movimiento; es decir, que el entorno del punto se mantiene constante. El flujo se calcula, en este caso, mediante correspondencias. Se crea para ello una ventana alrededor del pixel que define su entorno y se intentará encontrar su ventana correspondiente en la imagen siguiente. La siguiente ecuación realiza una minimización de las diferencias de cada pıxel con su supuesto homólogo en la imagen consecutiva. La ventana de correspondencia que minimice la función será la que defina el destino del flujo óptico hacia donde se ha movido el punto original en la imagen siguiente. [ ] Donde: 28

38 Esta ecuación calcula la diferencia de la ventana original del pixel, con todas las posibles ventanas de la imagen siguiente en un entorno acotado del punto. La correspondencia será con la ventana con la que haya habido una diferencia menor. Aquí aparece el problema de la apertura, ya que los puntos de borde pueden tener varias correspondencias. En este caso el flujo óptico apuntará hacia el pixel que esté centrado en la ventana que minimice las diferencias, pero, para las ventanas que solo incluyan bordes, las diferencias serán muy pequeñas y no se podrá asegurar que la dirección del movimiento sea la que se está indicando. Por este motivo conviene calcular el flujo óptico solo en puntos de interés como por ejemplo esquinas. La desventaja de usar ventanas alrededor del punto de interés es que grandes movimientos pueden mover el punto fuera de ella haciendo imposible que el algoritmo lo encuentre. Para ello se ha desarrollado el algoritmo de Lucas Kanade Piramidal, cual comienza en la imagen con menor resolución (con menos pixeles). El resultado de calcular el flujo óptico se traslada a la siguiente capa, en la que se procederá al cálculo del flujo óptico centrando la ventana de búsqueda en el resultado de la capa inmediatamente superior. Al realizarse los primeros cálculos en una imagen con menos resolución, la ventana cubre más imagen, pero el cálculo del flujo óptico resulta menos preciso. A medida que se va descendiendo a capas con mayor resolución, el cálculo se vuelve más preciso. El ahorro de cómputo se produce al reducir la ventana de búsqueda, porque en las capas con menor resolución se dan los indicios de movimientos grandes, y luego se van afinando en las capas con mayor resolución [19]. 29

39 Instante t Ventana de Pixel Instante t + 1 Ventana de Pixel Figura 19. Cálculo de Flujo Óptico Piramidal Emparejamiento de puntos homólogos correspondientes a puntos relevantes de la escena Una vez que se ha determinado los puntos de interés de los cuales se realizará el seguimiento en una de las imágenes, en este caso para la imagen izquierda, es necesario encontrar su correspondencia en la imagen derecha para poder realizar la reconstrucción 3D. El emparejamiento de puntos en este trabajo se ha elaborado bajo la técnica de Match Template el cual se realiza extrayendo una ventana, bloque o plantilla de píxel alrededor del punto relevante detectado en la imagen izquierda y deslizándolo en la fila correspondiente de la imagen derecha, encontrando la mejor correspondencia. Debido a que en esta etapa las imágenes ya se encuentran rectificadas, se conoce que la fila de la imagen derecha en la que se encontrará ubicada su correspondencia debe ser la misma fila en la que se encuentra el punto relevante en la imagen izquierda (líneas epipolares) Figura 20. La localización de la correspondencia puede ser encontrada si el bloque que contiene el punto relevante tiene la suficiente textura y no se encuentra oculto de la visibilidad de la cámara derecha. 30

40 Objeto x x y (x i, y i ) y (x d, y d ) (x i, y i ) I Línea Epipolar d = x i - x d D Plano de Imagen Izquierdo Plano de Imagen Derecho Función Matching Template Figura 20. Esquema de la Técnica Match Template Como se puede ver en la figura anterior, el objeto o punto de interés es visto por las cámaras en la misma línea, sin embargo su ubicación se encontrará en una columna diferente, lo que se ha denominado anteriormente como disparidad. También se puede observar que en la zona donde se encuentra la correspondencia se tiene una mayor similitud con respecto a las otras zonas pertenecientes a la misma fila. Para determinar esta similitud el algoritmo puede emplear varios métodos que se describen a continuación, en los que I será la imagen de la cámara derecha, T será la ventana cuyo centro es el punto de interés que se quiere emparejar y R es una matriz de resultados que entrega el algoritmo. Método de la Diferencia al Cuadrado.- encuentra diferencia elevada al cuadrado entre la imagen y la plantilla, produciendo que, en caso de correspondencia el resultado sea cero 0 y caso contrario tenga un valor elevado. [ ] Método de Correspondencia por Correlación.- este método encuentra las correspondencias realizando una serie de multiplicaciones entre los pixeles de la plantilla y de la imagen donde se busca encontrarla, por lo tanto, una buena 31

41 correspondencia tendrá un valor elevado, mientras que una errónea tendrá un valor bajo o cercano a cero. [ ] Para cada uno de los métodos mencionados existen versiones normalizadas que permiten reducir los efectos de las diferencias de iluminación entre la plantilla y la imagen, para ello el resultado de los métodos descritos debe ser dividido para el coeficiente de normalización [22]: Donde: T: Plantilla que contiene el punto a ser buscado I: Imagen donde se buscará el punto de interés. En la realización de este trabajo se han probado los diferentes métodos antes descritos, sin embargo el que mejores resultados ha entregado para cumplir los objetivos es el de correspondencia por correlación Filtro de Kalman En el ámbito de la visión por computador, varias son las aplicaciones como por ejemplo el seguimiento de puntos u objetos que debido a la presencia de ruido en la imagen, oclusiones o movimientos bruscos, puede ser perdido el rastro del objeto de interés y por ende su ubicación en la siguiente imagen de la secuencia. Es por esta razón que en este trabajo se ha recurrido a utilizar un algoritmo conocido como Filtro de Kalman. 32

42 El filtro de Kalman es un algoritmo recursivo que estima la posición e incertidumbre en la medida de un punto de interés en la siguiente imagen, es decir, predice donde se encontrará y cuan grande es la región donde debe ser buscado. El objetivo de este filtro es entonces, la obtención de un estimador óptimo de las variables de estado de un sistema dinámico, basado en observaciones ruidosas y en un modelo de incertidumbre de la dinámica del sistema [23]. En aplicaciones de visión por computador, específicamente en el caso de seguimiento o tracking de puntos, se define un estado, el cual describe el movimiento del punto de interés en el plano de la imagen, determinado por las coordenadas de posición, y las componentes de velocidad y. La observación en cambio es un vector de dos componentes y pertenecientes a las coordenadas de la posición del punto de interés observada por las cámaras. Las ecuaciones que rigen este proceso en tiempo discreto son: Donde: La matriz A es el modelo de transición de estados y relaciona el estado en tiempo k-1 con el estado en tiempo k, esta relación se muestra en las siguientes ecuaciones y matrices. [ ] [ ] 33

43 La matriz B relaciona la entrada control con el estado, mientras que la matriz H relaciona el estado con la medida y tiene la forma: [ ] [ ] Las variables aleatorias y representan el ruido del proceso y de la medida respectivamente asumiéndose que son independientes y blancos de media cero. El filtro de Kalman proporciona una ecuación que computa un estimador del estado a posteriori como combinación lineal del estimador a priori y la diferencia ponderada entre la observación actual z k y una predicción de la medida. La diferencia entre la observación actual y la predicción de la medida se conoce como innovación de la medida o medida residual. La matriz K k es conocida como Ganancia de Kalman y pondera la influencia del error entre la estimación y la medida, y se obtiene de la forma: Donde es la covarianza del error medido y es la matriz de estimación de la covarianza del error a priori (medida de la precisión del estado estimado) que se establece por: Con como la matriz de covarianza del sistema. Con estas ecuaciones, cuando la matriz se aproxima a cero, la ganancia ponderará el residuo con mayor peso, es decir, se tendrá mayor confianza en la observación actual. Por otra parte cuando el estimador de la covarianza del error a priori se aproxime a cero, se perderá confianza en la observación y la de la medida predicha se incrementará. 34

44 El ciclo de trabajo del filtro de Kalman (Figura 21) esta compuesto de dos fases bien definidas: predecir y actualizar. La fase de predicción usa el estado estimado del instante de tiempo previo para producir una estimación del estado en el tiempo actual. Esta predicción del estado estimado es también conocida como el estado estimado a priori, aunque esta es una estimación del estado en el tiempo actual, esta no incluye la información proveniente del observador, también en el tiempo actual. En la fase de actualización la predicción a priori es combinada con la observación actual para corregir el estado estimado, esto se conoce como el estado estimado a posteriori. Actualización en el Tiempo Corrección 1. Calcular la ganancia de Kalman. K k P k H k T H k P k H k T R k 2. Actualizar la estimación con la medida. x k x k K k z k H k x k 3. Actualizar la covarianza del error a posteriori. x k A k x k Bu k Valores iniciales x k P k Actualización en el Tiempo Predicción 1. Proyectar hacia adelante el estado. 2. Proyectar hacia adelante la covarianza del error a priori. P k A k P k A k T Q k P k I K k H k P k Figura 21. Ciclo de Trabajo del Filtro de Kalman Típicamente las dos fases se alternan, con la predicción estimando un estado y la actualización corrigiéndolo con la observación. Sin embargo si una observación no esta disponible por alguna razón, la fase de actualización puede ser omitida utilizándose así solamente los estados predichos. 35

45 Esta es una de las ventajas de la aplicación de este tipo de filtros en este trabajo, ya que en caso de no encontrarse la correspondencia del punto de interés en la imagen derecha, el filtro podrá predecir su ubicación y evitar así pequeñas oclusiones. La otra ventaja principal del uso de este filtro en este proyecto, es que se puede predecir el estado futuro en el que se encontrará ya sea un punto u objeto de interés compensando de esta manera el tiempo de computo consumido por la etapa de adquisición y procesamiento de las imágenes así como tiempo de ejecución de las tareas del robot. 36

46 CAPÍTULO 3 Desarrollo del Sistema de Visión En este capítulo se explica el desarrollo del sistema de procesado de imágenes así como las funciones utilizadas, que permiten efectuar la sincronización del manipulador de forma automática, a partir de los conceptos teóricos revisados en el Capítulo 2. La programación del algoritmo se ha realizado en Microsoft Visual Studio 2008 junto con las librerías de OpenCV para procesamiento de imágenes. El diagrama de flujo general del programa realizado se muestra en la Figura 22. INICIO Calibración y Rectificación de Cámaras Adquisición de Imágenes Detección de Puntos Singulares Tracking de Puntos NO Selección de Puntos? SI Emparejamiento de Puntos Homólogos Filtrado y Predicción Señales (Filtro de Kalman) Obtención del 3D (Posición y orientación) Envío de datos al Robot FIN Figura 22. Diagrama de Flujo General del Procesamiento de Imágenes 37

47 El algoritmo inicia con la calibración y rectificación de cámaras, procesos útiles ya que permitirán determinar la posición del objeto 3D con precisión ya que los datos entregados por el fabricante no son lo suficientemente exactos para este procedimiento. Con los parámetros intrínsecos y extrínsecos se procede a adquirir la secuencia de imágenes (de la región de interés en este caso el corazón) en las que se encontrarán los puntos relevantes de la escena, de la cual el operador tendrá la capacidad de escoger tres de ellos, para determinar el plano de interés, calcular la posición en el espacio 3D de su punto medio y la orientación de la normal para que el efector final del robot se oriente en modo opuesto. Este calculo de la posición se lo hace en base al origen de coordenadas que se ubicará en el eje óptico de la cámara izquierda, por lo que es necesario cambiar el sistema de coordenadas al del robot para que este pueda sincronizarse con el de la superficie de trabajo, esto se lo hace mediante el cómputo de una matriz de rotación y una de traslación entre sistemas de coordenadas. El algoritmo también hará el seguimiento (tracking) de esta superficie en tiempo real, sin embargo al ser las imágenes sensibles al ruido, es necesario añadir una etapa de filtrado de los datos de posición y orientación obtenidos, así como, la compensación del tiempo empleado en realizar todas las operaciones matemáticas del algoritmo, todo esto hecho mediante el filtro de Kalman. Una vez que estos datos hayan sido filtrados se enviarán al robot para que este sincronice su movimiento con el plano detectado. A continuación se realiza una descripción detallada de cada una de las etapas que conforman el algoritmo realizado. 3.1 Adquisición de Imágenes Para la etapa de adquisición de imágenes se han utilizado dos cámaras de la marca Logitech, cuyo modelo es el Hd Pro Webcam C920 con conexión USB, las cuales en esta aplicación se las ha hecho trabajar a una resolución de 640 x 480 píxeles en un ordenador de 4gb de RAM, y procesador Intel Core i7. 38

48 Figura 23. Logitech Hd Pro Webcam C920 Estas cámaras se han colocado en un soporte de aluminio separadas una distancia de 9.34 cm en relación al eje x, donde se ha tomado como eje de referencia el eje óptico de la cámara izquierda (vista desde la parte posterior) como se muestra en la Figura 24. x y z 9.34 cm Figura 24. Vista frontal de la disposición de las cámaras en modo estéreo 3.2 Calibración y Rectificación de Cámaras El algoritmo de calibración de cámaras permite obtener los parámetros intrínsecos y extrínsecos de las cámaras utilizadas. El esquema de este proceso se muestra en la Figura 25. Adquisición y Almacenamiento de Imágenes (Plantilla de Tabla de Ajedrez) Detección de Esquinas de los cuadrados Calibración de Cámaras Parámetros Intrínsecos y Extrínsecos Detección y Tracking de Puntos Eliminación de Distorsiones Rectificación de Imagenes Figura 25. Esquema del Algoritmo de Calibración, Rectificación y Eliminación de la Distorsión de Imágenes 39

49 El primer paso consiste en adquirir las imágenes que posean un patrón concreto, en este caso, la plantilla del tablero del ajedrez, la cual debe tener los bordes blancos para obtener resultados robustos. Las características de la plantilla usada son: Tamaño del cuadrado 2.5 cm Intersecciones horizontales: 8 Intersecciones verticales: 6 Figura 26. Imágenes de la Plantilla de Ajedrez Utilizadas Una vez obtenido el conjunto de imágenes se ha utilizado la función de OpenCV CvStereoCalib, la cual detectará automáticamente las esquinas de los cuadrados de las plantillas, asignará una ubicación a cada una de ellas y entregará como resultado las matrices de parámetros intrínsecos y extrínsecos de cada cámara. CvStereoCalib (): ObjectPoints: matriz del conjunto puntos de las esquinas en el espacio 3D ImagePoints1: matriz del conjunto de puntos vistos en la imagen de la cámara 1 ImagePoints2: matriz del conjunto de puntos vistos en la imagen de la cámara 2 pointcount: vector que contiene el número de puntos de cada una de las imágenes cameramatrix1,2: matrices de parámetros intrínsecos de las cámaras distcoeffs1,2: matrices de los parámetros de distorsión de la lente de las cámaras ImSize: tamaño de la imagen R: matriz resultado con los parámetros de rotación entre las cámaras T: matriz resultado con los parámetros de traslación entre las cámaras 40

50 Los parámetros intrínsecos y extrínsecos obtenidos de cada cámara son: PARÁMETROS INTRÍNSECOS DE LAS CÁMARAS Cámara Izquierda: Distancia focal: Coordenadas del punto principal: { Formando con estos valores la matriz M1: [ ] Matriz de Coeficientes de Distorsión D1: [ ] Cámara Derecha: Distancia focal: Coordenadas del punto principal: { Formando con estos valores la matriz M2: [ ] Matriz de Coeficientes de Distorsión D2: [ ] PARÁMETROS EXTRINSECOS DE LAS CÁMARAS 41

51 Los parámetros extrínsecos obtenidos experimentalmente obtenidos por el algoritmo son los siguientes: Cámara Izquierda: Matriz de Rotación [ ] [ ] Matriz de Traslación (cm) [ ] Cámara Derecha: Matriz de Rotación [ ] [ ] Matriz de Traslación (cm) [ ] Se puede ver con estos resultados que al ser el punto focal de la cámara izquierda el eje de referencia de este sistema, su vector de traslación será cero en las tres componentes, sin embargo en el caso de la cámara derecha esta se encuentra desplazada una distancia considerable en el eje de las x, mientras que en los ejes y y z hay unos pequeños desplazamientos que deberán ser tomados en cuenta en la matriz de proyección para los futuros cálculos. Con los parámetros de las cámaras obtenidos también es posible calcular la profundidad mínima y máxima que permiten medir las cámaras colocadas en la posición descrita, esto es: 42

52 Una vez conocidos los parámetros intrínsecos y extrínsecos de las cámaras se procede a realizar el proceso de rectificación de cámaras, con la finalidad de que las filas de las imágenes de las dos cámaras coincidan (líneas epipolares), esto mediante la función cvstereorectify CvStereoRectify(): CameraMatrix 1,2: matrices de parámetros intrínsecos las dos cámaras distcoeffs 1,2: matrices de los coeficientes de distorsión de las cámaras R: la matriz de rotación de la cámara derecha respecto de la izquierda T: matriz de traslación de la cámara derecha respecto de la izquierda R1,2: matriz de rectificación de las cámaras 1 y 2 P1,2: matrices de reproyección (rectificadas) de las cámaras 1 y 2 Q: matriz de reproyección para visión estéreo De aquí será de gran utilidad la matriz Q, ya que facilitará el cálculo de la profundidad al momento de estimar la posición 3D de los puntos escogidos pertenecientes a la superficie de interés. Esta matriz viene dada por: [ ] [ ] Mediante la función cvinitundistortrectifymap se calculan independientemente las matrices que permiten eliminar las distorsiones producidas por las lentes de las cámaras. CvInitUndistortRectifyMap (): cameramatrix: matriz de parámetros intrínsecos de la cámara 43

53 distcoeffs: matriz de los coeficientes de distorsión de la cámara R (matriz rectificada): matriz de la cámara 1 eliminada las distorsiones mapx: matriz resultado para el mapeo de la imagen en x mapy: matriz resultado para el mapeo de la imagen en y Una vez realizados estas etapas se tienen las imágenes resultantes: Figura 27. Imágenes sin Rectificar Figura 28. Imágenes Rectificadas y sin Distorsión En las imágenes mostradas se han graficado a modo ilustrativo 16 líneas (color verde) con la finalidad de observar el objetivo de la rectificación de imágenes, es decir, que un mismo punto visto desde dos cámaras, se encuentre en la misma fila en las dos tomas (líneas epipolares). El procedimiento de calibración y rectificación se realiza una sola vez, lo único que se debe hacer en la secuencia de imágenes es eliminar la distorsión mediante las matrices mapx y mapy de cada cámara. 44

54 3.3 Detección de Puntos de Interés y Seguimiento (Tracking) Una vez que se han calibrado y rectificado las cámaras, es posible realizar el seguimiento de los puntos característicos de interés de la secuencia de imágenes obtenida mediante las cámaras. En la Figura 29, se muestra el diagrama de flujo detallado de todo este procedimiento. Detección y Tracking de Puntos Adquisición de Imágenes Detección de Puntos Singulares (Cámara Izquierda) Seguimiento de los Puntos Localizados Num. puntos > Mínimo? NO SI Seleccionar 3 Puntos del Plano NO Seleccionar Punto 1 Seleccionar Punto 2 Seleccionar Punto 3 NO NO SI SI SI Emparejamiento Punto 1 Emparejamiento Punto 3 Emparejamiento Punto 3 Figura 29. Diagrama de flujo de la detección de puntos de interés y seguimiento El proceso inicia al adquirir las imágenes provenientes de las cámaras a una resolución de 640x480 pixeles, para luego encontrar su equivalente en escala de grises y así poder procesarlas. 45

55 Se toma entonces la imagen de la cámara izquierda en cada frame y se realiza la detección de los puntos singulares mediante la función goodfeaturestotrack, cuya tarea es aplicar el detector de esquinas de Harris en toda la imagen. GoodFeaturesToTrack(): Image. Imagen en la que se realizará la detección cornercount. numero máximo de puntos relevantes a detectar qualitylevel. factor de calidad para los puntos relevantes mindistance. Mínima distancia entre puntos detectados El número máximo de puntos ha ser detectados se ha establecido mediante ensayo y error en un límite de 200 puntos y sus coordenadas son almacenadas en forma de vector. El nivel de calidad que en el Capítulo 2 se lo denominó como λ y que será el valor mínimo que pueden poseer los valores propios de la matriz de correlación se ha establecido en un valor de 0.01, mientras que la distancia mínima (Euclídea) entre puntos se ha fijado a un valor de 10 píxeles para evitar superposición entre ellos y por ende errores en el tracking. Figura 30. Puntos Relevantes detectados en la Escena Una vez se tienen detectados los puntos relevantes de toda la escena, se procede a realizar el seguimiento o tracking de los mismos. Para ello se ha utilizado una función de OpenCV que implementa el algoritmo de Flujo Óptico de Lucas Kanade Piramidal calcopticalflowpyrlk. calcopticalflowpyrlk(): 46

56 prev. Imagen previa (anterior) curr. Imagen actual prevfeatures. Matriz de coordenadas de los puntos relevantes en la imagen previa currfeatures. Matriz (resultado) de coordenadas de los puntos relevantes encontrados en la imagen actual winsize. Tamaño de la ventana de búsqueda de cada nivel de la pirámide, para este caso (21x21 píxeles) level. niveles que conforman la pirámide, en este caso se ha escogido 3. En esta aplicación se realiza el seguimiento de todos los puntos singulares detectados en la escena, sin embargo para los fines de este trabajo, se ha establecido que solamente se deberá escoger tres de ellos para determinar el plano formado perteneciente a la superficie de interés. Estos puntos se pueden escoger mediante el uso del puntero del ratón del ordenador al hacer clic izquierdo sobre el elegido y se puede eliminar la selección al hacer clic derecho sobre el mismo. Debido a que el punto de interés es solo un pixel en la imagen, sería difícil para el operador seleccionarlo con precisión, por lo que se ha generado una ventana alrededor del punto seleccionado con el puntero de 5x5 píxeles, de modo que el punto de interés mas cercano será detectado siempre y cuando entre dentro de dicha ventana (Figura 31). Es importante tomar en cuenta que nunca se seleccionaran dos puntos al mismo tiempo mediante este algoritmo, ya que, como se ha mencionado anteriormente, la distancia euclídea entre puntos singulares se ha establecido a un valor de 10 píxeles, distancia superior a la de las dimensiones de la ventana (5x5). 47

57 Píxel de Interés Píxel Seleccionado Ventana 5x5 píxeles Punto Detectado Punto No Detectado Figura 31. Selección de Puntos de Interese en Pantalla 3.4 Emparejamiento de Puntos Singulares La etapa de emparejamiento de los puntos escogidos por el operador se la ha realizado mediante la implementación del algoritmo de Match Template. El diagrama de flujo del funcionamiento de esta etapa se muestra en la Figura 32. Emparejamiento de Puntos Punto de Interés Clic Izquierdo SI Activar Selección de Punto NO Clic Derecho SI Eliminar Selección del Punto Extracción de Ventana Alrededor del Punto (Ventana1) Fin Identificación de la ventana en la Imagen derecha donde se realizará el barrido (Ventana2) Barrido de la Ventana 1 sobre la Ventana 2 Obtención del Máximo valor de la Matriz de Resultados Filtro de Kalman Figura 32. Diagrama de Flujo Etapa de Emparejamiento de Puntos 48

58 Esta etapa se inicia al detectarse ya sea el clic izquierdo o derecho del mouse. En el primer caso, se selecciona el punto de interés en la imagen izquierda, una vez que se tiene la posición de ese píxel se crea una ventana a su alrededor, en este caso de dimensiones de 41x17, mientras que, en la imagen derecha crea una ventana de ancho variable W i y 23 píxeles de alto, alrededor del punto donde se predice se encontrará el homólogo (esto calculado por Kalman que se explica posteriormente) cuya línea media será la fila perteneciente a la coordenada Y del punto de interés seleccionado. Se ha escogido estas dimensiones para obtener mejores resultados al momento de realizar la búsqueda ya que se dispone de gran cantidad de datos. La función Match Template para hacer la detección del par requiere los siguientes parámetros: Match Template(): image. Imagen (ventana) donde se realizara la búsqueda de la correspondencia, dimensiones W i x H i templ. Ventana o plantilla que contiene al punto de interés a ser buscado, dimensiones W t x H t result. Mapa (matriz) de resultados, Dimensiones W R x H R method. Método utilizado, en este caso el de Correspondencia por Correlación Normalizado, tomando como coincidencia el valor máximo en la matriz result. La matriz result contiene los valores calculados por el método de Correlación de Coeficientes Normalizado para cada uno de los pixeles de la ventana donde se ha realizado la búsqueda. La matriz de resultados tiene las siguientes dimensiones: De esta matriz se debe buscar la ubicación del máximo valor calculado ya que será el pixel en donde se encuentra su correspondencia. Para hallar su equivalente en pixeles de la imagen derecha se han establecido las ecuaciones: 49

59 Donde: Px d, Py d. Pixel resultado en coordenadas de la imagen derecha. Max X, Max Y. Pixel resultados. resultado (valor máximo) en coordenadas de la matriz de Sin embargo muchas veces por efectos de luz u oclusiones, es decir, que la cámara derecha no ve la platilla alrededor del punto de interés vista por la cámara izquierda, pueden presentarse malos cálculos en la etapa de emparejamiento dando como resultado puntos erróneos (falsos positivos), esto se busca corregir mediante la implementación del filtro de Kalman que se explica a continuación. 3.5 Filtro de Kalman La función principal del filtro de Kalman es estabilizar dinámicamente el sistema, realizando el filtrado y la estimación de las posición de cada uno de los puntos de interés de las imágenes seleccionados por el usuario. La función de este filtro es atenuar el ruido y corregir la posición que muestra la cámara izquierda de cada uno de los puntos escogidos y predecir cual será su posición en el siguiente frame, dato que será de utilidad para calcular el 3D. Una vez que se tiene el dato de la posición del punto filtrado, se procede a la búsqueda de su homólogo en la otra imagen, punto que una vez hallado también es filtrado. De cada uno de los puntos filtrados en la imagen derecha, también se calcula una estimación o predicción de donde se encontrarán en el siguiente frame, para resolver el 3D, compensando el retardo de tiempo transcurrido en durante el procesado de imágenes. 50

60 El dato de la predicción de la ubicación de los puntos en la imagen derecha también permitirá evitar oclusiones en esta cámara en caso de que el algoritmo de emparejamiento no encuentre el punto homólogo en esta imagen, es decir, si el máximo valor de la matriz de resultados es menor a 0.8 (valor escogido mediante pruebas) se asume que no hay un correcto matching (entonces existe un falso positivo), por lo se utilizará el dato de la posición predicha, y el ancho de la ventana de búsqueda W i se incrementará en 20 pixeles para la siguiente imagen ya que hay mayor incertidumbre en la posición real del punto. El tamaño por defecto de W i es de 61 píxeles. El funcionamiento del algoritmo de Filtro de Kalman se puede ver en la Figura 33. Máximo Valor de la matriz de resultados Valor > 0.8 NO SI Usar coordenadas de la matriz de Resultados Usar coordenadas del Punto Predicho Wi = 61 Wi = Wi + 20 Filtro de Kalmam Punto Filtrado Punto Predicho para el siguiente Frame Mostrar en pantalla Cálculo del 3D Figura 33. Filtro de Kalman 51

61 3.6 Calculo de la Posición 3D Una vez que se tiene la ubicación de la predicción de los puntos seleccionados en las imágenes izquierda y derecha, se procede a realizar el cálculo de la posición en el espacio 3D mediante la matriz de reproyección Q calculada en el apartado 3.2. En el algoritmo implementado se calculan las coordenadas mediante las ecuaciones: Estas coordenadas establecen la posición 3D de un punto con respecto al origen de coordenadas que se encuentra ubicado en el punto focal de la cámara izquierda. Por lo tanto deben pasarse al sistema de referencia de robot para poder sincronizar el movimiento. Esto se lo hace mediante la obtención de una matriz de transformación que contiene la rotación y traslación entre los sistemas de coordenadas. 3.7 Transformación del sistema de coordenadas La transformación del sistema de coordenadas entre el de la cámara y el robot debe hacerse con la finalidad de que este último conozca como se mueve la región de trabajo y por ende pueda reproducir su movimiento. Para realizar este cambio entre sistemas de referencia lo que se hace es tomar varios puntos de una imagen de los que se conocen su posición 3D con respecto origen de coordenadas del robot y sus equivalentes en píxeles de la imagen, datos suficientes para calcular la matriz de rotación y traslación entre sistemas de referencia. La función cvfindextrinsiccameraparams2 facilita el calculo de estas matrices. 52

62 cvfindextrinsiccameraparams2(): objectpoints. matriz que posee las coordenadas en 3D de los puntos conocidos imagepoints. matriz con las coordenadas de los puntos en píxeles cameramatrix. matriz de parámetros intrinsecos de la cámara izquierda distcoeffs. coeficientes de distorsión de la cámara izquierda rvec matriz de rotación 3x3 (resultado) tvec matriz de traslación 3x1 (resultado) Los resultados obtenidos por esta función fueron: [ ] [ ] El cálculo de estas matrices se realiza una sola vez, y serán utilizadas en la etapa final solamente para cambiar el sistema de referencia como se ha mencionado anteriormente. Con las coordenadas en el sistema de referencia del robot, se calcula el vector normal al plano para lo que se realiza el producto cruz entre dos vectores pertenecientes al mismo. En este caso llamaremos a los tres puntos escogidos de la escena P, Q y R. Se toma entonces como referencia el punto P y se crea los vectores y. El vector normal al plano lo llamaremos. De aquí, se calculan los cosenos directores de la forma: Obteniendo el arcoseno de estos valores se obtiene entonces α, β y γ, que será la orientación en la que se situará el efector final del brazo robótico. 53

63 Z R v γ β P α Q Y X Figura 34. Representación del Plano de Trabajo y la Orientación de la Normal 3.8 Comunicación con el Robot El robot empleado para las pruebas de sincronización de movimientos con la superficie de interés es el ABB ARB-140 disponible en el laboratorio de robótica, Figura 35. Este robot industrial consta de 6 grados de libertad, cuya capacidad de carga máxima es de 6 kilogramos con un alcance de hasta 81cm. Su base tiene la posibilidad de rotar 360 permitiendo trabajar sobre un espacio de trabajo amplio con la posibilidad de ser montado en el suelo, suspendido, en pared o en cualquier ángulo que requiera la aplicación. Su Punto de Centro de Herramienta (TCP) puede alcanzar una velocidad y aceleración máxima de 2.5 m/s y 20m/s 2 respectivamente. Figura 35. Brazo Robótico ABB ARB

64 Las características de desplazamiento y velocidad máxima de cada uno de los ejes del robot ABB ARB 140 se presentan en la Tabla 1. Robot ABB ARB 140 Eje Angulo Máximo de Giro Velocidad Máxima /s /s /s 4 Ilimitado (400 por defecto) 360 /s /s 6 Ilimitado (800 por defecto) 450 /s Tabla 1. Características de las Articulaciones del Robot ABB ARB 140 Las longitudes de las articulaciones del robot, se aprecian en la Figura 36 [24]. Figura 36. Espacio de Trabajo y Dimensiones del Robot ABB ARB 140 Mediante el intérprete desarrollado por el laboratorio de robótica del ESAII se facilita el envío y ejecución de órdenes hacia el robot. En esta aplicación se ha modificado el interprete de tal modo que el algoritmo de visión una vez haya calculado la posición y orientación de la superficie de trabajo (en este caso de la maqueta del corazón), envíe automáticamente estos datos al robot, es decir la posición en X, Y, Z (milímetros) en las que se desea posicionar el TCP, y los ángulos α, β y γ (grados) correspondientes a su 55

65 orientación. Las variaciones de las posiciones y orientación son realizadas de forma incremental, es decir, solo se envía hacia el robot el incremento en la posición que se desea que se desplace. El diagrama de flujo del envío de datos al robot se muestra en la Figura 37. Se ha configurado la respuesta del robot de modo que responda 10 ms antes de llegar a la posición destino con la finalidad de que no haya pérdida de datos en caso de que el algoritmo de visión los calcule o vaya mas rápido que el robot y así buscar un equilibrio entre el cálculo y el envío. De este modo también se obtendrán movimientos suaves del manipulador ya que no se esperará a que se detenga por completo para recibir un nuevo dato de posición. INICIO Datos de posición y orientación calculados Calculo de los incrementos con respecto al frame anterior Robot listo para recibir datos NO SI Enviar datos Ejecutar Movimiento FIN Figura 37. Diagrama de Flujo de la Comunicación con el Robot ABB ARB

66 CAPÍTULO 4 Experimentos y Resultados En el presente capítulo se mostrará los resultados obtenidos en las distintas etapas del proyecto. Iniciando por ver los resultados obtenidos por la calibración y rectificación de cámaras, seguidos de la selección y emparejamiento de puntos singulares en imágenes estáticas para el cálculo de su posición 3D y así poder calcular el error generado por el sistema de visión. A continuación se observarán los resultados del tracking de los puntos y el comportamiento del filtro de Kalman con los datos obtenidos para finalizar con la etapa de compensación del retardo de tiempo generado por todo el sistema. 4.1 Calibración y rectificación de cámaras En esta parte se muestran los resultados obtenidos por la calibración y rectificación de imágenes para dos posiciones distintas del patrón, en las que se visualizan 30 líneas epipolares de color rojo (Figura 38 a. y b.). Con las figuras mostradas se puede determinar que hay una correcta calibración y rectificación de cámaras ya que al observar con detenimiento las líneas graficadas, estas pasan a lo largo de los mismos puntos vistos en la imagen izquierda y derecha. (a) 57

67 (b) Figura 38. Visualización de la Calibración y Rectificación de Cámaras para Dos Vistas de la Escena El error entregado por el programa en la etapa de calibración fue de ± pixeles, que no es más que la distancia entre los puntos esquina del tablero de ajedrez detectado y las líneas epipolares que pasan cerca de ellos, es decir, se tiene 1 pixel de error, valor que se encuentra dentro de los rangos tolerables para este procedimiento. 4.2 Detección y Emparejamiento de Puntos Singulares Como se ha mencionado anteriormente, en este trabajo se utilizado el detector de esquinas de Harris para la detección de puntos relevantes de la superficie de interés. En la realización de pruebas de esta etapa se ha tomado una parte de los puntos singulares de dos escenas distintas vistas por las cámaras para verificar si la etapa de emparejamiento está hallando los respectivos pares de puntos homólogos entre la imagen derecha y la izquierda. La Figura 39, muestra el resultado de realizar el emparejamiento de puntos de objetos que se encuentran cercanos a las cámaras (distancia menor a 50 cm), mientras que la Figura 40, muestra el emparejamiento de puntos de objetos lejanos (distancia de hasta 4 m). 58

68 Figura 39. Emparejamiento de puntos relevantes de una imagen estática con objetos cercanos (Entorno 1) Figura 40. Emparejamiento de puntos relevantes de una imagen estática con objetos lejanos (Entorno 2) A continuación se ha probado el algoritmo con una secuencia de imágenes con objetos cercanos, escogiéndose solamente tres puntos de interés establecidos por el usuario, observándose la correcta búsqueda del par en la imagen derecha. (a) 59

69 (b) Figura 41.a) y b) Emparejamiento de Puntos Relevantes Seleccionados en Video 4.3 Calculo de la Posición 3D respecto a la cámara Una vez que se poseen los pares de puntos en las imágenes de las cámaras estéreo se procede a realizar el cálculo de la posición 3D de los puntos de interés. El sistema se muestra en la Figura 42, mientras que los resultados obtenidos en cada uno de los planos se muestran en la Figura 43. X Y Z Figura 42. Disposición de los Elementos en la Escena 60

70 Figura 43. Puntos Observados en cada uno de los Planos del Espacio 3D Figura 44. Coordenadas de los puntos seleccionados (Entorno 1) Se ha realizado una medición estimada de la profundidad del sistema de la Figura 40, obteniéndose los resultados mostrados en la Tabla 2. 61

71 Punto Profundidad Z Calculada [cm] Profundidad Z Medida [cm] Error Absoluto [cm] Error Relativo [%] Tabla 2. Calculo de Errores de la Profundidad de Puntos de Interés *Medidas tomadas con la regla graduada, Marca: TAJIMA, Rango de Medida: mm, Precisión: 0.5mm, No. 063M. Como se puede observar en los resultados, la profundidad se ha calculado con buena precisión, obteniendo como máximo error, un valor de 1.48% que lo que nos hace notar nuevamente un correcto cálculo de los parámetros intrínsecos y extrínsecos de la cámaras así como una correcta rectificación y emparejamiento de los puntos detectados. En el caso de mayores distancias (Entorno 2, Figura 38), la ubicación calculada de los puntos se muestra en la Figura 45, para los planos XZ e YZ, mientras que los valores en coordenadas XYZ se los muestra en la Figura 46. Para esta aplicación se utilizará distancias cortas no superiores a los 50 cm desde el área de trabajo a las cámaras por lo que estos resultados, son presentados solamente para verificar el funcionamiento del algoritmo de estéreo visión a distancias mucho mayores. Los resultados presentados en las Figuras 45 y 46, para X, Y y Z de los puntos detectados, se hallan calculados con respecto a la cámara izquierda, sistema de referencia que debe ser cambiado al del robot para su movimiento. 62

72 Figura 45. Puntos Observados en los plano XZ y YZ (Entorno 2) Figura 46. Coordenadas de los puntos seleccionados (Entorno 2) 4.4 Calculo de la posición 3D respecto al sistema de coordenadas del robot Como se ha mencionado anteriormente, con el objetivo de que el robot pueda sincronizar sus movimientos con los de la superficie de interés es indispensable que las coordenadas de esta, se hallen en el mismo sistema de coordenadas que el robot y no en el de las cámaras, razón por la que se ha calculado las matrices de rotación y traslación para cambiar este ultimo sistema de referencia. 63

73 Para el cambio de sistemas de referencia hemos de tomar como objeto móvil conocido, el punto central efector final ya que mediante el controlador del robot podemos saber cual es su posición en todo momento y su posición en pixeles en la imagen. Este punto se muestra en la Figura 47. Punto Referencia del Elemento Terminal Figura 47. Efector Final Obtenidas las matrices de rotación y traslación se ha procedido a realizar las pruebas correspondientes para calcular el error de posición producto de esta transformación. Para ello, se ha tomado 10 posiciones distintas para el efector final del robot, algunas de las cuales se muestran en la Figura 48. Figura 48. Posiciones del Elemento Terminal Las coordenadas calculadas por el sistema de visión y las reales proporcionadas por el controlador del robot, se muestran en la Tabla 3. 64

74 Posición Coordenadas Reales [cm] Coordenadas Calculadas [cm] X Y Z X Y Z 1 60,20-16,00 31,70 60,17-15,98 31, ,20-16,00 24,70 57,18-15,93 24, ,20-16,00 25,70 63,08-16,29 25, ,20-18,00 13,70 59,39-18,10 13, ,20-17,00 10,70 56,02-17,40 11, ,20-21,00 25,70 58,90-21,20 24, ,20-21,00 26,70 52,90-21,23 26, ,20-8,00 10,70 59,27-8,16 10, ,20-16,00 31,70 63,35-16,16 31, ,20-14,00 28,70 58,46-14,13 28,89 Tabla 3. Posiciones Reales y Calculadas en Coordenadas del Robot La Tabla 4, contiene los errores absoluto y relativo calculados para cada una de estas posiciones. Posición Error Absoluto [cm] Error Relativo [%] X Y Z X Y Z 1 0,03 0,02 0,40 0,05-0,12 1,26 2 0,02 0,07 0,24 0,03-0,44 0,97 3 0,12 0,29 0,20 0,19-1,81 0,78 4 0,19 0,10 0,42 0,32-0,56 3,07 5 0,82 0,40 0,40 1,49-2,35 3,74 6 0,30 0,20 1,20 0,51-0,95 4,67 7 0,30 0,23 0,70 0,56-1,10 2,62 8 0,07 0,16 0,10 0,12-2,00 0,93 9 0,15 0,16 0,15 0,24-1,00 0, ,26 0,13 0,19 0,45-0,93 0,66 Tabla 4. Error Absoluto y Relativo del Cambios de Sistema de Coordenadas Como se puede observar con los resultados obtenidos, el máximo error relativo que se tiene en la etapa de cambio de coordenadas entre el sistema de referencia de la cámara hacia el del robot es del 4.67%, esto puede deberse a que en las ubicaciones 5 y 6, no se pudo 65

75 detectar el punto con precisión en las dos imágenes y por ende existe error en el computo de la visión estéreo. 4.5 Errores de Emparejamiento Se han realizado pruebas de la etapa de emparejamiento de puntos singulares para diferentes tamaños de plantillas (ventanas) alrededor del punto de interés, tal y como se muestra en la Figura 49. Emparejamiento Matching Plantilla (5x5) (11x11) (17x17) (41x17) Figura 49. Resultados de la Etapa de Emparejamiento Como se puede observar, mejores resultados se presentan cuando la plantilla alrededor del punto de interés es de mayor tamaño, esto gracias a que se tienen más pixeles (datos) con los que se puede realizar las comparaciones respectivas contra la imagen derecha. Sin embargo, esto al mismo tiempo implica un mayor coste computacional por el número de 66

76 operaciones que se realizan. En este caso se ha escogido la plantilla de dimensiones 41x17, ya que se observó que al incrementar el número de pixeles en el eje de las x de la plantilla se obtuvieron resultados más estables, es decir el par encontrado se mantuvo fijo sin oscilar su posición de manera frecuente. 4.6 Interfaz de Usuario En las siguientes imágenes, se muestra la interfaz de usuario empleada en este trabajo, la cual se compone de dos ventanas, la del sistema de visión diseñada completamente, para mostrar las imágenes vistas y datos adquiridos (Figura 50), y la ventana de comunicación (intérprete) con el robot a la que se le han añadido funcionalidades para que estas puedan trabajar conjuntamente Figura , 2. Imágenes captadas por la cámara izquierda y derecha respectivamente. 3, 4. Puntos de interés escogidos por el usuario (color verde), punto medio del plano formado (color azul). 5. Líneas epipolares (emparejamiento de puntos). 6, 8, 10. Posición del punto central (color azul) en coordenadas X, Y y Z respectivamente. 7, 9, 11. Ángulos de orientación del vector normal al plano, alfa, beta y gamma respectivamente. 12, 13, 14. Grafica de datos de posición obtenidos en X, Y y Z respectivamente. Figura 50. Ventana 1 Adquisición y presentación de datos, y descripción de sus elementos en pantalla 67

77 1. Cuadro de mensajes enviados por los robots (posiciones y orientaciones) 1 2. Izquierda - Ángulos de cada una de las articulaciones. Derecha - Posición y orientación del efector final de los robots Cuadros de ingreso de datos para movimientos del robot, en ángulos o en posición del efector. 4. Accionamiento de los robots, selector de velocidad. 5. Movimientos por defecto programados Botones de selección del tipo de movimiento deseado para los robots Botones de calibración del filtro de Kalman. 8. Botón para adquirir datos en archivo xml. 9. Control del offset de las gráficas de la ventana de visión Botón de paro. Figura 51. Ventana 2 Interfaz del Interprete y descripción de los elementos en pantalla 4.7 Adquisición y Filtrado de Datos En esta sección se presentan los datos de posición adquiridos para diversos puntos seleccionados del área de trabajo, y los resultados obtenidos una vez filtrados por Kalman con diferentes valores para las matrices Q k y R k, parámetros que influyen en el comportamiento y en la respuesta que entregará el algoritmo. Cabe recordar que en esta aplicación: [ ] [ ] Donde se ha variado los parámetros de Q y R. Conjunto de Puntos 1. R = 1, Q =