Repositorio Institucional de Documentos

2014 34 Óscar García Grasa Visual SLAM for Measurement and Augmented Reality in Laparoscopic Surgery Departamento Director/es Instituto de Investigación en Ingeniería [I3A] Martínez Montiel, José María Director/es Tesis Doctoral Autor Repositorio de la Universidad de Zaragoza – Zaguan http://zaguan.unizar.es UNIVERSIDAD DE ZARAGOZA Departamento Director/es Óscar García Grasa VISUAL SLAM FOR MEASUREMENT AND AUGMENTED REALITY IN LAPAROSCOPIC SURGERY Director/es Instituto de Investigación en Ingeniería [I3A] Martínez Montiel, José María Tesis Doctoral Autor 2014 Repositorio de la Universidad de Zaragoza – Zaguan http://zaguan.unizar.es UNIVERSIDAD DE ZARAGOZA Departamento Director/es Director/es Tesis Doctoral Autor Repositorio de la Universidad de Zaragoza – Zaguan http://zaguan.unizar.es UNIVERSIDAD DE ZARAGOZA Universidad de Zaragoza PHD THESIS PhD on Biomedical Engineering Visual SLAM for Measurement and Augmented Reality in Laparoscopic Surgery. SLAM visual para Mediciones y Realidad Aumentada en Cirug´ıa Laparosc´opica. ´ Oscar Garc´ıa Grasa Thesis Advisor: Jos´e Mar´ıa Mart´ınez Montiel Instituto de Investigaci´on en Ingenier´ıa de Arag´on (I3A) Universidad de Zaragoza January 2014 En memoria de mi t´ıo Eduardo. Agradecimientos Durante el desarrollo de esta tesis muchas personas han estado a mi lado ayund´andome, apoy´andome y anim´andome en los momentos m´as dif´ıciles tanto personales como profesionales. Aqu´ı va la lista de todos ellos. Espero no olvidarme de nadie. Jos´e Mar´ıa Mart´ınez Montiel. Confiaste en mi para dirigirme la tesis sin conocerme de nada. Al principio tu quer´ıas una tesis rob´otica, pero finalmente te redirig´ı al mundo cl´ınico. Gracias por permitirlo y por aportar todos tus conocimientos y tu experiencia en este nuevo campo. Espero que esta nueva l´ınea de investigaci´on, que hemos abierto en el departamento, te sea muy fruct´ıfera. Ernesto Bernal, Santiago Casado e Ismael Gil. Sin vuestro trabajo los datos experimentales nunca se habr´ıan podido obtener. Gracias por aceptarme como uno m´as y por iluminarme con vuestro conocimiento tanto dentro como fuera de quir´ofano. Me hab´eis demostrado lo gran profesionales y magn´ıficas personas que sois. Ojal´a todo el mundo fuese igual de responsable y competente. Javier Civera. Me ayudaste a entender el funcionamiento del EKFSLAM soport´andome durante los primeros a˜nos de doctorado. Gran parte de los resultados de esta tesis te los debo a ti. V´ıctor F. Mu˜noz. Gracias por aportar la primera secuencia laparosc´opica con la cual se comenz´o a trabajar. Antonio G¨uemes, Miguel ´ Angel Bielsa y F´elix Lamata. Vosotros me facilit´asteis mi primer acceso a un quir´ofano. Gracias a ello pude realizar mi primera captura de una intervenci´on laparosc´opica completa. Andrew Davison, Ian Reid y Brian Williams. Gracias tanto por el software inicial de SLAM monocular como por el de relocalizaci´on. A todos mis compa˜neros del grupo de rob´otica. Vosotros me hab´eis hecho m´as llevaderos estos a˜nos gracias a los ratos vividos durante las comidas y a las “sesiones de terapia” en los caf´es. Marta Salas. Dicen que un amigo es un tesoro. Espero que yo forme parte de tu fortuna igual que tu formas parte de la m´ıa. Ojal´a no te pierda nunca como amiga aunque el tiempo y la distancia se interpongan en nuestras vidas. Gracias por ayudarme siempre que te lo he pedido y por contagiarme tu afici´on a la fotograf´ıa. Est´ıbaliz Fraca. Si la riqueza en el mundo se midiese por el tama˜no del coraz´on y el buen hacer de la gente, tu formar´ıas parte de ese 1% de la poblaci´on que controla el 40% de la riqueza mundial. Gracias por permitirme conocerte. T´u siempre has estado ah´ı, disponible para lo que yo necesitase. Bel´en Masi´a. Pusiste en marcha el algoritmo de relocalizaci´on durante tu PFC y has revisado mi ingl´es siempre que te lo he pedido sin poner ninguna objeci´on. M´as a´un, te ofrec´ıas voluntaria para hacerlo. Tu PFC me permiti´o descubrir a una maravillosa persona que hizo que me prendara de su forma de ser y de pensar. Sigue siendo como eres, igual de madura, formal y responsable. Eres una de esas personas que me doler´ıa perder como amiga. A mis amigos de la infancia, de la universidad y del dotorado. Nombraros a todos me llevar´ıa un par de p´aginas. A todos vosotros gracias por los buenos ratos vividos que hicieron que me evadiese del doctorado y de mis problemas personales. Ahora que voy a ser libre, espero vivir muchos m´as. A toda mi familia, por estar siempre ah´ı aguantando y apoyando, y sobre todo a mis padres por todos los duros momentos que nos han tocado vivir durante el desarrollo de esta tesis. Finalmente, al proyecto espa˜nol FIT-360005-2007-9, al proyecto europeo RAWSEEDS (FP6-IST-045144), y al proyecto del ministerio de ciencia e innovaci´on DPI2009-07130 por aportar la financiaci´on necesaria para el desarrollo de este trabajo. Contents 1 Introduction 1 1.1 Laparoscopy............................ 1 1.2 SLAM............................... 3 1.3 The aim: Laparoscopy as a monocular SLAM problem . . . . 4 1.4 RelatedWork........................... 5 1.5 Contributions of this Thesis . . . . . . . . . . . . . . . . . . . 7 2 Monocular EKF-SLAM 11 2.1 SLAMMethods.......................... 12 2.1.1 Keyframe Methods . . . . . . . . . . . . . . . . . . . . 12 2.1.2 Filtering Methods . . . . . . . . . . . . . . . . . . . . 13 2.2 Monocular EKF-SLAM . . . . . . . . . . . . . . . . . . . . . 14 2.2.1 State Vector Definition . . . . . . . . . . . . . . . . . . 16 2.2.2 Dynamic Model . . . . . . . . . . . . . . . . . . . . . . 18 2.2.3 Measurement Model . . . . . . . . . . . . . . . . . . . 19 2.2.4 Data Association & Map Management . . . . . . . . . 20 2.3 Robust Data Association: JCBB . . . . . . . . . . . . . . . . 22 2.4 SLAM Capabilities . . . . . . . . . . . . . . . . . . . . . . . . 24 2.4.1 Distance Measurement . . . . . . . . . . . . . . . . . . 24 2.4.2 Augmented Reality . . . . . . . . . . . . . . . . . . . . 26 2.4.3 Photorealistic Reconstruction . . . . . . . . . . . . . . 26 2.5 EKF-SLAM in Laparoscopy. A Proof of Concept . . . . . . . 28 2.5.1 Image Processing . . . . . . . . . . . . . . . . . . . . . 28 2.5.2 Experimental Results . . . . . . . . . . . . . . . . . . 30 xiii xiv 2.6 Conclusions............................ 32 3 Robust Monocular SLAM 37 3.1 Relocalization........................... 38 3.2 1-PointRANSAC......................... 42 3.2.1 RelatedWork....................... 45 3.2.2 1-PR EKF Algorithm . . . . . . . . . . . . . . . . . . 48 3.2.3 1-PR EKF Exhaustive Algorithm . . . . . . . . . . . . 55 3.2.4 Experimental Validation: Benchmark Method for 6 DoF Camera Motion Estimation . . . . . . . . . . . . 56 3.2.5 Experimental Validation: Monocular EKF-Based Estimation for Long Outdoor Sequences . . . . . . . . . . 65 3.3 Laparoscopic Experiments . . . . . . . . . . . . . . . . . . . . 68 3.3.1 Results .......................... 69 3.4 Conclusions............................ 75 3.4.1 1-Point RANSAC . . . . . . . . . . . . . . . . . . . . . 75 3.4.2 Laparoscopic Experiments . . . . . . . . . . . . . . . . 77 4 Exhaustive System Validation 79 4.1 Ventral Hernia Repair Procedure . . . . . . . . . . . . . . . . 80 4.2 Hernia Repair SLAM Assisted Procedure . . . . . . . . . . . 82 4.3 Simulation............................. 85 4.4 Experimental Validation Description . . . . . . . . . . . . . . 90 4.4.1 Data Acquisition . . . . . . . . . . . . . . . . . . . . . 90 4.5 SLAM Engineering Validation . . . . . . . . . . . . . . . . . . 94 4.6 Clinical Validation . . . . . . . . . . . . . . . . . . . . . . . . 96 4.6.1 Surgical procedure . . . . . . . . . . . . . . . . . . . . 99 4.6.2 Results .......................... 100 4.7 Conclusions and Future Work . . . . . . . . . . . . . . . . . . 101 4.7.1 Engineering Validation . . . . . . . . . . . . . . . . . . 103 4.7.2 Clinical Validation . . . . . . . . . . . . . . . . . . . . 104 5 Conclusions and Future Work 105 5.1 Conclusions............................ 105 5.2 FutureWork ........................... 107 5.3 Conclusiones ........................... 108 5.4 TrabajoFuturo.......................... 110 List of Figures 2.1 Inverse Depth parametrization . . . . . . . . . . . . . . . . . 17 2.2 EKF Individual Compatible Matching . . . . . . . . . . . . . 21 2.3 Example of JCBB working . . . . . . . . . . . . . . . . . . . . 24 2.4 Pattern measurement. Red arrow corresponds with the reconstruction scale. Cyan arrows correspond with the dimensions tobemeasured........................... 26 2.5 Steps for photorealistic reconstruction . . . . . . . . . . . . . 27 2.6 Color frame and its decomposition in color channels along with their corresponding Fourier spectrums. The red channel (2.6b) has very light areas with small contrast (no high frequency details), as shown by its Fourier spectrum (2.6e). On the contrary, green (2.6c) and blue channels preserve more high frequency details (2.6f, 2.6g). Visually, the green channel seems to have more contrast than the blue. . . . . . . . . . . . . . . 29 2.7 Feature extraction and reflections . . . . . . . . . . . . . . . . 30 2.8 Experiment with a hand-held laparoscope sequence of an abdominal cavity exploration (341 frames) . . . . . . . . . . . . 31 2.9 Experiment with a hand-held laparoscope sequence of an abdominal cavity exploration (186 frames) . . . . . . . . . . . . 33 2.10 Map size, inliers and outliers from a real laparoscopic sequence 35 3.1 Hypotheses selection in RLR . . . . . . . . . . . . . . . . . . 39 3.2 RandomisedList ......................... 39 3.3 Example of relocalization in laparoscopy (I) . . . . . . . . . . 41 xv xvi LIST OF FIGURES 3.4 Example of relocalization in laparoscopy (II) . . . . . . . . . 42 3.5 RANSAC steps for 2D line estimation . . . . . . . . . . . . . 44 3.6 1-Point RANSAC steps for 2D line estimation . . . . . . . . . 45 3.7 1-Point RANSAC stages (I) . . . . . . . . . . . . . . . . . . . 50 3.8 1-Point RANSAC stages (II) . . . . . . . . . . . . . . . . . . 53 3.9 Benchmarking method based on Bundle Adjustment . . . . . 58 3.10 Camera location error for different RANSAC configurations . 60 3.11 Number of iterations for 5-PR and 1-PR . . . . . . . . . . . . 61 3.12 Camera location error for different JCBB configurations . . . 63 3.13 Cost and map sizes for RANSAC and JCBB . . . . . . . . . . 64 3.14 Spurious match rate for JCBB and RANSAC . . . . . . . . . 65 3.15 Histograms of the errors for three experiments . . . . . . . . 66 3.16 Estimated trajectories from monocular data and GPS data . 67 3.17 Map size, number of inliers and number of outliers in EKF + 1-PR + RLR laparoscopic example . . . . . . . . . . . . . . . 70 3.18 Deformations as outliers . . . . . . . . . . . . . . . . . . . . . 70 3.19 Computation time budget and map size . . . . . . . . . . . . 71 3.20 Histogram showing the computational cost . . . . . . . . . . . 72 3.21 Histogram displaying feature persistence . . . . . . . . . . . . 73 3.22 Long-term features . . . . . . . . . . . . . . . . . . . . . . . . 73 3.23 Cycle time and map size corresponding to operation Figure 4.9c 74 3.24 Cycle times and outliers . . . . . . . . . . . . . . . . . . . . . 74 4.1 Intra-abdominal pressure and prosthetic mesh . . . . . . . . . 81 4.2 Hernia defect measurement methods . . . . . . . . . . . . . . 82 4.3 SLAM measurement process of the hernia defect (I) . . . . . 83 4.4 SLAM measurement process of the hernia defect (II) . . . . . 84 4.5 Simulation of a laparoscopic operation . . . . . . . . . . . . . 86 4.6 Laparoscopic 30ooptics ..................... 86 4.7 Estimation camera error for the simulation results . . . . . . 87 4.8 Estimation error for the simulation results . . . . . . . . . . . 88 4.9 Thumbnails from 15 ventral hernia repairs . . . . . . . . . . . 91 4.10 External measurements . . . . . . . . . . . . . . . . . . . . . 93 4.11 Procedure to take the calibration images . . . . . . . . . . . . 94 4.12 Measurement procedure comparison among two methods . . . 96 4.13 Comparison between ground-truth internal measurements and SLAM measurements (I) . . . . . . . . . . . . . . . . . . . . . 97 4.14 Comparison between ground-truth internal measurements and SLAM measurements (II) . . . . . . . . . . . . . . . . . . . . 98 4.15 Comparison between ground-truth internal measurements and SLAM measurements (III) . . . . . . . . . . . . . . . . . . . . 98 LIST OF FIGURES xvii 4.16 Trocar locations in the left flank . . . . . . . . . . . . . . . . 99 4.17 Measurement procedure comparison among 3 methods . . . . 102 1 Introduction 1.1 Laparoscopy Laparoscopic surgery is a modern technique of minimally invasive surgery (MIS). In this technique, operations inside the abdominal cavity are performed with small incisions (5-10 mm) through the abdominal wall in contrast with large incisions of classical open surgery (laparotomy). Laparoscopy requires of a telescopic rod lens system connected to a videocamera (endoscope or laparoscope) that gathers images of the abdominal cavity. These images are displayed on some monitor (TV, computer screen) and used by surgeons during the operation in order to see the interior of the abdomen. Since the abdominal cavity is dark, attached to the lens is a fiber optic cable system connected to a “cold” light source (halogen or xenon) that illuminates the operative field. The abdominal wall has to be separated from the internal organs, and then the abdomen is insufflated with carbon dioxide gas (CO2) and blown up like a balloon creating a workspace called pneumoperitoneum. CO2is used because it is a gas produced by the human body, therefore, it is easily absorbed by tissues and removed by the respiratory system. Additionally, CO2is non-flammable, which is important because electrosurgical devices are commonly used in laparoscopic procedures. Finally, both camera and tools are inserted in the abdominal cavity through 5-10 mm cannula-shaped input ports called trocars. Despite the incisions of the input ports being small, laparoscopy has its own risks. Precisely the most important ones are caused by trocars. During their insertion, they can damage internal organs (small or large bowel) causing perforations and peritonitis, or penetrate blood vessels causing vascular injuries such as hematomas or hemorrhages that may be life-threatening. 1 21. Introduction Electrosurgical tools can cause electrical burns that can lead to organ perforations and even peritonitis. Patients with existing pulmonary or heart disorders may not tolerate pneumoperitoneum resulting in a need for conversion to open surgery after the initial attempt at the laparoscopic approach. Besides, the pressure exerted by CO2may cause difficulties in the venous return and increase the cardiac output making this surgery more dangerous for this type of patients. Finally, since not all of the CO2introduced into the abdominal cavity is removed through the incisions, it tends to rise and push the diaphragm, muscle that separates the abdominal cavity from the thoracic cavity and facilitates breathing, putting pressure on the phrenic nerve and causing sensation of pain that disappears as CO2is eliminated through respiration. Nevertheless, these risks are thoroughly compensated with the advantages of this kind of surgery versus laparotomy. Although there exists a minimal risk of hemorrhages, this is much lower than in laparotomy, reducing the chance of needing a blood transfusion. Smaller incisions reduce muscular injuries in the abdominal wall resulting in a lesser post-operative aesthetic impact, and a lower risk of wound infections and pain; therefore, less antibiotics and pain medication are needed. Smaller incisions also shorten recovery time, often with a same day discharge, which leads to a faster return to everyday living. Besides, reduced exposure of internal organs to possible external contaminants decreases the risk of acquiring infections. From the surgeons’ perspective, laparoscopy presents several disadvantages with respect to laparotomy. The limited range of motion of surgical tools, that results in a loss of dexterity; the tool tip that moves in the opposite direction to the surgeon’s hands, due to it pivoting around the fulcrum (entering point in the abdominal cavity), making laparoscopic surgery nonintuitive; and the indirect manipulation of tissues through the laparoscopic tools, that results in a reduction of tactile sensation making diagnosis of tissues tissues by feeling more difficult (e.g. to detect tumors), and perform delicate procedures such as suturing, make the learning curve be complex and require to make a great effort to learn this technique. Additionally, since the operations are usually performed through monocular images (the majority of endoscopes are monocular), surgeons are faced with the loss of depth perception. Besides, the endoscope Field of View (FoV) is limited and not all the operative field is visible at each moment, as a consequence, surgeons require a deep knowledge of the patient’s anatomy. This knowledge is also required when surgeons need to remove critical structures such as tumors or work in critical areas such as areas near vital blood vessels. The first use of a laparoscope was performed in a dog by Georg Kelling 1.2. SLAM 3 in 1901. In 1910, Hans Christian Jacobaeus, based on Kelling’s works, reported the first human laparoscopic human intervention [Hat+06]. However, the use of laparoscopy was very limited, only diagnosis and performance of simple gynecologic procedures were performed, until 1975 when Tarasconi performed the first organ resection (salpingectomy) first reported in the Third AAGL (American Association of Gynecologic Laparoscopist) Meeting in 1976, and later published in [Tar81]. Nowadays, laparoscopy is very extended and practically any abdominal or pelvic surgery can be performed with this technique (cholecystectomies, hepatectomies, bowel resections, hernia repairs, ...). The present and future of laparoscopy is strongly tied to the developments in computer vision and robotics which ease the dexterity and improve the depth perception, the FoV, and the tactile sensation by means of robots, stereo endoscopes, SLAM algorithms or haptic interfaces. It is worth to mention the DaVinci System as an example of these developments. 1.2 SLAM Simultaneous Localization And Mapping (SLAM) is a classical problem and one of the most researched topics in mobile robotics. Given a mobile sensor moving along an unknown trajectory in an unknown environment, SLAM is able to simultaneously estimate both the environment structure (a 3D map of the environment) and the sensor location with respect to that map. This estimation process is carried out taking the information gathered by the sensor as the sole input data to the SLAM algorithm. Additionally, SLAM is usually required to work in real-time at frame rate. In the most typical SLAM problem, sensory information comes from proprioceptive sensors –odometry or inertial measurement units– and exteroceptive sensors, that measure entities external to the robot. Traditionally, laser has been the predominant exteroceptive sensor used in SLAM, although other sensors, like sonars, have also been used. It is only very recently that cameras have been adopted massively by the robotic community as the main SLAM sensor. In this thesis, a monocular camera (laparoscope) is used as the unique sensor and full 3D SLAM is performed. This configuration is usually named monocular SLAM. The monocular SLAM problem is particularly challenging because only a sequence of 2D projections of a 3D scene is available; in any case, 30 Hz real-time systems estimating up-to-scale 3D camera motions and maps of 3D points using commodity cameras and computers are widely available for mobile robotics environments nowadays. All these systems provide extensive experimental validation of the SLAM algorithms and real-time performance 41. Introduction for man-made, mainly rigid, scenes which are typical in mobile robotics. Monocular SLAM has been possible thanks to intense research on salient feature detection and description [Can86; HS88; ST94; Low04; RD05] and spurious rejection [FB81; NT01] which has provided with an automated way of robustly matching features along images. Finally, it has been tackled from two radically different approaches. On the one hand, algorithms based on keyframes (keyframe methods) which try to adapt traditional pairwise offline Stucture form Motion (SfM) + Bundle Adjustment (BA) methods to achieve sequential online estimation. In order to do so, a sparse set of keyframes is chosen and SfM methods and BA are applied over a subset of them that are close to the current frame. One of the best performers in this vein is the Parallel Tracking And Mapping (PTAM) algorithm proposed by Klein and Murray [KM07]. On the other hand, filtering methods which apply Bayesian filtering techniques that, at each step, integrate the information from the current frame into a multidimensional probability distribution that summarizes the information gathered for all previous frames along the sequence. Davison was the first to demonstrate real-time performance with this approach using an Extended Kalman Filter (EKF) [Dav03; Dav+07]. 1.3 The aim: Laparoscopy as a monocular SLAM problem The main goal of this thesis is to demonstrate the feasibility of applying SLAM algorithms, that come from robotics and computer vision and that recover the 3D of a scene and the camera motion in real time, to laparoscopic environments. Laparoscopy can be posed as a monocular SLAM problem. In laparoscopy, a surgeon explores the abdominal cavity by pivoting the laparoscope around the fulcrum. Then, the tip of the laparoscope moves inside the abdomen gathering images from this cavity. SLAM algorithms enable to estimate an up-to-scale 3D map of the observed cavity from these images without resorting to any additional sensor such as optical or magnetic trackers, accelerometers, structured light, or artificial landmarks. Furthermore, it is worth noting that SLAM not only recovers the 3D model, but also the actual trajectory followed by the laparoscope. SLAM opens new interesting possibilities inside the surgical field. The knowledge of the laparoscope location with respect to the abdominal cavity enables to do augmented reality annotations directly on laparoscopic images (e.g. alignment with preoperative 3D CT models, signal critical regions like blood vessels, or provide other additional information); intracavity 3D dis- 2 Monocular EKF-SLAM In the last years, SLAM research has focused on monocular cameras as the unique sensorial input, giving origin to monocular SLAM methods. 30 Hz real-time systems estimating full 3D camera motions and maps of 3D points using commodity cameras and computers have been reported [Dav03; KM07; ED08]. Traditionally, there have been two different approaches to monocular SLAM: keyframe methods and Bayesian filtering methods. They are briefly summarized in Section 2.1. The main goal of this thesis is to demonstrate that these methods can be applied in laparoscopic surgery. Thus, without loss of generality, the monocular Extended Kalman Filter SLAM (EKF-SLAM) proposed in [Dav03; CDM08] has been chosen as a basic demonstrator of the feasibility of this type of algorithms in laparoscopy. The original implementation of monocular EKF-SLAM, proposed by Davison [Dav03], encodes the map features in a 3D vector which represents the world point localization (Euclidean parametrization). However, this parametrization suffers from large linearization errors when there are points that have been seen with low parallax. Since EKF estimation strongly depends on measurement model linearity, bad linearizations will produce a degradation of this estimation. The inverse depth (ID) parametrization, proposed by Civera et al. [CDM08], improves this situation, however, it increases the size map, and hence increments the computational cost of the estimation. Therefore, the best way to operate with monocular EKF-SLAM is to encode low parallax features in ID and, as the estimation evolves and they are seen with enough parallax, convert them to Euclidean. Section 2.2 details the monocular EKF-SLAM, the Euclidean parametrization and the ID parametrization and its conversion to Euclidean. 11 12 2. Monocular EKF-SLAM Additionally, the EKF update stage assumes a perfect data association. This, however, is not true and just one spurious match may wreck the estimation. Joint Compatibility Branch and Bound (JCBB), detailed in Section 2.3, is a state-of-the-art algorithm for data association in EKF-SLAM. This algorithm detects and removes the spurious measurements before the EKF update, improving the robustness of SLAM. Finally, monocular SLAM recovers a sparse 3D map of the scene and the camera motion along with the corresponding covariances. Both the map and the motion can be used as a geometrical backbone to support useful information such as 3D distance measurements, augmented reality (AR) insertions or photorealistic reconstructions (Section 2.4). The first contribution of this thesis is a proof-of-concept, reported in [GG+09b; GG+09a], that proves the feasibility of using monocular visual SLAM algorithms in laparoscopic surgery showing its potential in this surgical field (Section 2.5). It is worth noting that this contribution, up to the author’s knowledge, is the first one which applies monocular visual SLAM algorithms in real laparoscopic human surgeries. 2.1 SLAM Methods Monocular SLAM has been tackled by means of methods based on keyframes, which try to adapt traditional pairwise offline Stucture form Motion (SfM) + Bundle Adjustment (BA) in order to achieve sequential online estimation, or by means of methods based on filtering, which apply Bayesian filtering techniques that, at each step, integrate the information from the current frame into a multidimensional probability distribution which summarizes the information gathered for all previous frames along the sequence. 2.1.1 Keyframe Methods Keyframe methods are strongly related to SfM methods, whose origins can be traced back to the so-called Photogrammetry (second half of 19th century). Photogrammetry aims to extract geometric information from images. Initially, it started with a set of features manually identified by the user, and then applied non-linear optimization techniques, known as BA [MBM01], to minimize the reprojection error. Nowadays, research on computer vision has allowed to achieve complete automation by assuming rigidity in the scene and by automatizing the feature extraction, matching, and spurious detection between images. SfM has been usually processed by pairwise geometry algorithms [HZ04] 2.1. SLAM Methods 13 and refined by global optimization procedures (BA) [Tri+00] in order to minimize the reprojection error and refine the estimation into a globally consistent one. One of the main drawbacks of the traditional SfM + BA combination is its incapacity to deal with long sequences of images, since it was initially thought for the processing of sparse sets of images (SfM relates pairs –at most triplets– of images, but lacks a global formulation for an image stream). Recently, there has been significant novel research in the field aiming to extend the capabilities of SfM methods to sequentially process large image sequences, the estimation being carried out in real-time [KM07; Mou+09]. In this vein, one of the best performers in visual SLAM is the Parallel Tracking And Mapping (PTAM) algorithm proposed by Klein and Murray [KM07]. This keyframe-based algorithm makes use of two parallel processing threads. The first one performs camera tracking assuming a known map of natural features at 30 fps. The second constructs a consistent map performing global BA over selected frames of the sequence which summarize the whole sequence. 2.1.2 Filtering Methods Filtering methods are based on Bayes filters [TBF05]. They estimate recursively a probability distribution function over the unknown parameters (camera location and 3D map of the scene) of a state vector from measurements gathered by a sensor (camera) and the dynamic and measurement models in two steps: prediction, and update. In the first one, the probability distribution function for the frame at time instant k−1 is projected into the frame at time instant kbased on the probabilistic dynamic model of the system. In the update stage, measurements are collected, according to the measurement model known in advance, and fused with the probability distribution function from the prediction step using Bayes’ rule. The key difference between filtering methods and keyframe ones is that they do not operate in a pairwise manner –estimating location from one frame with respect to another– nor do they pile up measurements waiting for a BA optimization. Instead of that, the overall state of the system is summarized into a multidimensional probability distribution, which is updated as new measurements arrive and their information is integrated in it. Therefore, its computational complexity scales with the size of the state and not with the number of frames, being naturally suited for the processing of large streams of measurements. Historically, the Extended Kalman Filter (EKF), the non-linear version of the Kalman Filter, was the first filtering technique to offer a solution to the SLAM task (EKF-SLAM) [SSC87; Dis+01; Cas+99], and also the first one to demonstrate real-time performance in visual SLAM [Dav03; Dav+07]. 14 2. Monocular EKF-SLAM In addition to EKF, other different filters have been proposed trying to relax the EKF assumptions, but generally incurring in higher computational cost: the Unscented Kalman Filter [JU97], Particle Filters [Mon+02], and Sum of Gaussians Filter [DW+03]. Particle Filters and the Unscented Kalman Filter have been used for visual SLAM in [ED06] and [HKM09], respectively. One of the drawbacks of EKF-SLAM is its quadratic computational cost with respect to the state size. SLAM research has pursued to reduce this computational cost, leading to interesting results. Information filters have taken advantage of the sparsity of the problem when presented in information form –dual of the covariance form–, e.g. [Thr+04; ESL05]. Submappingbased techniques have also been developed to cope with the complexity of the filtering-based SLAM estimation [ENT05]. EKF-based submapping has been applied to the visual estimation case in [Cle+07; Paz+08; PT08]. In spite of the quadratic computational cost, EKF filtering has been chosen in this thesis because it is well known, mature, and performs well in small environments where it reaches real time. Thus, an improved version of Davison’s work [Dav03; Dav+07] will be used as a starting point. This version will be robustified by combining EKF with RANSAC and will be adapted and validated to work over real human laparoscopic sequences in the next chapters. 2.2 Monocular EKF-SLAM EKF, the first Bayesian filtering solution successfully applied to the SLAM estimation problem, was proposed for monocular visual SLAM in [Dav03]. This algorithm estimates recursively a probability distribution function over the unknown parameters of a state vector xfrom measurements gathered by a sensor and the dynamic and measurement models in two steps: prediction, and update. In the first one, the probability distribution function p(xk−1) from step k−1 is projected into step kbased on the probabilistic dynamic model of the system pxk|k−1|xk−1|k−1,uk–equation (2.1). In the update stage, measurements zkare collected, according to the measurement model pzk|xk|k−1, known in advance, and fused with the probability distribution function from prediction step using the Bayes’ rule –equation (2.2). pxk|k−1=Zpxk|k−1|xk−1|k−1,ukp(xk−1)dxk−1(2.1) pxk|k=η p zk|xk|k−1pxk|k−1.(2.2) ηcorresponds to the normalization constant that converts pxk|kinto a probability distribution function. It is worth noting that in order to com- 2.2. Monocular EKF-SLAM 15 pute the probability distribution recursively, the algorithm requires the initial probability distribution over x,p(x0). In the case of the Kalman Filter, it is assumed that dynamic and measurement models are linear and represented by multivariate normal distributions –equation (2.3). Under these assumptions and knowing the initial Gaussian probability distribution at time k= 0, the a posteriori distribution over the estimated parameters is Gaussian and thus, it may be represented by its mean and covariance x∼ N (ˆx,P). However, most of the real systems are not linear but show some degree of linearity. The EKF relaxes the linearity assumption by linearizing the dynamic and measurement models in the mean value at every step of the estimation. Hence, the more linear both models are, the better the EKF estimation is. It is worth noting that the EKF does not give the real a posteriori probability distribution function, but only a Gaussian approximation. p(x) = det (2πΣ)−1 2exp{−1 2(x−µ)>Σ−1(x−µ)},(2.3) The EKF prediction and update stages involve working with the mean ˆ x and the covariance Pof the state. The prediction equations are: ˆxk|k−1=fk(ˆxk−1|k−1,ˆuk) (2.4) Pk|k−1=FkPk−1|k−1F> k+GkQkG> k(2.5) being fˆxk|k−1,ˆukthe non-linear equation modeling the dynamic evolution of the system; ukthe input given to the system (uk=0in monocular SLAM); Fkthe derivatives of the dynamic model with respect to the state vector (Fk=∂fk ∂x); Qkthe state noise covariance; and Gkthe derivatives of the dynamic model with respect to such noise (Gk=∂fk ∂nk), being nkthe state noise. The equations of the update state are: νk=zk−h(ˆxk|k−1) (2.6) Sk=HkPk|k−1H> k+Rk(2.7) Kk=Pk|k−1H> kS−1 k ˆxk|k=ˆxk|k−1+Kkνk(2.8) Pk|k= (I−KkHk)Pk|k−1(2.9) where zkare measurements gathered at step k;hˆxk|k−1the function that defines the sensor measurement model; Hkthe derivatives of the measurement function by the state vector (Hk=∂hk ∂x); Rkthe covariance of the 16 2. Monocular EKF-SLAM measurement noise; νkand Skthe filter innovation and its covariance; and Kkthe filter gain. Therefore, to compute the EKF estimation, it is mandatory to define the state vector (x), the dynamic model or state transition equations (f), and the measurement model (h). These definitions, for the case of the visual SLAM problem, are detailed in Sections 2.2.1, 2.2.2 and 2.2.3, respectively. 2.2.1 State Vector Definition In visual SLAM, the world map and the camera location are represented in a stochastic framework. This probabilistic representation at step kis coded in a unique state vector modeled as a multivariate Gaussian, xk: xk=x> v,y> 1,y> 2,· · · ,y> n>.(2.10) It is composed of the camera state, xv, and the map defined by the location of every point, yi. See Section 2.2.4 for map point management details. The camera state, xv, is formed by position, rW C , orientation encoded in a quaternion, qW C , and linear and angular velocities, vWand ωC. The map is composed of npoint features, (y> 1,· · · ,y> n)>, whose locations are encoded either in Euclidean coordinates, yi= (Xi, Yi, Zi)>, or in inverse depth (ID), yi= (xi, yi, zi, θi, φi, ρi)>. The original monocular EKF-SLAM by Davison uses only Euclidean parametrization, which suffers from large linearization errors in the measurement model at low parallax. In order not to degrade the EKF estimation, low parallax features (features whose depth is much bigger than camera translation or recently initialized features which, even if close to the camera, produce low parallax) must be treated separately from the main map until there is enough information to insert them into the filter (delayed initialization). In the presence of this situation, the system initialization requires an initial known map obtained from a pattern. Low parallax features are important because, although they cannot be used to estimate camera translation, they contribute to the estimation of orientation, and hence to improve the EKF performance. Unlike Euclidean point coding, ID point coding [CDM08] improves the measurement linearity at low parallax. As a result, ID improves EKF performance, even for maps only composed of close features, by taking into account low parallax features. Besides, it avoids the use of a pattern during system initialization and the delayed feature initialization by immediate insertion of new features into the main map. 2.2. Monocular EKF-SLAM 17 An ID feature is a 6 parameter vector: yi= (xi, yi, zi, θi, φi, ρi)>(2.11) where xi, yi, zicorrespond to camera location when the point was observed for the first time, and θiand φiare azimuth and elevation angles which define the ray unit vector m(θi, φi). Point depth is coded by its inverse ρi= 1/di, so a point world location xiis (Figure 2.1): xi=   Xi Yi Zi   =   xi yi zi   +1 ρi m(θi, φi) (2.12) m(θi, φi) = (cos φisin θi,−sin φi,cos φicos θi)>(2.13) Figure 2.1: Camera-(1) defines the world frame, W. A feature is observed for the first time by camera-(i), the feature world location is defined with respect to the camera-(i) pose, (xi, yi, zi)>, using the distance between camera-(i) and the feature, di= 1/ρi, and a unit directional vector, m(θi, φi), defined by its azimuth and elevation angles. The αangle stands for the feature parallax between camera-(i) and camera-(j) which can be computed with m(θi, φi) and hWdirectional vectors, both of them defined in the world frame. The only drawback of ID is that it doubles the size of each feature vector (it needs a 6D vector versus the 3D vector required by Euclidean parametrization) affecting directly to the computational cost of the EKF update. In 18 2. Monocular EKF-SLAM order to obtain accuracy without compromising the computational cost, the use of ID is restricted to low parallax map features converting them to Euclidean when they are seen with enough parallax. The parallax of a feature is defined as the angle αbetween the directional vector m(θi, φi) when the feature was initialized and the vector hW, in the world frame, that joins the current camera with the feature (Figure 2.1). The criterion for conversion is determined by the next linearity index (Ld): Ld=4σd khWk|cos α|(2.14) σd=σρ ρ2 i ,cos α=m>hW khWk(2.15) σρ=qPyi(6,6),hW=xi−rW C j(2.16) where ρiand σρare the feature inverse depth and its standard deviation obtained from the state vector and the feature covariance matrix; mcorresponds to Equation 2.13; and hWis obtained from the 3D current camera (rW C j) and feature (2.12) world positions. After each EKF estimation, Ld is computed for all ID features and those whose Ld<10% are converted to Euclidean encoding. 2.2.2 Dynamic Model Regarding the state transition equation for the camera, a dynamic constant velocity model that encodes its smooth motion is proposed: fv=     rW C k+1 qW C k+1 vW k+1 ωC k+1      =      rW C k+vW k+VW k∆t qW C k×qωC k+ ΩC∆t vW k+VW ωC k+ ΩC       (2.17) where qωC k+ ΩC∆tis the quaternion defined by the rotation vector ωC k+ ΩC∆t. The state noise vector (n) is assumed to be composed of linear, aW, and angular acceleration, αC, acting as inputs producing, at each step, an impulse of linear velocity, VW=aW∆t, and angular velocity ΩC=αC∆t. Both of them are modeled as zero mean Gaussian processes with known covariance, diag (QaW,QαC). Regarding the state transition equation for the scene points, a static model with zero state noise to encode the scene as perfectly rigid is proposed: 2.2. Monocular EKF-SLAM 19 yik+1 =yik.(2.18) The complete dynamic model (fk) is the stacking of (2.17) and an instance of (2.18) for each map point. The final state noise (nk) is assumed to be a zero mean multivariate normal distribution with known covariance Qk= diag QaW,QαC,01,· · · ,0n, where each 0icorresponds to the i-th map point. 2.2.3 Measurement Model The measurements, zk=h(xk), are provided by a pinhole camera: h= u v!=  u0−f dx hC x hC z v0−f dy hC y hC z  (2.19) where u, v are the pixel coordinates of the observation in the image. u0, v0, f, dx, dyare the camera intrinsic parameters corresponding to the principal point, the focal length, and the pixel size. hC=hC x, hC y, hC z>is the vector joining the current camera location with the observed map feature, expressed in the camera frame which. For Euclidean parametrization it is: hC=RCW    Xi Yi Zi −rW C   .(2.20) In the case of ID, (Xi, Yi, Zi)>in Equation (2.20) are replaced by Equation (2.12). Equation 2.19 gives the 2D image coordinates assuming a pure projective model. Therefore, in order to compensate the lens radial distortion, a twoparameter distortion model [MBM01] is applied. In this model, the ideal projective coordinates h= (u, v)>are recovered from the real distorted ones hd= (ud, vd)>: h= u0+ (ud−u0)(1 + κ1r2 d+κ2r4 d) v0+ (vd−v0)(1 + κ1r2 d+κ2r4 d)!(2.21) rd=q(dx(ud−u0))2+ (dy(vd−v0))2(2.22) where κ1and κ2are the radial distortion coefficients. The distorted coordinates are computed from the ideal ones as follows: 20 2. Monocular EKF-SLAM hd=  u0+(u−u0) (1+κ1r2 d+κ2r4 d) v0+(v−v0) (1+κ1r2 d+κ2r4 d) (2.23) r=rd(1 + κ1r2 d+κ2r4 d) (2.24) r=q(dx(u−u0))2+ (dy(v−v0))2(2.25) Notice that ris available from (2.19, 2.25), but rdmust be numerically solved from (2.24). Finally, Equation (2.23) is used to compute the distorted point. Regarding the covariance of the measurement noise (Rk), it corresponds to the image measurement error covariance and is assumed to be a diagonal matrix. 2.2.4 Data Association & Map Management The EKF prediction ˆxk|k−1(2.4, 2.5), provides a prior over the current pose which is used to restrict the search of visual feature correspondences. This is known as active search and has two advantages due to the limited search area. First, it allows the system to run in real time, and second, it reduces the chance of spurious matches. Additionally, the EKF prediction also provides an estimate for the relative pose of every map point with respect to the camera. This prediction is accurate enough to synthesize in a patch the point image appearance, compensating for rotation and scale variations along the sequence. Therefore, the combination of the FAST feature extractor [RD05] and simple patch correlation is used to extract and recognize the map features because it is cheap and performs satisfactorily. Besides, this combination is favored in the particular case of laparoscopy where, due to the small depth variation of the abdominal cavity and the limited laparoscope movements (it only pivots and slides over the fulcrum), features do not undergo severe perspective changes. According with this, each map point is identified by an 11 ×11 pixel planar texture patch extracted when the point is first observed, being unnecessary to resort to expensive invariant descriptors and extractors which would be overkill in SLAM. Data association is performed by means of active search and synthesized point patches. Then, every map point is exhaustively searched inside the elliptical region defined by its innovation (2.6, 2.7) on the current image by means of normalized correlation with its synthesized patch (Figure 2.2). The pixel scoring highest, zi, if over a threshold, is selected as the match in the 2.4. SLAM Capabilities 27 that observe the complete corresponding triangle. Figure 2.5 sketches the photorealistic modeling process. Since triangulation is a live process –map points, and consequently triangles, are continuously created, erased and their estimates changed–, maintenance operations are performed to deal with new and deleted triangles as the SLAM estimation evolves, and to take textures from the images for the triangles. In the case of laparoscopy, this real-time photorealistic modeling process eases the 3D cavity visualization. The textured 3D model allows the synthesis of a panorama that expands the limited field of view (FoV) of the laparoscope. (a) Features in 3D, the current image and the world X-Y plane. (b) Features projected onto a X-Y plane and triangulation on this plane. (c) Triangulated image backprojection to obtain textures. (d) Final photorealistic reconstruction. Figure 2.5: Steps of the photorealistic reconstruction. 28 2. Monocular EKF-SLAM 2.5 EKF-SLAM in Laparoscopy. A Proof of Concept The first contribution of this thesis is to prove the feasibility of using monocular visual SLAM algorithms with real monocular laparoscopic sequences. In order to use SLAM, two assumptions have been made: the abdominal cavity is rigid, and the laparoscope undergoes a smooth and non-pure rotational motion. These conditions are fulfilled by a number of medical applications, such as laparoscopic ventral hernia repairs. The chosen algorithm is one of the leading-edge monocular SLAM algorithms (EKF + JCBB). SLAM has been applied over two laparoscopic abdominal exploration sequences. The primary result has been a sparse up-to-scale 3D map composed of salient points –features– of the observed cavity for each sequence. This SLAM map has shown to be adequate to support 3D distance measurements along with the measurement error. Additionally, the map has been used as a backbone for real-time photorealistic modeling to ease the 3D cavity visualization. The textured 3D model allows to synthesize a panorama that expands the laparoscope FoV. Finally, since the camera motion with respect to the 3D map is accurately known in real time, AR annotations can be supported live in medical sequences. 2.5.1 Image Processing Monocular SLAM in robotics uses a correlation score based on luminance, neglecting color information. As laparoscope captures color images, a procedure to convert color images to B&W must be applied. One of the quickest procedures is to select one out of the three channels that compose a color image. The channel selected must preserve high frequencies in order to ease the performance of the feature extractor (features are high frequency components). As can be seen in Figure 2.6, in contrast to the red channel, green and blue channels preserve high frequencies, and hence both channels are possible candidates. Finally, the green channel has been the preferred one because visually it seems to contain a richer contrast than the blue one and a nice texture to produce distinctive patches for recognition. On the other hand, human tissues are prone to produce reflections caused by illumination. These reflections can erroneously fire the feature extractor and then incorrect map points could be initialized. Additionally, as in laparoscopic scenes the light source is fixed to the laparoscope, when it is moved, the light source is also moved and reflections change producing erroneous data associations. In order to avoid both situations, reflections are removed 2.5. EKF-SLAM in Laparoscopy. A Proof of Concept 29 (a) Color (b) Red channel (c) Green channel (d) Blue channel (e) Red channel Fourier spectrum (f) Green channel Fourier spectrum (g) Blue channel Fourier spectrum Figure 2.6: Color frame and its decomposition in color channels along with their corresponding Fourier spectrums. The red channel (2.6b) has very light areas with small contrast (no high frequency details), as shown by its Fourier spectrum (2.6e). On the contrary, green (2.6c) and blue channels preserve more high frequency details (2.6f, 2.6g). Visually, the green channel seems to have more contrast than the blue. assuming that they produce pixels with a high luminance. If any pixel in a patch around a detected feature is over a threshold (200 over 255), this feature is rejected. Figure 2.7 shows a frame of a laparoscopic sequence. Figures 2.7a and 2.7b, show how specularities have been detected and initialized as map points. However, these specularity features have been rejected in Figures 2.7c and 2.7d. 30 2. Monocular EKF-SLAM (a) Color frame with specularities. (b) B&W frame with specularities. (c) Color frame without specularities. (d) B&W frame without specularities. Figure 2.7: Four figures corresponding to the same frame from the same sequence processed without reflection filtering (2.7a, 2.7b) and with reflection filtering (2.7c, 2.7d). In the unfiltered case, the feature detector has detected some specularities (white points) as valid features. These specularities are not present in the filtered case. 2.5.2 Experimental Results Experimental validation is performed on real images 360x288@25 Hz gathered from a hand-held monocular laparoscope observing two abdominal cavities (341 and 186 frames respectively). The goal of the validation is to prove the feasibility of using SLAM in laparoscopy. To this end, three experiments over these two sequences were carried out showing SLAM performing photorealistic reconstructions, distance measurements, and AR annotations. The sequences were the only data input to the algorithm, achieving realtime performance in all the experiments measuring up to 25 features. Laparoscope intrinsic parameters were calibrated using a standard planar pattern calibration method, based on Zhang’s initial solution [Zha00], followed by Bundle Adjustment. 2.5. EKF-SLAM in Laparoscopy. A Proof of Concept 31 (a) Some frames of a laparoscopic sequence of an abdominal cavity exploration (341 frames). (b) Photorealistic reconstruction with a measurement between two points of the organ. The scale is defined with two other organ points. (c) Historical evolution of the measurement and its error. Notice that the error reduction as the camera moves and gathers information from different points of view providing higher parallax. Figure 2.8: Hand-held laparoscope sequence of an abdominal cavity exploration (341 frames). In the case of the 341-frame sequence (Figure 2.8), from which several frames are shown in Figure 2.8a, a 3D distance measurement experiment was performed (Figure 2.8b). The sequence corresponds to a laparoscope 32 2. Monocular EKF-SLAM exploration inside the abdominal cavity. No tool was inserted; therefore, in order to make the distance measurement, two arbitrary points were marked as reference which define the unity of the scale factor (blue arrows in Figure 2.8b). Another two arbitrary points were selected and their relative distance was measured (the red arrow in Figure 2.8b). The distance along with its error were computed relative to the defined scale. Assuming the scale was the real scale, the conversion to real distances is immediate according to (2.31, 2.35). Figure 2.8c shows the estimate history both for the distance and the error. Initially, error uncertainty is large, but as the camera translates and the scene is seen with parallax, point location error decreases and consequently the distance error decreases too. Since the uncertainty is computed in real time, visual feedback gives the surgeon information on how to move the camera in order to reduce the distance error. This experiment can be found in the video [GGb]. In the case of the 186-frame sequence (Figure 2.9), where several frames are shown in Figure 2.9a, an AR reality experiment was carried out. The sequence corresponds to a laparoscopic abdominal exploration during a human ventral hernia repair. Since the 3D map and the camera location with respect to the map are available in real time, it is possible to anchor AR annotations to map points. Figure 2.9b shows an AR cylinder both in 3D and superimposed on the live laparoscopic image. As the virtual insertions are fixed to the map, they can be observed at their real location even when they are out of the camera FoV. This experiment can be found in the video [GGa]. For both sequences a textured triangular mesh model was obtained (Figures 2.8b and 2.9b). Despite the sparse map being composed of a reduced number of points, the 3D live photorealistic models provide an easy understanding of the 3D cavity structure. Videos [GGb] and [GGa] show both photorealistic reconstruction processes. Besides, video [GGd] shows in more detail the photorealistic reconstruction for the 186-frame sequence. It is worth noting that these experiments, corresponding with the first monocular SLAM experiments over real human laparoscopic sequences in the literature, along with the experiment of Figure 2.4, were crucial to speak with surgeons and to prepare an intervention where SLAM could be relevant and easily validated. 2.6 Conclusions Unlike previous works where human cavity reconstructions are obtained using fixed or moving stereo endoscopes, or monocular endoscopes are used on 2.6. Conclusions 33 (a) Some frames of a laparoscopic sequence of an abdominal cavity exploration (186 frames). (b) AR cylindrical insertion back projected in live laparoscopic video and the photorealistic 3D model recovered. Figure 2.9: Hand-held laparoscope sequence of an abdominal cavity exploration (186 frames). phantoms, this chapter presents the first results of using monocular SLAM in real human laparosocopic surgeries. The proof of concept with real laparoscopic imagery has shown the potential of this robotics technique in the medical field, however, although the combination of EKF + JCBB has proven to be very promising, some problems must still be overcome. Monocular SLAM recovers a 3D map of the cavity and the trajectory followed by the laparoscope, in real time at 25 fps, using the laparoscopic sequence as only input, opening new venues for the surgery of the future. SLAM may be exploited to increase synthetically the FoV by means of photorealistic reconstructions computed in real time, to do internal distance measurements 34 2. Monocular EKF-SLAM along with their error, and to insert AR notations that facilitate the operation. All these capabilities have been shown on real images gathered from a monocular laparoscope observing the abdominal cavity. After testing the feasibility of EKF + JCBB in laparoscopic imagery, several issues are still open. The current algorithm assumes: 1) scene rigidity; 2) smooth laparoscope motion; 3) that the laparoscope is always inside the cavity; and 4) low motion clutter and occlusions. These assumptions do not hold in general medical scenes: non rigidity is almost prevalent, sudden motions are frequent, the laparoscope is extracted and reinserted inside the cavity, and tools cause a significant motion clutter and occlusions. They produce tracking failures and an increment of the number of spurious matches. Figure 2.10 shows these drawbacks extracted from a real laparoscopic sequence; it depicts the size map, the inlier and the outliers matches, and a tracking failure caused by laparoscope extraction and reinsertion (blue dashed rectangle); it can be seen how in some frames the number of spurious matches is similar to the number of inliers. Regarding tracking failure, relocation algorithms such as [WKR07; CN08] recover the track of the system when it is lost providing robustness to the whole system. Regarding spurious matches, JCBB works fine in man-made environments (mobile robotics scenes) where the scene is completely static and thus the number of spurious low. However, its exponential computational complexity causes this algorithm to not run in real time when several spurious are present, as shown in Section 3.2. Additionally, JCBB uses the EKF prediction to detect spurious matches, which entails linearization errors. If the linearization is not a good approximation, the reconstruction error will degrade as the estimation evolves. Therefore, a more efficient data association is mandatory for using monocular SLAM in laparoscopic surgery. These questions are addressed in Chapter 3. After the system improvement of Chapter 3, an extensive validation of the accuracy is necessary for showing the system feasibility in medical imagery. This validation is performed in Chapter 4. 2.6. Conclusions 35 0 100 200 300 400 500 600 700 800 900 0 50 100 150 Frame #Features Outliers Map Size #Features measured #Outliers Temporal window. The system is lost Figure 2.10: Map size –black–: the total number of map features. Inlier matches –red–: measured features. Spurious matches –magenta–: matches found inside the active search region (IC matches) but marked as spurious by robust data association. The blue dashed rectangle corresponds to frames where tracking was lost. 3.2. 1-Point RANSAC 43 of the RANSAC, a simple 2D line estimation example with spurious data is used to explain the standard RANSAC algorithm (Figure 3.5) and, after that, its adaptation to the EKF framework with the proposed 1-PR algorithm (Figure 3.6). 1-PR is thoroughly detailed along this section and is shown as a practical matching algorithm. Standard RANSAC starts from a set of data, 2D points in this simple example, and the underlying model that generates the data, a 2D line. In the first step, RANSAC constructs hypotheses for the model parameters and selects the one that gathers most support. Hypotheses are randomly generated from the minimum number of points necessary to compute the model parameters, which is two in the case of line estimation. Support for each hypothesis can be computed in its most simple form by counting the data points inside a threshold (related to the data noise), although more sophisticated methods have been used [TZ00]. Hypotheses involving one or more outliers are assumed to receive low support, as is the case in the third hypothesis in Figure 3.5. The number of hypotheses nhyp necessary to ensure that at least one spurious-free hypothesis has been tested with probability pcan be computed from this formula: nhyp =log (1 −p) log (1 −(1 −)m),(3.3) where is the outlier ratio and mthe minimum number of data points necessary to instantiate the model. The usual approach is to adaptively compute this number of hypotheses at each iteration, assuming the inlier ratio is the support set divided by the total number of data points in this iteration [HZ04]. Data points that voted for the most supported hypothesis are considered clear inliers. In a second stage, clear inliers are used to estimate the model parameters. Individual compatibility is checked for each one of the rest of the points against the estimated model. If any of them is rescued as inlier, as happens in the example in Figure 3.5, the model parameters are re-estimated again in a third step. Figure 3.6 illustrates the idea behind 1-PR in the same 2D line estimation problem. As the first key difference, the starting point is a data set and its underlying model, but also a prior probability distribution over the model parameters. RANSAC hypotheses are then generated based on this prior information and data points, differently from standard RANSAC hypothesis based solely on data points. The use of prior information can reduce the size of the data set that instantiates the model to the minimum size of one point, and it is here where the computational benefit of this method with 44 3. Robust Monocular SLAM ⎪ ⎪ ⎩ ⎪ ⎪ ⎨ ⎧ 3RD STEP: MODEL RE-ESTIMATION WITH RESCUED INLIERS DATA CONTAMINATED WITH OUTLIERS Most supported hypothesis Outliers?? Inlier 1st STEP: RANDOM HYPOTHESIS 10 votes 3 votes 8 votes 2nd STEP: MODEL INSTANTIATION WITH CLEAR INLIERS Outliers?? Outliers Outliers Figure 3.5: RANSAC steps for the simple 2D line estimation example: First, random hypotheses are generated from data samples of size two, the minimum to define a line. The most supported one is selected, and data voting for this hypothesis are considered inliers. Model parameters are estimated from those clear inliers in a second step. Finally, the remaining data points consistent with this latest model are rescued and the model is re-estimated again. respect to RANSAC arises: according to Equation 3.3, reducing the sample size mgreatly reduces the number of RANSAC iterations and hence the computational cost. The order of magnitude of this reduction can be better understood if instead of this simple estimation example, a real visual estimation application is used. According to [Nis04], at least five image points are necessary to estimate the 6 degrees of freedom (DoF) camera motion between two frames (so m= 5). Using Equation (3.3), assuming an inlier ratio of 0.5 and a probability pof 0.99, the number of random hypotheses would be 146. Using the 1-PR scheme, assuming that probabilistic a priori information is available, the sample size mcan be reduced to one point and the number of hypotheses would be reduced to 7. Having an a priori probability distribution over the camera parameters is unusual in classical pairwise Structure from Motion (SfM) which assumes widely separated views [HZ04], and methods like standard RANSAC, which generate hypotheses from candidate feature matches, 3.2. 1-Point RANSAC 45 are mandatory in this case. But in sequential SfM from video ([Dav03; KM08; Mou+09]), smooth interframe camera motion can be reasonably assumed and used to generate a prior distribution (prediction) for the image correspondences. For the specific case of the EKF implementation of sequential SfM, this prior probability is naturally propagated by the filter and is straightforwardly available. ⎪ ⎪ ⎩ ⎪ ⎪ ⎨ ⎧ 3RD STEP: MODEL RE-ESTIMATION WITH RESCUED INLIERS DATA CONTAMINATED WITH OUTLIERS AND A PRIORI INFORMATION Most supported hypothesis Outliers?? Inlier 1st STEP: RANDOM HYPOTHESIS USING 1 POINT 10 votes 5 votes 9 votes 2nd STEP: MODEL INSTANTIATION WITH CLEAR INLIERS Outliers?? Outliers Outliers Inlier Figure 3.6: 1-PR steps for the simple 2D line estimation example: As a key difference from standard RANSAC, the algorithm assumes that an a priori probability distribution over the model parameters is known in advance. This prior knowledge allows to compute the random hypotheses using only 1 data point, hence reducing the number of hypotheses and the computational cost. The remaining steps do not vary with respect to standard RANSAC in Figure 3.5. . 3.2.1 Related Work Data Association RANSAC [FB81] was introduced early in visual geometric estimation [TM93] and has been the preferred outlier rejection tool in the field. Recently, an important stream of research has focused on reducing the model verification 46 3. Robust Monocular SLAM cost in standard RANSAC ([RFP08; CM08; Cap05; Nis05]) via the early detection and termination of bad hypotheses. The 1-PR algorithm proposed here is related to this stream in the sense that it also reduces the hypothesis generation and validation cost. Nevertheless, it does so in a different manner: instead of fast identification of good hypotheses among a large number of them, the number of hypotheses is greatly reduced from the start by considering the prior information given by a dynamic model. Incorporating probabilistic information into RANSAC has rarely been discussed in the computer vision literature. Only Moreno et al. ([MNLF08]) have explored the case where weak a priori information is available in the form of probabilistic distribution functions. More related to this method, the combination of RANSAC and Kalman filtering was proposed by Vedaldi et al. [Ved+05]. 1-PR might be considered a specific form of Vedaldi’s quite general approach. They propose an iterative scheme in which several minimal hypotheses are tested; for each hypothesis, all the consistent matches are iteratively harvested; no statement about the cardinality of the hypotheses is made. Here, a definite and efficient method, in which the cardinality of the hypotheses generator size is 1, and the inlier harvesting is not iterative but in two stages, is proposed. The method is described in reproducible detail to deal efficiently with the EKF algorithm by splitting the expensive EKF covariance update in two stages in order to reach real time. RANSAC using 1-point hypotheses has also been proposed in [SFS09] as the result of constraining the camera motion. While at least 5 points would be needed to compute monocular SfM for a calibrated camera undergoing general 6 DoF motion [Nis04], fewer are needed if the motion is known to be less general: as few as 2 points in [OM01] for planar motion and 1 point in [SFS09] for planar and nonholonomic motion. As a clear limitation of both approaches, any motion performed out of the model will result in estimation error. In fact, it is shown in real-image experiments in [SFS09] that although the most constrained model is enough for RANSAC hypotheses (reaching then 1-PR), a less restrictive model offers better results for motion estimation. In the case of the 1-PR method, extra information for the predicted camera motion comes from the probability distribution function that the EKF propagates over time. The method presented is then, in principle, not restricted to any specific motion, being suitable for 6 DoF estimation. The only assumption is the existence of tight and highly correlated priors. This assumption is reasonable within the EKF framework since the filter itself only works in such circumstances. Among non-RANSAC-based methods for data association, JCBB has 3.2. 1-Point RANSAC 47 been the preferred technique within the EKF framework being successfully used both in visual [Cle+07; WKR07] and non-visual SLAM [FNL02]. As discussed in Section 2.3, JCBB extracts the maximum set of matches that is jointly compatible with the multivariate Gaussian prediction from all IC matches. Nevertheless, JCBB entails two limitations: its exponential computational cost in the number of measurements, and its lack of accuracy for operating on the linearized predicted state of the measurements, which are overcome by 1-PR. Regarding the former, the computational complexity of 1-PR is linear in the number of measurements with low cost variation in the number of spurious matches (outliers). Regarding the latter, JCBB operates with the prediction of the measurements before fusing them, in contrast, 1PR, and RANSAC in general, operates after fusing a subset of them, which corrects part of the predicted model error with respect to the real system. Two methods are also of interest for this work. First, Active Matching (AM) [CD08] which is a clear inspiration for 1-PR. In AM, feature measurements are integrated sequentially; the choice of a measurement, at each step, is driven by expected information gain; the results of each measurement in turn are used to narrow the search for subsequent correspondences. 1-PR can be seen as lying in the middle ground between RANSAC or JCBB, which obtain point correspondence candidates and then aim to resolve them, and AM with its fully sequential search for correspondences. The first step of 1-PR is very similar to AM confirming that integrating the first match highly constrains the possible image locations of other features but, afterwards, both algorithms diverge. A problem with AM is the unreasonably high computational cost when scaling to large numbers of feature correspondences per frame (1-PR has much better properties in this regard), though an improvement to AM has also addressed this issue in a different way [Han+10]. The second method is Randomized Joint Compatibility proposed by Paz et al. [PTN08]. This basically randomizes the jointly compatible set search by avoiding the complete Branch and Bound search. At the first step, an initial small set of jointly compatible inliers is obtained via Branch and Bound search in random sets. Then, the joint compatibility of each remaining match is checked against the initial set. Although this approach lowers the computational cost of the JCBB, it still faces the accuracy problems derived from the use of the predicted measurement function before data fusion. Benchmarking Carefully designed benchmark datasets and methods have come into standard use in the vision community [SS02; Eve+10]. Robotic datasets have reached a high level of detail presenting either detailed benchmarking proce- 48 3. Robust Monocular SLAM dures [K¨u+09], or datasets with reliable ground truth and open resources for comparison [Smi+09; BMG09]. The RAWSEEDS dataset [RAW11], which includes monocular streams for large scale scenarios, has been used for the validation of 1-PR. While being suitable to benchmark very large real-image experiments, robotic datasets face two main inconveniences: First, the robot motion is planar in all the datasets, thus not allowing to evaluate full 6-DoF motion estimation. And second, GPS only provides translational data so angular estimation cannot be benchmarked. Simulation environments, like the one described in [FP09], can provide the translational and angular ground truth for any kind of camera motion. Nevertheless, these simulation environments usually cannot represent full real world complexity. The benchmarking method proposed and used in this thesis overcomes all these limitations. It consists of comparing the estimation results against a Bundle Adjustment solution over high resolution images. Full 6 DoF motion can be evaluated with low user effort (only the generation of a Bundle Adjustment solution is required), requirements for hardware are low (a high resolution camera), and any kind of motion or scene can be evaluated, since the method operates over the real images themselves. This approach is not entirely new: the use of a global Bundle Adjustment solution to benchmark sequential algorithms has already been used in [ED07; Mou+09]. The contribution here is the validation of the algorithm showing that the Bundle Adjustment uncertainty is much lower than the sequential methods to benchmark. As another novelty, global Bundle Adjustment is applied over high resolution images further improving accuracy. While it is true that a Bundle Adjustment solution may still suffer from scale drift, it will be much lower than that of the sequential algorithms. Also, scale drift can be driven close to zero by carefully choosing the images over which to apply Bundle Adjustment, in order to form a well-conditioned network [Tri+00], so the validity of the method is not compromised. 3.2.2 1-PR EKF Algorithm Algorithm 1 outlines the proposed combination of 1-PR inside the EKF framework in its most general form in the belief that this method may be of application in a large number of estimation problems. Figures 3.7 and 3.8 illustrate the algorithm steps over a laparoscopic image. 3.2. 1-Point RANSAC 49 Algorithm 1 1-Point RANSAC EKF 1: INPUT: ˆxk−1|k−1,Pk−1|k−1{EKF estimate at step k−1} 2: th {Threshold for low-innovation points.} 3: OUTPUT: ˆxk|k,Pk|k{EKF estimate at step k} 4: {A. EKF prediction and individually compatible matches} 5: [ˆxk|k−1,Pk|k−1] = EKF prediction(ˆxk−1|k−1,Pk−1|k−1,u) 6: [ˆ hk|k−1,Sk|k−1] = measurement prediction(ˆxk|k−1,Pk|k−1) 7: zIC =search IC matches(ˆ hk|k−1,Sk|k−1) 8: {B. 1-Point hypothesis generation and evaluation} 9: zli inliers = [ ] 10: nhyp =∞ {Initial value. Updated in the loop} 11: for i= 0 to nhyp do 12: zi=select random match(zIC ) 13: ˆxi=EKF state update(zi,ˆxk|k−1){Only state; NO covariance} 14: ˆ hi=predict all measurements(ˆxi) 15: zth i=find matches below a threshold(zIC ,ˆ hi, th) 16: if size(zth i)> size(zli inliers)then 17: zli inliers =zth i 18: = 1 −size(zli inliers) size(zIC ) 19: nhyp =log(1−p) log(1−(1−)) 20: end if 21: end for 22: {C. Partial EKF update using low-innovation inliers} 23: [ˆxk|k,Pk|k] = EKF update(zli inliers,ˆxk|k−1,Pk|k−1) 24: {D. Partial EKF update using high-innovation inliers} 25: zhi inliers = [ ] 26: for every match zjabove a threshold th do 27: [ˆ hj,Sj] = point j prediction and covariance(ˆxk|k,Pk|k, j) 28: νj=zj−ˆ hj 29: if νj>Sj−1νj< χ2 α,d {α: Confidence level; d: DoF}then 30: zhi inliers =add match j to inliers(zhi inliers,zj) 31: end if 32: end for 33: [ˆxk|k,Pk|k] = EKF update(zhi inliers,ˆxk|k,Pk|k) 50 3. Robust Monocular SLAM (a) Individually compatible –IC– matches. State prediction ( ) with their corresponding elliptical search regions. (b) Consensus hypothesis and low-innovation matches. The match generating the hypothesis ( ). Low-innovation supporting matches ( ). Non-supporting matches ( ). Figure 3.7: 1-PR stages corresponding to one frame for the operation in Figure 4.9a (I): (a) Individually compatible –IC– matches. (b) RANSAC winner hypothesis and consensus low-innovation matches. The estimated state is represented by its projection in the image, ( ) stands for the estimate and the ellipse stands for the covariance. The measurements are displayed as ( ). Different colors are used to code different matching categories. Zoom is made over 4 paradigmatic matches for each class of matches. 3.2. 1-Point RANSAC 51 EKF Prediction and Individually Compatible Matching (lines 5–7) The algorithm begins with standard EKF prediction: the estimation for the state vector xk−1|k−1at step k−1, modeled as a multidimensional Gaussian xk−1|k−1∼ N ˆxk−1|k−1,Pk−1|k−1, is propagated to step kthrough the known dynamic model fk: ˆxk|k−1=fkˆxk−1|k−1,uk(3.4) Pk|k−1=FkPk−1|k−1F> k+GkQkG> k.(3.5) In the above equation ukstands for the control inputs to the system at step k;Fkis the Jacobian of fkwith respect to the state vector xk|k−1at step k;Qkis the covariance of the zero-mean Gaussian noise assumed for the dynamic model, and Gkis the Jacobian of fkwith respect to that noise at step k. The predicted probability distribution for the state xk|k−1can be used to ease the correspondence search (active search), as described in Section 2.2.4 for the visual SLAM case. Propagating this predicted state through the measurement model hioffers a Gaussian prediction for each measurement: ˆ hi=hiˆxk|k−1(3.6) Si=HiPk|k−1H> i+Ri,(3.7) where Hiis the Jacobian of the measurement hiwith respect to the state vector xk|k−1, and Riis the covariance of the Gaussian noise assumed for each individual measurement. The actual measurement zishould be exhaustively searched for inside the 99% probability region defined by its predicted Gaussian, Nˆ hi,Si, by comparison to the chosen local feature descriptor. Figure 3.7a shows measurement predictions of the map points (ˆ h: ), their elliptical search region and their corresponding found measurements (z: ) obtained for a monocular laparoscopic example. All measurements (z) compose a set of individually compatible matches (zIC = (z1,· · · ,zi,· · · ,zn)>). Active search allows computational savings and also constraints the matches to be individually compatible with the predicted state xk|k−1. Nevertheless, ensuring geometric compatibility for each separated match zidoes not guarantee the global consensus of the whole set. Therefore, the joint compatibility of the data against a global model still has to be checked for the set of individually compatible matches zIC previous to the EKF update. 1-Point Hypothesis Generation and Evaluation (lines 9–21) Following the principles of RANSAC, random state hypotheses ˆxiare generated and data support is computed by counting measurements below a 52 3. Robust Monocular SLAM threshold. It is assumed here that the predicted measurements are highly correlated, such that every hypothesis computed from one match reduces most of the common uncertainty producing an inlier uncertainty close to the measurement noise Ri. As the key difference with respect to standard RANSAC, random hypotheses will be generated not only based on the data zIC but also on the predicted state xk|k−1∼ N ˆxk|k−1,Pk|k−1. Exploiting this prior knowledge allows to reduce the sample size necessary to instantiate the model parameters from the minimal size to define the DoF of the model to only one data point. Since the termination criterion of the RANSAC algorithm in (3.3) grows exponentially with the sample size, using only one point reduces drastically the number of hypotheses to try. Another key aspect for the efficiency of the algorithm is that each hypothesis ˆxigeneration only needs an EKF state update using a single match zi. A covariance update, which is of quadratic complexity in the size of the state, is not computed and hence the cost per hypothesis is low. Hypothesis support is calculated by projecting the updated state into the camera, which can also be performed at very low cost compared with other stages in the EKF algorithm. All features whose Euclidean distance between their measurement and their new estimate is lower than an arbitrary threshold (originally this threshold was established as 2 times the measurement noise) are considered as supporters. Figure 3.7b shows a match generating a hypothesis, its supporting match set (low-innovation inliers), and the non-supporting matches for the monocular laparoscopic example. Partial Update with Low-Innovation Inliers (line 23) Data points voting for the most supported hypothesis, zli inliers, are designated as low-innovation inliers. They are assumed to be generated by the true model, since they are at a small distance from the most supported hypothesis. The rest of the points may be outliers but also inliers, even if they are far from the most supported hypothesis. It is well known that distant points are useful for estimating camera rotation while close points are necessary to estimate translation. In the RANSAC hypothesis generation step, a distant feature would generate a highly accurate 1-point hypothesis for rotation, while translation would remain inaccurately estimated. Other distant points would in this case have low innovation and would vote for this hypothesis. But as translation is still inaccurately estimated, nearby points would presumably exhibit high innovation even if they are inliers. 3.2. 1-Point RANSAC 59 from the 2796 images long sequence are also displayed. Results for different experiments using this benchmarking method have been grouped for better visualization and comparison: Figures 3.10 and 3.12 show estimation errors for different tunings of 1-PR and JCBB; and Figure 3.13 details their computational cost. All the experiments were run on an Intel(R) Core(TM) i7 processor at 2.67GHz. 1-Point RANSAC vs 5-Point RANSAC First, the performances of 5-point RANSAC (5-PR) and 1-PR are compared, to ensure that there is no degradation of performance when the sample size is reduced. Figures 3.10a and 3.10b show the errors of both algorithms with respect to the reference camera motion, along with their 99% uncertainty regions. It can be observed that reducing the sample size from 5 to 1 does not have a significant effect either on the accuracy or the consistency of the estimation. On the contrary, the figure even shows 1-PR outperforming 5-PR. This may be attributed to the fact that, unlike in classical SfM algorithms [RFP08], the theoretical number of hypotheses, given by Equation 3.3, was not inflated in the experiments. By increasing the number of iterations, 5PR comes close to 1-PR; but it is remarkable that without this augmentation 1-PR already shows good behavior. The standard deviation of image noise was chosen to be 0.5 for the experiments since subpixel matching is used. While the accuracy and consistency remains similar, the computational cost is much higher for the usual 5-PR than the proposed 1-PR. The details of the computational cost of both algorithms can be seen in Figures 3.13a and 3.13b. The cost of RANSAC is low compared with the rest of the EKF computations for the 1-PR case, but it is several orders of magnitude higher and is the main cost in the 5-PR case. This is caused by the increase in the number of random hypotheses in frames with a large number of spurious matches. Figures 3.11a and 3.11b show the number of hypotheses in both cases, revealing that in 5-PR this is two orders of magnitude. Hypothesis generation from a single point opens the possibility of an exhaustive approach rather than a random one: while an exhaustive generation of all the possible combinations of 5 points in the measurement subset would be impractical, an exhaustive generation of 1-point hypotheses implies only as many hypotheses as measurements. Figure 3.10c details the errors for the 1point exhaustive hypothesis generation case. Compared with 1-point random hypothesis generation in Figure 3.10b, a similar accuracy and consistency is observed. Figure 3.11c shows the number of iterations needed for comparison with the random adaptive case (Figure 3.11b). The computational cost is increased but, as shown in Figure 3.13c, it is still dominated by the EKF 60 3. Robust Monocular SLAM 0 20 40 60 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (a) 5-PR, σz= 0.5 pixels. 0 20 40 60 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (b) 1-PR, σz= 0.5 pixels. 0 20 40 60 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (c) 1-PR exhaustive hypothesis, σz= 0.5 pixels. 0 20 40 60 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (d) 1-PR, σz= 0.2 pixels. Figure 3.10: Camera location error (thick blue line) and uncertainty (thin red line) for different RANSAC configurations. Similar error and consistency are shown for 5-PR and 1-PR in Figures 3.10a and 3.10b, respectively. Figure 3.10c also reports similar results for exhaustive hypothesis testing. Figure 3.10d shows smaller errors as a result of making 1-PR stricter by reducing the standard deviation of measurement noise. 3.2. 1-Point RANSAC 61 0 500 1000 1500 2000 2500 0 1000 2000 3000 #Frame #Iterations (a) Number of iterations along the sequence for 5-PR. 0 500 1000 1500 2000 2500 0 10 20 30 #Frame #Iterations (b) Number of iterations along the sequence for 1-PR. 0 500 1000 1500 2000 2500 0 10 20 30 #Frame #Iterations (c) Number of iterations along the sequence for exhaustive hypothesis generation. Figure 3.11: Number of iterations for 5-PR and 1-PR. Notice that the several orders of magnitude for the 5-PR case cause a large cost overhead when compared with 1-PR (Figures 3.13a, 3.13b and 3.13c detail the computational cost for the three cases respectively). update cost. Both options are thus suitable for real-time implementation. Analyzing the computational cost in Figure 3.13b it can be concluded that the cost for 1-PR is always low compared with EKF computation even when the spurious match rate is high (the spurious match rate is shown in Figure 3.14b). As will be shown later, the latter becomes an important advantage over JCBB, whose cost grows exponentially with the rate of spurious matches. This efficiency opens the possibility of making the RANSAC algorithm stricter by reducing the measurement noise standard deviation and hence discarding high noise points in the EKF. Such analysis can be done by reducing the standard deviation from 0.5 to 0.2 pixels: high noise points were discarded as outliers, as can be seen in Figures 3.14b and 3.14d. The computational cost increases, as shown in Figure 3.13e with respect to 3.13b, 62 3. Robust Monocular SLAM but still remains small enough to reach real-time performance at 22 Hz. The benefit of discarding high noise points can be observed in Figure 3.10d: errors and their uncertainty were reduced (but still kept mostly consistent) as a result of measuring more accurate points. 1-Point RANSAC vs Joint Compatibility Branch and Bound (JCBB) RANSAC and JCBB tuning is a thorny issue when benchmarking both algorithms. Since both cases assume Gaussian distributions for the measurements and decide based on probability, choosing equal significance levels for the probabilistic tests of both algorithms is considered the fairest. The significance level was chosen to be 0.05 in the χ2test that JCBB performs to ensure joint compatibility for the matches. Consistently, the probabilistic threshold for RANSAC was set to 95% for voting (line 15 in Algorithm 1 in Section 3.2.2) and for the rescue of high-innovation matches (line 29 in the algorithm). The results of benchmarking JCBB are shown in the following figures. First, Figure 3.12a details the errors and uncertainty regions for the EKF using JCBB. It can be observed that the estimation in Figure 3.12a shows larger errors and inconsistency than the 1-PR one in Figure 3.12b, repeated here for visualization purposes. The reason can be observed in Figure 3.14 where the outlier rates for 1-PR and JCBB are shown: the number of matches considered outliers by 1-PR is greater than by JCBB. The points accepted as inliers by JCBB are the ones that spoil the estimation. A stricter version of JCBB has been benchmarked by reducing the standard deviation of uncorrelated measurement noise to 0.2 pixels, as was done with 1-PR. The spurious match rates of both algorithms, shown in Figures 3.14c and 3.14d, show that 1-PR remains more discriminative and hence produces a more accurate estimation than JCBB (Figure 3.12c). 1-PR errors for the same tuning are repeated in Figure 3.12d for comparison purposes. Also, as previously noted, the computational cost of JCBB grows exponentially when made stricter: Figure 3.13f shows peaks over one second in the worst cases. JCBB can also be made stricter by increasing the significance level αof the χ2test it performs to check the joint compatibility of the data. Several experiments were run varying this parameter. The lowest estimation errors, shown in Figure 3.12e, were reached for α= 0.5 instead of the usual α= 0.05. Estimation errors for this best JCBB tuning are still larger than in any of the 1-PR experiments. 3.2. 1-Point RANSAC 63 0 20 40 60 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (a) JCBB, σz= 0.5 pixels 0 20 40 60 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (b) 1-PR, σz= 0.5 pixels 0 20 40 60 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame (c) JCBB, σz= 0.2 pixels 0 20 40 60 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (d) 1-PR, σz= 0.2 pixels 0 20 40 60 −0.15 −0.1 −0.05 0 0.05 0.1 0.15 X Error #Frame 0 20 40 60 Y Error #Frame 0 20 40 60 Z Error #Frame 0 20 40 60 −0.1 −0.05 0 0.05 0.1 Roll Error [rad] #Frame 0 20 40 60 Pitch Error [rad] #Frame 0 20 40 60 Yaw Error [rad] #Frame (e) JCBB, σz= 0.2 pixels, α= 0.5 Figure 3.12: Camera location errors when using JCBB is shown in Figures 3.12a and 3.12c, for standard deviations of 0.5 and 0.2 pixels respectively. Figures 3.12b and 3.12d show 1-PR results for the same filter tuning, are repeated here for comparison. It can be seen that 1-PR outperforms JCBB in both cases. Figure 3.12e shows the best JCBB tuning found which still gives worse results than 1-PR. 64 3. Robust Monocular SLAM 0 500 1000 1500 2000 2500 0 0.05 0.1 0.15 #Frame seconds Cicle time 2nd EKF Update 1st EKF Update Ransac Prediction #Features Times & Map Size 0 20 40 60 80 100 120 140 Map Size (a) 5-PR, σz= 0.5 pixels 0 500 1000 1500 2000 2500 0 0.01 0.02 0.03 0.04 0.048 #Frame seconds Cicle time 2nd EKF Update 1st EKF Update Ransac Prediction #Features Times & Map Size 0 20 40 60 80 100 120 140 Map Size (b) 1-PR, σz= 0.5 pixels 0 500 1000 1500 2000 2500 0 0.01 0.02 0.03 0.04 0.048 #Frame seconds Cicle time 2nd EKF Update 1st EKF Update Ransac Prediction #Features Times & Map Size 0 20 40 60 80 100 120 140 Map Size (c) 1-point exhaustive hypothesis, σz= 0.5 pixels 0 500 1000 1500 2000 2500 0 0.05 0.1 0.15 #Frame seconds Cicle time EKF Update JCBB Prediction #Features Times & Map Size 0 20 40 60 80 100 120 140 Map Size (d) JCBB, σz= 0.5 pixels 0 500 1000 1500 2000 2500 0 0.01 0.02 0.03 0.04 0.048 #Frame seconds Cicle time 2nd EKF Update 1st EKF Update Ransac Prediction #Features Times & Map Size 0 20 40 60 80 100 120 Map Size (e) 1-PR, σz= 0.2 pixels 0 500 1000 1500 2000 2500 0 0.2 0.4 0.6 0.8 1 #Frame seconds Cicle time EKF Update JCBB Prediction #Features Times & Map Size 0 20 40 60 80 100 120 140 Map Size (f) JCBB, σz= 0.2 pixels Figure 3.13: Detail of times and map sizes for different RANSAC and JCBB configurations in double y-axis figures: times are shown as areas and measured in seconds on the left y-axis; the map size is displayed as a a red line and is measured on the right y-axis. 1-PR exhibits much lower computational cost than 5-PR and JCBB. 1-PR also shows only a small increase when made exhaustive or stricter, making it suitable for real-time implementation at 22 Hz for the map size detailed in the figures. 3.2. 1-Point RANSAC 65 0 500 1000 1500 2000 2500 0 0.2 0.4 0.6 0.8 1 #Frame outliers/(inliers+outliers) (a) JCBB, σz= 0.5 pixels. 0 500 1000 1500 2000 2500 0 0.2 0.4 0.6 0.8 1 #Frame outliers/(inliers+outliers) (b) 1-PR, σz= 0.5 pixels 0 500 1000 1500 2000 2500 0 0.2 0.4 0.6 0.8 1 #Frame outliers/(inliers+outliers) (c) JCBB, σz= 0.2 pixels 0 500 1000 1500 2000 2500 0 0.2 0.4 0.6 0.8 1 #Frame outliers/(inliers+outliers) (d) 1-PR, σz= 0.2 pixels. Figure 3.14: Spurious match rate for JCBB and RANSAC when measurement noise standard deviation σzis reduced to 0.2 pixels. It can be observed that reducing the measurement noise makes both techniques stricter, but 1-PR remains more discriminative. 3.2.5 Experimental Validation: Monocular EKF-Based Estimation for Long Outdoor Sequences Three different sequences from the RAWSEEDS [RAW11] dataset have been used to test the validity of the 1-PR EKF for long-term camera motion estimation. All sequences were recorded by a 320 ×240 Unibrain camera with a wide-angle lens capturing at 30 fps. The estimated camera trajectories were validated against GPS data by means of an Euclidean distance: k=rrW Ck−rW GP Sk>rW Ck−rW GP Sk.(3.15) rW Ckcorresponds with the estimated position (not the orientation) for camera kand rW GP Skcorresponds with the GPS position for the same camera after aligning and scaling both trajectories by means of optimization. 66 3. Robust Monocular SLAM Table 3.1: EKF-based visual estimation error for long camera trajectories. Trajectory length [m] Sensor used Mean error [m] Maximum error [m] % mean error over the trajectory 146 monocular 1.3 4.2 0.9% 153 monocular 1.9 3.3 1.1% 650 monocular 6.4 11.1 1.0% 0 1 2 3 4 0 0.025 0.05 0.075 0.1 Error [m] Frequency 146 metres sequence, adaptive 1−Point RANSAC (a) 146 meters trajectory 0 1 2 3 0 0.01 0.02 0.03 0.04 Error [m] Frequency 153 metres sequence, adaptive 1−point RANSAC (b) 156 meters trajectory 0 1 2 3 4 5 6 7 8 9 10 11 12 0 0.01 0.02 0.03 0.04 0.05 Error [m] Frequency 650 metres sequence, 1−point adaptive RANSAC (c) 650 meters trajectory 0 2 4 6 8 10 0 0.01 0.02 0.03 0.04 0.05 Error [m] Frequency 650 metres sequence, exhaustive hypothesis generation (d) 650 meters trajectory; 1-PR exhaustive Figure 3.15: Histograms of the errors for the three experiments. In the first sequence, consisting of 6000 images, the robot translates around 146 meters. The second sequence has 5400 images and the robot describes a similar trajectory length, around 153 meters. Finally, a very long and challenging sequence is evaluated that consists of 24180 frames (13.5 minutes of video) in which the robot describes a trajectory of 650 meters. In order to reduce scale drift error, around two hundred features per frame 3.2. 1-Point RANSAC 67 (a) 146 meters trajectory (b) 156 meters trajectory (c) 650 meters trajectory Figure 3.16: Estimated trajectories from monocular data and GPS data. 68 3. Robust Monocular SLAM had to be measured. This high number increased the computational cost of the EKF beyond real-time bounds. In the particular experiments presented, the algorithm ran at about 1 Hz. Table 3.1 details the maximum and mean errors obtained in these experiments. It is worth noting that although for the three experiments the accumulated drift makes the error noticeable when plotted with the GPS trajectory, the relative error with respect to the trajectory keeps a low value (1% of the trajectory length). Figure 3.15 shows histograms of the errors for the three sequences. Subfigures 3.15c and 3.15d show histograms of the errors for the 650 meters experiment in two different versions of the 1-PR algorithm: the first one using Algorithm 1 and the second one replacing the random hypothesis generation with exhaustive hypothesis generation (Algorithm 2) as evaluated in Figure 3.10c. The conclusion from Section 3.2.4 –“1-Point RANSAC vs 5-Point RANSAC”– is confirmed here: exhaustive hypothesis generation improves very slightly the estimation errors. Figure 3.16 shows the estimated (in black) and the GPS (in red) trajectories over a top view extracted from Google Maps for each sequence. The accuracy of the estimated trajectories is clear from visual inspection. 3.3 Laparoscopic Experiments In order to prove the EKF + 1-PR (in its exhaustive version –Algorithm 2) + RLR combination performance in laparoscopy, two series of laparoscopic sequences were captured. The performance relies on the configuration of three thresholds: 1) FAST feature initialization threshold, which corresponds with a Shi-Tomasi score, and indicates how distiguible is the point (the higher the score is, the more distinguible the point is); 2) matching normalized correlation threshold (when it tends to 100%, the correlation is better); and 3) reobservation rate threshold, which determines the life time of features (when it tends to 0%, it is more difficult to remove features from the map). In case of laparoscopic sequences, these thresholds are defined as 30, 40% and 40%, respectively. In contrast, they are stricter for traditional robotics sequences (scenes of man-made environments) whose typical values are defined as 300, 95% and 75%. The first series consists of a 874 frame laparoscopic sequence at 360x288@25 Hz. The sequence, which corresponds to an abdominal exploration where a real human ventral hernia (hole) can be seen, is the same as the second experiment (Figure 2.9) of Section 2.5.2. However, in this section, only 186 frames could be processed with the EKF + JCBB combination. The 3.4. Conclusions 75 Typical map sizes are between 50 and 100 points. Up to 40 map features are measured per frame. Figure 3.24b shows a histogram of the outlier count for all frames in all the sequences. Although nearly 30% of frames do not contain any spurious match, only one of the sequences can be successfully processed if 1-PR is disabled. Therefore, it can be concluded that algorithms robust to spurious data are a must for EKF SLAM even in the case of a low spurious-matches rate. 1-PR cost is linear in number of measurements and state size while the outliers have a low influence on the computational cost (<20% of the total budget corresponding to 1-PR hypotheses generation and consensus). Hence, the proposed system can achieve real time even when ∼25% of frames contain more than 3 outliers. In contrast, methods like exhaustive JCBB, with exponential complexity in the number of outliers, would not perform in real time. 3.4 Conclusions 3.4.1 1-Point RANSAC This Chapter presents a novel RANSAC algorithm which, for the first time and differently from standard purely data-driven RANSAC, incorporates a priori probabilistic information into the hypothesis generation stage. As a consequence of using this prior information, the sample size for the hypothesis generation loop can be reduced to the minimum size of 1 point data. 1-PR has two main strengths that worth summing up here. First, as in standard RANSAC, model constraints are checked after hypothesis data has been fused with the a priori model. Second, using 1-point plus prior knowledge hypotheses greatly reduces the number of hypotheses to construct, and hence the computational cost compared with usual RANSAC based solely on data. In a practical sense, 1-PR presents a linear computational complexity in the number of outliers that means an overhead of less than 20% of the standard EKF cost, making it suitable for real-time implementation in visual SLAM. Comparing with JCBB, where its relevance resides on their generality, the main advantage of 1-PR is its efficiency. The rich variety of correlation patterns that a covariance matrix can encode is manageable by JCBB. However, 1-PR exploits the very simple pattern where all the correlations are mainly explained by sensor motion, and hence small size data subsets are enough to constraint the rest of the measurements. Therefore, 1-PR is directed to the particular case of rigid scenes, thus for more complex models like non-rigid scenes, 1-PR may not offer such a satisfactory result. Nevertheless, it is also true that estimation from a moving sensor data 76 3. Robust Monocular SLAM stream in an almost rigid scene covers a great percentage of SLAM problems; and a specific method more efficient than general methods can be of importance. In this sense, 1-PR outperforms existing approaches by presenting lower cost and scaling well with the state vector and measurement size, and also with the outlier rate (1-PR presents a linear cost in the number of outliers versus the exponential complexity of JCBB). Besides its efficiency, 1-PR has also some advantages in dealing with nonlinearities as a result of checking rigidity after data fusion where some of the inaccuracies introduced by non-linearities have been compensated. On the contrary, JCBB checks rigidity before data fusion which is a serious drawback of the algorithm. This chapter also presents a method for benchmarking 6-DoF cameramotion-estimation results. The method shows three clear advantages: Firstly, it is intended for real image sequences and includes effects difficult to reproduce by simulation (like non-Gaussian image noise, shaking handy motion, image blur or complex scenes). Secondly, it is easily reproducible as the only hardware required is a high resolution camera. And thirdly, the effort required by the user is low. The uncertainty of the estimated solution also comes as an output of the method and the appropriateness of BA estimation as reference can be validated. The method has been used to prove the claimed superiority of the 1-PR method. The general EKF + 1-PR algorithm has been experimentally tested for the case of large camera trajectories in outdoor scenarios. Errors around 1% of the trajectory have been obtained for trajectories up to 650 meters from a publicly available dataset. The number of tracked features in the image has to be increased to two hundreds in order to avoid scale drift. This high number makes this case currently moves away from real-time performance, and the method runs at 1 frame per second. Finally, it is also worth remarking that, although this thesis is focused on the particular case of monocular EKF-SLAM, the 1-PR method is independent of the type of sensor used. The only requirement is the availability of highly correlated prior information, which is typical of EKF-SLAM for any kind of sensor used. Also, as highly correlated priors are not exclusive to EKF-SLAM, the applicability of 1-PR could be even broader. As an example, the camera pose tracking in keyframe schemes [KM07; Mou+09] would benefit from 1-PR cost reduction provided that a dynamic model were added to predict camera motion between frames. 3.4. Conclusions 77 3.4.2 Laparoscopic Experiments An improved robust version of the EKF-SLAM has been proposed in this chapter. The new version includes the integration of a relocalization system (RLR) into the EKF framework and the substitution of JCBB data association by 1-PR procedure. This new combination has been tested over two laparoscopic sequence series. The first one consists of a 874 frame laparoscopic sequence. This sequence is the same as the second experiment (Figure 2.9) of the Section 2.5.2 and was processed in 2010 with an unoptimized version of the new combination. The second series is composed of fifteen in-vivo human laparoscopic ventral hernia repairs, which were processed in 2013 with an optimized code version achieving real-time performance. The integration of RLR enables to recover the system after tracking losses. This is essential in laparoscopy where instrument occlusions or typical laparoscope maneuvers may cause the loss of tracking. RLR detects these losses and stops the EKF integration persevering the integrity of the map from a possible corruption. Then, RLR searches for possible putative matches between the current image and the fixed map. After a set of putative matches is found, RLR tries to recover the tracking by 3-point-pose PnP algorithm and RANSAC. When a good camera location is found, RLR reactivates the normal EKF working. 1-PR has also demonstrated to greatly outperform JCBB. 1-PR along with RLR have shown their performance over the 874 frame sequence of the first series. For this sequence, the EKF + JCBB combination (Chapter 2) only could process 186 out of 874 frames, but the new combination processed the complete sequence. This is mainly possible thanks 1-PR copes with a high number of outliers caused by a laparoscopic tool interaction (JCBB does not), and thanks RLR system which allows to support losses of tracking avoiding a complete system failure. Then, the combination of EKF + 1-PR + RLR has shown to be appropriate to build a map from laparoscopic sequences. This combination has demonstrated to be able to cope with typical challenges in this kind of sequences: sudden motions, surgical tool cluttering, temporary tissue deformation, large occlusions and laparoscope extraction and reinsertion in the abdominal cavity. Since the system is based on an EKF filter, the computational cost is quadratic in the map size and linear in the number of measured features in the image. In laparoscopic experiments, a significant number of map points need to be measured to achieve robust relocalization. In the first series of experiments, the map size was above 100 features and the number 78 3. Robust Monocular SLAM of measured features was fixed to 45 resulting times lower than 3 times realtime (120 ms). However, in the second series, an optimized version of the code along with the reduction of measured features (40) and the limited map size to 100 features, achieve times lower than 40 ms per frame (25 Hz), thus it works in real time. It must be noted that the proposed algorithm is able to compute a nice summary of the scene after processing the whole sequence. A survival-ofthe-fittest process selects what scene features are included in the map. Only locally salient, trackable, and distinctive for relocation points are included in the final map. This rigid map is excellently exploited by relocalization procedure to recover from tracking losses and to relocate at reinsertions. The computed map might well be the starting point for learning priors to process sequences corresponding to similar procedures performed to different patients. All results have been validated over real sequences, so it can be concluded that monocular SLAM in the abdominal cavity is a valid mapping method that does not need any additional sensor but just a standard monocular endoscope and commodity computers. Despite an experimental validation has been provided for the method, it would be interesting to compare its solution with respect to a ground truth. Chapter 4 is devoted to an extensive validation of the accuracy of this system. Additionally to the accuracy validation, another validation from clinical point of view is carried out, showing the possible advantages that monocular visual SLAM entails with respect to the patient and inside the operating room. 4 Exhaustive System Validation Chapter 3 has shown the maturity of visual SLAM algorithms in the field of robotics both for relative small environments, like the first experiment compared with a bundle-adjustment ground truth, and for large environments, like those several-hundred-meter trajectories validated with GPS data. However, to date, the performance of these algorithms applied in medical imaging has not thoroughly been validated over real surgeries. Most of works in medical imaging make subjective or objective validations of the algorithms by using external trackers, phantoms, synthetic data, ex-vivo data, in-vivo animal data, or a combination of them (e.g. [MY10; Mir+12; Hu+12]). The most important contribution of this thesis is precisely the exhaustive validation of these algorithms with human real laparoscopic interventions. The validation has been carried out with simulations and real in-vivo surgeries. Fifteen laparoscopic ventral hernia repair (LVHR) operations have been captured to validate monocular visual SLAM because: 1) the scene is almost rigid and textured; 2) the standard LVHR procedure includes accurate distance measurements that can be used as ground-truth; 3) the surgical procedure has not been modified at all, except for the addition of an exploratory endoscope maneuver; 4) SLAM exploits the images simplifying the surgical procedure without a disruptive modification of the workflow; and 5) image sequences exhibit significant inter-patient variability in texture, illumination, input port placement, and exploratory trajectory. This chapter is devoted to detailing the visual SLAM validation over laparoscopic surgeries both from engineering and clinical points of view. Both validations have been reported in [GG+14] and [Ber+], respectively. Section 4.1 describes the ventral hernia repair procedure. Section 4.2 explains the new exploratory endoscope maneuver needed to use SLAM in LVHR proposed in 79 80 4. Exhaustive System Validation [Gil+11a; Gil+11b]. Section 4.3 details the simulations performed to validate the accuracy of the scene reconstruction and the trajectory recovered by SLAM at different camera configurations [GG+14]. Section 4.4 explains the protocol followed for data acquisition. Finally, Sections 4.5 and 4.6 show the engineering and clinical validation of these methods in laparoscopic images [GG+14; Ber+]. It is worth noting that the works [Gil+11a], [Gil+11b] and [Ber+] correspond with clinical publications, and, for that reason, the first authors are surgeons. However, these publications would not exist without the engineering contribution by the author of this thesis that has been essential in all of them. 4.1 Ventral Hernia Repair Procedure A ventral hernia is a defect –hole– that appears in the internal abdominal wall due to muscular strain, weak abdominal muscles, or as a result of previous surgery (incisional hernias). Ventral hernias are dangerous because part of an organ –usually the bowel or intestine– might protrude through the hernia and cause an obstruction or strangulation of the organ accompanied by intense pain and necrosis. The reported overall prevalence of ventral hernias ranges from 2% to 13% [MH85; SR93; Ban+12] for the incisional case being a common pathology confronted by surgeons. Repair of a ventral hernia ideally involves placement of a prosthetic mesh in the preperitoneal subaponeurotic plane, in a tension-free manner with the edges well beyond the borders of the hernia defect. Uniformly distributed intra-abdominal pressure contributes to fixation of the mesh (Pascal’s principle), reducing the risk of recurrence. Both open surgery with retromuscular mesh placement and laparoscopic surgery with intraperitoneal mesh placement can benefit from uniformly distributed intra-abdominal pressure (Figure 4.1). Historically, the most widely used surgical treatment for ventral hernias was the open retromuscular (Rives-Stoppa) repair procedure, which had the best outcomes for most incisional hernias and some primary ventral hernias. This procedure involves extensive parietal dissection and placement of a nonresorbable polypropylene or polyester mesh behind the posterior rectus fascia. Developments in biocompatible materials and endoscopic surgery [LB93] have enabled laparoscopic placement of bilaminar intraperitoneal prosthetic mesh, with minimal dissection. The mesh overlies the hernia defect and extends 3–5 cm beyond the borders of the defect [LeB+03; LeB07], and is fixed to the abdominal wall with tackers using the double-crown technique 4.1. Ventral Hernia Repair Procedure 81 Figure 4.1: Intra-abdominal pressure helps to secure the prosthetic mesh in the intraperitoneal sublay position to the abdominal wall. [MC+05], or transfascial sutures, or a combination of these methods; an evaluative review of the fixation methods can be found in [LeB07]. Primary closure of the hernia has good outcomes, but is technically complex [Ban+12; Ore+11]. Recently, biological adhesives such as fibrin have been used to fix the prosthetic mesh in place [Ste+10]. However, there are still uncertainties in the laparoscopic technique regarding the optimal mesh type, mesh fixation method, and measurement of hernia defect size; and the incidence of seromas. A video showing the LVHR procedure is available in [GGg]. The LVHR technique offers the advantages of the laparoscopic approach, i.e., a short hospital stay, less postoperative pain, and fast postoperative recovery. The procedure carries an acceptable risk of complications, a low risk of recurrence, and an excellent cosmetic result. LVHR is considered to be a good alternative to open surgery, at least in experienced hands [Ore+11; Sau+11]. In the LVHR procedure, the hernia defect is measured in-vivo to cover the defect with a customized-in-size patch. The elliptical patch axes are those of the defect plus the predefined safety margin (3-5 cm). If possible, a piece of a sterilized tape measure is introduced inside the abdominal cavity and at least one of the two main hernia axes is measured (Figure 4.2c). If the tape measurement cannot be taken, other less accurate indirect methods as external measurement based on needle insertion (Figures 4.2a and 4.2b) are used. A video showing these two measuring techniques is avaliable in [GGi]. In this thesis, a cross-fertilization between LVHR and visual SLAM algorithms is established. On one hand, LVHR provides internal measurements that can be used as a ground-truth to validate the SLAM geometrical accuracy. A 0.5 cm tape measurement resolution determines the ground-truth accuracy. On the other hand, visual SLAM can be used as a computerized method of measuring the hernia dimensions by making use of only the image sequence gathered by the laparoscope, and a standard computer. The 82 4. Exhaustive System Validation visual SLAM method can be smoothly integrated into the LVHR procedure to provide measurements, that are as accurate as the classical methods (Figure 4.2), but take less time and do not require insertion of needles or a tape measure into the abdominal cavity. (a) Internal view of the needles at the borders of the defect. (b) External measurement between the needle insertion points. (c) Tape measure method, with direct internal measurement of the defect. Figure 4.2: (a, b) Needle insertion method. (c) Tape measure method. 4.2 Hernia Repair SLAM Assisted Procedure In order to enable the use of SLAM in LVHR, a new exploratory laparoscope maneuver, proposed in [Gil+11a; Gil+11b], extends the standard LVHR procedure at the measurement stage. This new exploratory laparoscope maneuver is performed aimed at translating the endoscope tip while the region of interest is kept in the field of view (FoV) (Figure 4.3c). Doing so, it is possible to gather a sequence with enough parallax for an accurate SLAM. This 4.2. Hernia Repair SLAM Assisted Procedure 83 (a) Tape measurement considered as ground-truth. (b) Two points over a forceps define the scale (magenta crosses). Five or more points over the hernia defect boundary (yellow points). (c) Internal and external hand-held exploratory laparoscope motion. It is worth noting that the hernia is always inside the FoV. Notice also the fulcrum effect between internal and external maneuver (when surgeon moves the laparoscope to left, the laparoscope tip move to right and vice versa). Figure 4.3: Measurement process (I): internal measurement of the hernia, definition of the ellipse, and exploratory maneuver for the operation in Figure 4.9b. 84 4. Exhaustive System Validation (a) SLAM measurement, map and ellipses projected as augmented reality over a sequence frame. (b) Camera trajectory, 3D map and ellipses in 3D. Top view. Figure 4.4: Measurement process (II): estimated ellipses with the estimated hernia dimensions, 3D map and camera trajectory for the operation in Figure 4.9b. sequence is processed to estimate a cavity map and the endoscope trajectory. Two videos explaining this maneuver can be found in [GGe] and in [GGh] (0:11 - 1:12). Unlike visual SLAM using stereo images, monocular SLAM provides an up-to-scale 3D model of the cavity. To compute the actual measurements of the defect, the real scale of the 3D model is defined using a laparoscopic tool with a known tip size. Before the exploratory maneuver, additional key points are manually enforced to be in the map: two predefined points over a forceps to define the reconstruction scale, s, and several points (five or more) scattered over the defect boundary to estimate the hernia contour and size (Figure 4.3b). The hernia defect is modeled as a virtual 3D ellipse in a three-stage way. In the first stage, an initial guess of the dominant plane defined by the five or more defect boundary points is computed by least squares. This guess is covariance-weighted in the second stage by an information filter extracting the needed covariances from the probabilistic map of the EKF monocular SLAM. After that, the points are projected on the weighted plane where the planar ellipse is fitted. Finally, the defect major and minor axis sizes are estimated from the ellipse (Figure 4.4a). Their dimensions are computed from the scale factor saccording with (2.31) where dm(i, j) and dm(r1, r2) correspond with the length of one of the axes and the relative distance between the two forceps points, respectively, both in the SLAM map. Resulting from the exploration, the SLAM algorithm estimates the scene 4.4. Experimental Validation Description 91 (a) H-SE (b) HVSE (c) HVS- (d) H-S- (e) HVSE (f) HVSE (g) HVSE (h) H-SE (i) H-S- (j) H-S- (k) HVSE (l) –SE (m) –SE (n) –SE (o) HV-E Figure 4.9: The thumbnails –labeled from (a) to (o)– corresponding to the 15 ventral hernia repair surgeries used to validate the system. The “HVSE” code in the captions stands for the availability of (H) Horizontal tape measurement, (V) Vertical tape measurement, (S) SLAM measurement, and (E) External measurement. The SLAM map was successfully computed for all of them, while ellipse measurement was not possible in (o) due to the lack of texture around the defect. output; a standard computer (Intel Core i7 CPU, 2.93 GHz, 4 GB RAM) equipped with a frame grabber; and a videocamera. In order to make the 92 4. Exhaustive System Validation most of each operation ensuring the data capture and minimizing technical problems during surgeries, the next action protocol for each operation was established: 1. Computer connection with the endoscope: The frame grabber of the computer was connected to a free PAL output of the endoscope. Frames were subsampled and captured at 384x288@25 fps in order to avoid the combing effect due to interlacing. The sequences were captured uncompress to avoid losses in the image quality. 2. Installation of the external camera: All interventions except the corresponding to operation 4.9l were externally filmed with the external videocamera hanged on the roof lamps of the operating theatre. Video and audio of the external recordings were essential to: obtain external measurements; obtain measure times of the SLAM, internal, and external measuring methods; and capture the basic endoscope movements of the method. Figure 4.10 shows 10 out of 11 external measurements captured with the external camera. 3. Endoscope configuration: A correct endoscope illumination is mandatory for a correct visual SLAM performance. The endoscope illumination was configured to be greater than 60%. 4. Measurements: At the moment of measuring, the three methods (SLAM, internal, and external) were carried out whenever was possible. In the case of SLAM method, the only one performed in the fifteen surgeries, a laparoscopic tool had to be fixed inside the laparoscope field of view. The other two are not available in all surgeries usually due to difficulty in maneuverability or patients’ medical conditions. 5. Laparoscope calibration: When the surgery ended and previously to the optics removal from endoscope, a calibration planar pattern was imaged for laparoscope calibration according to Zhang’s method [Zha00]. As the laparoscope has a 30oDoV, eight photos of the planar pattern were taken with a special laparoscopic positioning as shows Figure 4.11. During surgeries all essential information was taken down. After the operation, this information was compared with the external and internal videos to ensure the correctness of data. The external recording for the intervention 4.9l is not available, therefore, the external measurements and times were taken trusting in operating room notes. 4.4. Experimental Validation Description 93 (a) Corresponds to Figure 4.9a. (b) Corresponds to Figure 4.9b. (c) Corresponds to Figure 4.9e. (d) Corresponds to Figure 4.9f. (e) Corresponds to Figure 4.9g. (f) Corresponds to Figure 4.9h. (g) Corresponds to Figure 4.9k. (h) Corresponds to Figure 4.9m. (i) Corresponds to Figure 4.9n. (j) Corresponds to Figure 4.9o. Figure 4.10: External measurements taken with the external videocamera corresponding to 10 out of 11 operations with external measurements. 94 4. Exhaustive System Validation (a) Two left images: laparoscope positioning, Z axis aims to the pattern center. Notice that the 30ooptics DoV means that the Z axis is not the same that the laparoscope main axis. Four right images: planar pattern images, one per each side. (b) Left images: laparoscope positioning, the laparoscope is 90ocounter clock-wise rolled around the Z axis which aims to the pattern center. Right images: planar pattern images of two opposite sides. (c) Left images: laparoscope positioning, the laparoscope is 90oclock-wise rolled around the Z axis which aims to the pattern center. Right images: planar pattern images of the another two opposite sides. Figure 4.11: Procedure to take the calibration images. Laparoscope positioning and the eight images of the planar pattern needed to calibrate. 4.5 SLAM Engineering Validation For the EKF SLAM validation, the same parameters, experimentally tuned, were applied for all of the experiments: image measurement error of 0.5 pixels standard deviation; 40% is the acceptance threshold for normalized correlation score to eventually accept a map point match in the new image; new features are assigned an initial 1 inverse depth, with an initial σρ= 1, in order to have an initial direct depth acceptance region starting in 0.3 and extending to include infinite; regarding linear and angular accelerations, 4.5. SLAM Engineering Validation 95 standard deviations are 2.51 s2and 3rad s2respectively, as monocular cannot observe the scale, both depth and linear acceleration have no length units; finally, map management initializes features in order to have 40 map points observable in the image. The proposed EKF SLAM has been able to successfully compute the map and the camera trajectory for the fifteen sequences (Figure 4.9). It has been able to cope with a variety of illuminations, textures and input port geometries. If a weakness has to be mentioned, it is the inability to perform the measurement in one of the sequences (Figure 4.9o) because of the lack of texture around the defect. In the rest of sequences, the EKF SLAM was always able to measure both ellipse axes because the defect visibility is required during the surgery and SLAM profits from that. In the failing case, the EKF SLAM was able to build the map; however, the clicked points signaling the defect were not trackable due to the lack of stable texture in the defect boundary area and the particular point detection method. A more dedicated work in image processing (e.g. using contours) is quite likely to overcome this limitation. In contrast, classical tape measuring procedure sometimes fails to produce the measurement because of the limited maneuverability resulting from the port placement. The surgical time consumed by SLAM is mainly due to the exploration, which takes less than 1 minute irrespective of the sequence. Since the algorithm runs live (Figure 4.12c), no additional time is needed for the processing, except for selecting the points over defect boundary and over the forceps to define the scale. Both are easy to automate with the corresponding surgical time saving. In contrast, the internal tape measurement procedure, used as a ground truth in this validation, is rather uncertain (the time length ranges from 2 to 5 minutes). It has to be noted that in three cases where longer times were anticipated, the surgeons did not even try to measure. In any case, SLAM recovers not only two measurements but a full 3D model and the support for augmented reality. To validate the SLAM geometrical accuracy, the dimensions of the hernia defect’s main axes have been estimated from the 3D recovered model and compared with those of internal tape measurement (the ground-truth), accurate up to 0.5cm. Figures 4.13, 4.14, and 4.15 show captures comparing the internal measurements with the SLAM measurements. No significant differences can be observed so it can be concluded that SLAM is as accurate as the internal tape measurement. Figures 4.12a - 4.12b depict measurements in the two axes. 96 4. Exhaustive System Validation a b c d e f g h i j k l m n o 0 10 20 30 40 50 60 70 80 # Operation Measurement [mm] Horizontal measurements Tape Measure Monocular (a) Horizontal axis measurement comparison. a b c d e f g h i j k l m n o 0 10 20 30 40 50 60 # Operation Measurement [mm] Vertical measurements Tape Measure Monocular (b) Vertical axis measurement comparison. a b c d e f g h i j k l m n o 0 50 100 150 200 250 300 350 # Operation Measuring time [s] Measuring times Tape Measure Monocular 1−3 minutes (c) Measuring time comparison. Figure 4.12: Measurement procedure comparison. Both accuracy (a), (b) and surgical time (c) are exhaustively plotted, one bar per operation per method. Missing data are represented as a missing bar. The labels correspond with those on Figure 4.9. 4.6 Clinical Validation In addition to the engineering validation, a clinical validation has also been made. The clinical validation consists in a descriptive and comparative prospective study analyzing data from the fifteen LVHR. All LVHR were performed with a bilaminar intraperitoneal tissue-separating mesh. The mesh was fixed in place using transfascial non-absorbable sutures at the four cardinal points (four vertices of the hernia), and the edges of the mesh were fixed using absorbable tackers according to the double-crown technique [MC+05]. 4.6. Clinical Validation 97 (a) Corresponds to Figure 4.9b. (b) Corresponds to Figure 4.9c. (c) Corresponds to Figure 4.9e. (d) Corresponds to Figure 4.9f. (e) Corresponds to Figure 4.9g. (f) Corresponds to Figure 4.9k. Figure 4.13: Comparison between ground-truth internal measurements and SLAM measurements (I). Internal measurements in both axes. For each LVHR procedure, measurements were performed using three methods: the two classical methods (needle and tape. Figure 4.2), and the Visual SLAM Measurement (VSM) method. The study protocol, including the documents for obtaining informed con- 98 4. Exhaustive System Validation (a) Corresponds to Figure 4.9a. (b) Corresponds to Figure 4.9d. (c) Corresponds to Figure 4.9h. (d) Corresponds to Figure 4.9i. (e) Corresponds to Figure 4.9j. Figure 4.14: Comparison between ground-truth internal measurements and SLAM measurements (II). Internal measurements only in one axis. (a) Corresponds to Figures 4.9l, 4.9m, 4.9n (b) Corresponds to Figure 4.9o. Figure 4.15: Comparison between ground-truth internal measurements and SLAM measurements (III). (a) No internal but SLAM measurements. (b) No SLAM but internal measurements. sent from patients, were approved by Comit´e ´ Etico de Investigaci´on Cl´ınica de Arag´on (CEICA) and were in accordance with the Spanish law 14/2007 regarding biomedical research. 4.6. Clinical Validation 99 4.6.1 Surgical procedure Antibiotic and antithrombogenic prophylaxis were administered to all patients. Abdominal access was established. The edges of the parietal defect were drawn on the skin, guided by tactile localization. A pneumoperitoneum was created by inserting a Veress needle into the left upper quadrant. A pressure of 12 mmHg was used to safely separate the viscera from the abdominal wall. Three trocars were placed along a line as far as possible from the hernia defect: two 5-mm diameter working trocars, and a central 10-mm diameter trocar for the camera and for inserting the prosthetic mesh (Figure 4.16). A camera with 30oDoV (Figure 4.6) was used to examine the anterior abdominal wall, particularly the areas around the trocars. The abdominal cavity was explored to locate the viscera, identify adhesions, and locate and evaluate all hernia defects. 13 patients had a defect in the central abdominal wall or the right flank, and in these patients the ports were placed in the left flank. 2 patients had a defect in the left flank, and in these patients the ports were placed in the right flank. Figure 4.16: Trocar locations in the left flank. After creating the pneumoperitoneum, the fat and visceral adhesions were dissected from the hernia sac. Adhesiolysis was performed at the borders of the hernia defect to locate the edges of the intact abdominal wall. For adhesions close to the intestines, monopolar coagulation was avoided to avoid inadvertent perforation. 100 4. Exhaustive System Validation To assess the size of the defect without enlarging the hernia, the pneumoperitoneum pressure was reduced to 8 mmHg. The two diameters of the defect were measured to determine the required size of the prosthetic mesh using the three measurement methods. First, four needles were placed through the abdominal wall under endoscopic guidance to determine the sizes of the two main axes of the defect, which was considered to be elliptical in shape; after correct insertion of the needles, an external tape measure was used to measure the distances between them (Figures 4.2a and 4.2b). Second, a sterilized tape measure was introduced into the abdominal cavity to measure the two axes of the defect (Figure 4.2c). Third, the defect was measured using the VSM method; the surgeon fixed a forceps inside the abdominal cavity and moved the tip of the laparoscope in a cross-shape, keeping the tip of the forceps and the defect in the field of view (Figure 4.3c); after the surgery was finished, the endoscopic sequence was processed to estimate the size of the defect (Figure 4.4a); in the first image of the sequence, several points were marked: two predefined points on the forceps, whose relative distance was known, to define the scale, and five or more points at the borders of the defect to estimate the hernia contour and size (Figure 4.3a). The mesh was rolled along its major axis and grasped with forceps to insert it through the 10 mm trocar. Inside the abdominal cavity, the mesh was unrolled and oriented to cover the borders of the hernia defect. The mesh was fixed at the four cardinal points with non-absorbable monofilament sutures, and then fixed along the edges with tackers according to the double-crown technique, with 1 cm between tackers. An abdominal compression bandage was applied postoperatively. Oral ingestion was started after 8 hours and ambulation was started after 12 hours. Patients returned for a follow-up visit after 30 days. The main steps of the whole procedure for the LVHR are shown in this video [GGg]. Besides, the video [GGi] shows the needle and the internal tape measurement methods. 4.6.2 Results Fifteen ventral hernias were repaired (Figure 4.9), 9 females (60%) and 6 males (40%). The mean patient age was 42 years (range, 27–69 years). Ten patients (67%) had recurrent hernias and five (33%) had primary hernias. The mean operation time was 80 min (range, 40–120 min). Patient comorbidities included obesity (n = 9), hypertension (n = 7), smoking and alcoholism (n = 3), diabetes mellitus (n = 2), chronic obstructive pulmonary disease (n = 2), ischemic heart disease (n = 2), chronic renal failure (n = 1), and human immunodeficiency virus infection (n = 1). Six of the patients 5.2. Future Work 107 enabling the possibility of making augmented reality annotations). Additionally, these methods have shown to be non-invasive and easily incorporated in routine surgical procedures without disturbing either surgeons or classical procedures. Therefore, in the future, these methods will become an essential surgical tool in the surgeon’s armory. 5.2 Future Work Despite the promising results shown in this thesis, monocular SLAM in laparoscopy, and in endoscopy in general, still presents several issues that must be solved before a real application inside the surgical room. In the first place, the camera calibration problem should be faced with. Currently, camera calibration is performed after surgical intervention in order to avoid a possible contamination of the laparoscope, which is sterilized previous to the operation. This is one of the main issues because these algorithms are not usable in a surgical room yet. The ideal SLAM system should solve the complete problem (estimate the 3D structure of the scene, the camera location with respect to this structure, and the camera calibration parameters) during exploratory movements inside the abdominal cavity. In the second place, these algorithms work with point features extracted from images, thus they cannot deal with textureless scenes. An interesting research would be to tackle this problem by means of SLAM systems that handle point features, edges and regions of interest, or even combining them with photometric methods. In the third place, current SLAM methods assume that the scene is completely rigid. This assumption is extremely strong for internal scenes of the body. Recently, there is a great research in deformable SLAM field. Works such as those of Agudo et al. [ACM12b; ACM12a] have proved that the combination EKF-FEM can deal with deformations in real time. This approach is relevant for medical images because it can exploit the biomechanical characteristics of the tissues in order to support possible deformations instead of treating them as spurious. In the fourth place, for the particular SLAM case presented in this thesis, EKF has a quadratic computational cost in the state size (directly related with map size), therefore, it only handles a few hundred points in real time. Adapting any method based on keyframes + BA such as the proposed by Klein & Murray [KM07] would be an interesting work. These methods enable to work with a few thousand points rendering dense scene maps that help a better understanding of the scene. In the fifth place, this thesis has proposed to use the 3D reconstruction 108 5. Conclusions and Future Work as a backbone for augmented reality. The use of augmented reality has been shown with simple annotations over laparoscopic images and it has been mentioned its possible use along with multimodal registration images –CT or MRI– in real time. A research along this line would be very interesting and relevant since it would allow to show patient’s preoperative data in real time during intervention. This would help tremendously surgeon’s work during surgical procedures that entail a high level of risk. Finally, this thesis proposes to apply SLAM techniques in laparoscopy and demonstrates their feasibility over 15 ventral hernia repairs. It would be interesting, from a clinical point of view, to search for other surgical procedures that could benefit from SLAM results. Some examples are thoracic surgery (thoracoscopy), joint surgery (arthroscopic surgery or arthroscopy), or gastrointestinal tract surgery (endoscopy, colonoscopy). 5.3 Conclusiones Desde el punto de vista de la rob´otica y la visi´on por computador, la laparcosopia se puede interpretar como un problema de SLAM monocular. En la laparoscopia tradicional las im´agenes capturadas por el laparoscopio ´unicamente son mostradas en un monitor para posteriormente ser desechadas. Sin embargo, si se tratase la laparoscopia como un problema de SLAM monocular, esas im´agenes ser´ıan explotadas recuperando en tiempo real una reconstrucci´on 3D de la cavidad abdominal al mismo tiempo que se localizar´ıa el laparoscopio con respecto a esa reconstrucci´on. Los algoritmos de SLAM han sido profundamente estudiados y validados en entornos de rob´otica m´ovil (exteriores, interiores, construcciones humanas, ...), sin embargo, ning´un trabajo anterior a esta tesis, y dedicado a aplicar estos algoritmos sobre t´ecnicas endoscopicas (endoscopia, laparoscopia, colonoscopia, ...), ha validado de una forma extensiva este tipo de algoritmos. Estos trabajos hacen validaciones subjetivas, analizando la apariencia de la reconstrucci´on, o bien con maniqu´ıes, datos ex-vivo, datos in-vivo de animales, o usando dispositivos adicionales lo que hace que queden bastante lejos de una posible inmediata aplicaci´on cl´ınica. En esta tesis se ha demostrado la viabilidad de estos algoritmos dentro de un entorno cl´ınico mediante la realizaci´on de una validaci´on experimental exhaustiva con 15 operaciones reales de hernia ventral. En este tipo de operaciones el cirujano necesita medir las dimensiones del defecto herniario. Estas dimensiones han sido usadas como referencia para comprobar las reconstrucciones obtenidas por el SLAM monocular. Adem´as de la validaci´on con secuencias reales, tambi´en se han realizado simulaciones con diferentes config- 5.3. Conclusiones 109 uraciones del sistema. Tanto las secuencias reales como las simulaciones han mostrado que se puede obtener reconstrucciones en tiempo real (25 fps) con errores milim´etricos. Por otra parte, la validaci´on sobre las 15 operaciones ha demostrado la robustez de estos algoritmos ante la variabilidad interpaciente (diferentes texturas, iluminaciones, disposiciones de los trocares y trayectorias exploratorias) ya que todas las secuencias han sido procesadas con los mismos par´ametros de configuraci´on. Para mostrar la viabilidad de estos algoritmos en laparoscopia, se ha elegido un algoritmo de SLAM basado en EKF. Se ha seleccionado esta t´ecnica de SLAM por ser una t´ecnica madura, profundamente conocida, y que funciona bastante bien y en tiempo real (25 ∼30 fps) en entornos reducidos. En esta tesis se ha demostrado que un sistema de SLAM monocular funcionar´a correctamente en laparoscopia siempre y cuando tenga implementado un sistema robusto y eficaz de detecci´on y rechazo de espurios, y un sistema de detecci´on de p´erdida del tracking con su posterior relocalizaci´on. Debido a la naturaleza intr´ınseca de las im´agenes laparosc´opicas, estas son propensas a contener grandes cantidades de espurios adem´as de sufrir p´erdidas de tracking. Las fuentes principales de generaci´on de espurios y de p´erdidas de tracking son: la presencia de deformaciones temporales causadas por la respiraci´on, los latidos del coraz´on o por fuerzas externas como las ejercidas por las herramientas; las oclusiones causadas por las herramientas o incluso por tejidos u ´organos; im´agenes borrosas; movimientos repentinos del laparoscopio; o la extraci´on y reinserci´on del laparoscopio dentro de la cavidad abdominal. Todos estos problemas son eficazmente resueltos con el algoritmo 1-PR propuesto para el tratamiento de espurios y con el sistema de relocalizaci´on RLR [WKR07]. El algoritmo de SLAM utilizado asume que la escena es completamente r´ıgida. Esta asunci´on de rigidez junto con la asociaci´on de datos robusta del 1-PR han permitido que peque˜nas deformaciones hayan sido consideradas como espurios. De esta forma, se ha impedido una posible integraci´on de caracter´ısticas deformables dentro de la estimaci´on de la escena, lo que habr´ıa causado una degradaci´on de esta e incluso un fallo completo del sistema de SLAM. Finalmente, este tratamiento de las deformaciones ha dado lugar a mapas de unas decenas de caracter´ısticas r´ıgidas f´acilmente identificables, reobservables y bien distribuidas a lo largo de la escena. Este tipo de mapa permite que el algoritmo RLR se relocalice de una forma bastante eficiente ante posibles p´erdidas de tracking, habilitando el procesamiento de secuencias de exploraciones intracavitarias relativamente largas. En cuanto a las reconstrucciones obtenidas, estas han demostrado ser 110 5. Conclusions and Future Work ´utiles para ampliar sint´eticamente el FoV perdido mediante reconstrucciones fotorrealistas; recuperar la profundidad perdida por trabajar con im´agenes 2D; permitir realizar mediciones de distancias 3D en el interior del cuerpo, e incluso se podr´ıan realizar mediciones de superficies; y soportar anotaciones en realidad aumentada. Desde el punto de vista cl´ınico, estos m´etodos han demostrado reducir el tiempo de las operaciones (menos anestesia para el paciente) y aportar seguridad (tanto por evitar la posible introducci´on de elementos extra˜nos dentro del cuerpo, como por la posibilidad de realizar anotaciones en realidad aumentada). Adicionalmente, estos m´etodos son no invasivos y f´aciles de incorporar en las rutinas quir´urgicas, sin llegar a ser una molestia para el cirujano ni interferir con los procedimientos habituales. Por lo tanto, en el futuro, estos m´etodos se pueden convertir en una nueva herramienta imprescindible dentro del arsenal quir´urgico del cirujano. 5.4 Trabajo Futuro A pesar de los resultados prometedores mostrados en esta tesis, el SLAM en laparoscopia, y endoscopia en general, todav´ıa presenta ciertos problemas que deben de ser solventados antes de tener un sistema para uso en quir´ofano. En primer lugar est´a el problema de calibraci´on de la c´amara. Actualmente la calibraci´on se realiza tras la intervenci´on quir´urgica para evitar una posible contaminaci´on del laparoscopio, el cual est´a esterelizado, antes de la operaci´on. Este es uno de los principales motivos por el que estos algoritmos a´un no se pueden utilizar dentro de quir´ofano. El sistema de SLAM ideal ser´ıa aquel que permitiese resolver el problema completo (estimaci´on de la estructura 3D de la escena, de la localizaci´on de la c´amara y de su calibraci´on) durante los movimientos exploratorios dentro de la cavidad abdominal. En segundo lugar, estos sistemas, al funcionar sobre caracter´ısticas puntuales extra´ıdas de las im´agenes, no soportan escenas sin textura. Ser´ıa interesante abordar este problema mediante la utilizaci´on de sistemas de SLAM que soporten, adem´as de caracter´ısticas puntuales, segmentos y regiones de inter´es o incluso combinarlos con m´etodos fotom´etricos. En tercer lugar, los m´etodos actuales de SLAM asumen que la escena es completamente r´ıgida. Esta asunci´on es muy fuerte para el interior de cavidades corporeas. Actualmente hay una gran investigaci´on en el campo del SLAM en escenas deformables. Trabajos como los presentados por Agudo et al. [ACM12b; ACM12a] han demostrado que la combinaci´on de SLAM con elementos finitos pueden tratar las deformaciones en tiempo real. Este acercamiento es bastante relevante para el caso de las im´agenes m´edicas ya 5.4. Trabajo Futuro 111 que permitir´ıa explotar las caracter´ıticas biomec´anicas de los tejidos para soportar las posibles deformaciones sin tener que tratarlas como espurios. En cuarto lugar, para el caso particular presentado en esta tesis, el SLAM empleado est´a basado en una implementaci´on en EKF, la cual tiene un coste computacional cuadr´atico en el tama˜no del estado (tama˜no del mapa). Por lo tanto, solo se pueden manejar mapas de unos pocos cientos de puntos. Una investigaci´on interesante ser´ıa el adaptar alg´un m´etodo basado en keyframes + BA como el propuesto por Klein y Murray [KM07]. Estos m´etodos permiten trabajar con miles de puntos obteniendo reconstrucciones densas de la escena lo que ayudar´ıa a una mejor comprensi´on de esta. En quinto lugar, en esta tesis se ha propuesto utilizar la reconstrucci´on 3D de la escena como soporte para realidad aumentada. El uso de la realidad aumentada se ha mostrado con simples anotaciones sobre las im´agenes de laparoscopia, y se ha nombrado su posible utilizaci´on junto con registro multimodal de im´agenes de TAC o MRI en tiempo real. Una investigaci´on en esta l´ınea ser´ıa muy interesante y relevante ya que permitir´ıa mostrar datos preoperativos del paciente en tiempo real durante la operaci´on, ayudando enormemente al trabajo del cirujano en los procedimientos quir´urgicos que conlleven un elevado nivel de riesgo. Finalmente, en esta tesis se ha propuesto emplear las t´ecnicas de SLAM en laparoscopia y se ha demostrado su aplicaci´on sobre 15 eventroplastias. Ser´ıa interesante, desde el punto de vista cl´ınico, buscar otros procedimientos, no solo laparosc´opicos, que se pudieran beneficiar de los resultados obtenidos por el SLAM. Algunos ejemplos podr´ıan ser la cirug´ıa tor´acica (toracoscopia), la cirug´ıa en articulaciones (artroscopia), o la relacionada con el tubo digestivo (endoscopia, colonoscopia). Bibliography [ACM12a] Antonio Agudo, Bego˜na Calvo, and J. M. M. Montiel. “3D Reconstruction of Non-Rigid Surfaces in Real-Time Using Wedge Elements”. In: 5th Workshop on Non-Rigid Shape Analysis and Deformable Image Alignment (ECCV). Vol. 7583. 2012, pp. 113–122. doi:10.1007/978-3-642-33863-2_12. [ACM12b] Antonio Agudo, Bego˜na Calvo, and J. M. M. Montiel. “Finite Element based Sequential Bayesian Non-Rigid Structure from Motion”. In: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR). 2012, pp. 1418–1425. doi:10.1109/ CVPR.2012.6247829. [Ban+12] Ambar Banerjee et al. “Laparoscopic ventral hernia repair: Does primary repair in addition to placement of mesh decrease recurrence?” In: Surgical Endoscopy 26.5 (2012), pp. 1264–1268. doi: 10.1007/s00464-011-2024-3. [Ber+] Ernesto Bernal et al. “Computer vision distance measurement from endoscopic sequences. Prospective evaluation in laparoscopic ventral hernia repairs.” In: Surgical Endoscopy . Under revision (). [BMG09] Jose-Luis Blanco, Francisco-Angel Moreno, and Javier Gonzalez. “A collection of outdoor robotic datasets with centimeteraccuracy ground truth”. In: Autonomous Robots 27.4 (2009), pp. 327–351. doi:10.1007/s10514-009-9138-7. 113 114 BIBLIOGRAPHY [Bur+05] Darius Burschka et al. “Scale-Invariant Registration of Monocular Endoscopic Images to CT-Scans for Sinus Surgery”. In: Medical Image Analysis 9.5 (2005), pp. 413–426. doi:10.1016/ j.media.2005.05.005. [Can86] John Canny. “A Computational Approach to Edge Detection”. In: IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI) 8.6 (1986), pp. 679–698. doi:10.1109/TPAMI. 1986.4767851. [Cap05] David Capel. “An Effective Bail-out Test for RANSAC Consensus Scoring”. In: Proceedings of the British Machine Vision Conference (BMVC). 2005, pp. 78.1–78.10. doi:10.5244/C. 19.78. [Cas+99] JA Castellanos et al. “The SPmap: a probabilistic framework for simultaneous localizationand map building”. In: IEEE Transactions on Robotics and Automation 15.5 (1999), pp. 948– 952. doi:10.1109/70.795798. [CB12a] Toby Collins and Adrien Bartoli. “3D Reconstruction in Laparoscopy with Close-Range Photometric Stereo”. In: Int. Conf. on Medical Image Computing and Computer Assisted Intervention (MICCAI). Vol. 7511. 2012, pp. 634–642. doi:10.1007/ 978-3-642-33418-4_78. [CB12b] Toby Collins and Adrien Bartoli. “Towards Live Monocular 3D Laparoscopy Using Shading and Specularity Information”. In: Int. Conf. on Information Processing in Computer-Assisted Interventions(IPCAI). Vol. 7330. 2012, pp. 11–21. doi:10.1007/ 978-3-642-30618-1_2. [CD08] Margarita Chli and Andrew J. Davison. “Active Matching”. In: Proceedings of the European Conference on Computer Vision (ECCV). 2008, pp. 72–85. doi:10.1007/978-3-540-886822_7. [CDM08] Javier Civera, Andrew J. Davison, and J. M. M. Montiel. “Inverse Depth Parametrization for Monocular SLAM”. In: IEEE Transactions on Robotics (T-RO) 24.5 (2008), pp. 932–945. doi:10.1109/TRO.2008.2003276. [Civ+09a] Javier Civera et al. “1-Point RANSAC for EKF-Based Structure from Motion”. In: IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS). 2009, pp. 3498–3504. doi:10 . 1109/IROS.2009.5354410. BIBLIOGRAPHY 115 [Civ+09b] Javier Civera et al. “Drift-Free Real-Time Sequential Mosaicing”. In: Int. Journal of Computer Vision (IJCV) 81.2 (2009), pp. 128–137. doi:10.1007/s11263-008-0129-5. [Civ+10] Javier Civera et al. “1-Point RANSAC for Extended Kalman Filtering: Application to Real-Time Structure from Motion and Visual Odometry”. In: Journal of Field Robotics 27.5 (Sept. 2010), pp. 609–631. doi:10.1002/rob.20345. [Cle+07] Laura A. Clemente et al. “Mapping Large Loops with a Single Hand-Held Camera”. In: Robotics Science and Systems. 2007. [CM08] Ondrej Chum and Jiri Matas. “Optimal randomized RANSAC”. In: IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI) 30.8 (2008), pp. 1472–1482. doi:10.1109/TPAMI.2007.70787. [CN08] Mark Cummins and Paul Newman. “FAB-MAP: Probabilistic Localization and Mapping in the Space of Appearance”. In: The International Journal of Robotics Research 27.6 (2008), pp. 647–665. doi:10.1177/0278364908090961. [Dan+07] Koppel Dan et al. “Toward Automated Model Building from Video in Computer-Assisted Diagnoses in Colonoscopy”. In: Proc. of the SPIE Medical Imaging Conf. 2007. doi:10.1117/ 12.709595. [Dav+07] Andrew J. Davison et al. “MonoSLAM: Real-Time Single Camera SLAM”. In: IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI) 29.6 (2007), pp. 1052–1067. doi: 10.1109/TPAMI.2007.1049. [Dav03] Andrew J. Davison. “Real-Time Simultaneous Localisation and Mapping with a Single Camera”. In: Int. Conf. on Computer Vision (ICCV). 2003, 1403–1410 vol.2. doi:10.1109/ICCV. 2003.1238654. [Dis+01] M. Dissanayake et al. “A solution to the simultaneous localization and map building (SLAM) problem”. In: IEEE Transactions on Robotics and Automation 17.3 (2001), pp. 229–241. doi:10.1109/70.938381. [DW+03] H. Durrant-Whyte et al. “A Bayesian Algorithm for Simultaneous Localisation and Map Building”. In: Robotics Research: The Tenth International Symposium (2003). 116 BIBLIOGRAPHY [ED06] E. Eade and T. Drummond. “Scalable Monocular SLAM”. In: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR). 2006, pp. 469–476. doi:10.1109/CVPR.2006.263. [ED07] Ethan Eade and Tom Drummond. “Monocular SLAM as a Graph of Coalesced Observations”. In: Int. Conf. on Computer Vision (ICCV). 2007, pp. 1–8. doi:10 . 1109 / ICCV . 2007 . 4409098. [ED08] Ethan D. Eade and Tom W. Drummond. “Unified Loop Closing and Recovery for Real Time Monocular SLAM”. In: Proceedings of the British Machine Vision Conference (BMVC). 2008, pp. 6.1–6.10. doi:10.5244/C.22.6. [ENT05] C. Estrada, J. Neira, and JD Tardos. “Hierarchical SLAM: Real-time accurate mapping of large environments”. In: IEEE Transactions on Robotics (T-RO) 21.4 (2005), pp. 588–596. doi:10.1109/TRO.2005.844673. [ESL05] RM Eustice, H. Singh, and JJ Leonard. “Exactly Sparse Delayed-State Filters”. In: IEEE Int. Conf. on Robotics and Automation (ICRA). 2005, pp. 2417–2424. doi:10.1109/ROBOT. 2005.1570475. [Eve+10] Mark Everingham et al. “The Pascal Visual Object Classes (VOC) Challenge”. In: Int. Journal of Computer Vision (IJCV) 88.2 (2010), pp. 303–338. doi:10.1007/s11263-009-0275-4. [FB81] Martin A. Fischler and Robert C. Bolles. “RANdom SAmple Consensus: A Paradigm for Model Fitting with Applicatlons to Image Analysis and Automated Cartography”. In: Communications of the ACM 24.6 (1981), pp. 381–395. doi:10.1145/ 358669.358692. [FNL02] John W. Fenwick, Paul M. Newman, and John J. Leonard. “Cooperative concurrent mapping and localization”. In: IEEE Int. Conf. on Robotics and Automation (ICRA). Vol. 2. 2002, pp. 1810–1817. doi:10.1109/ROBOT.2002.1014804. [FP09] Jan Funke and Tobias Pietzsch. “A Framework For Evaluating Visual SLAM”. In: Proceedings of the British Machine Vision Conference (BMVC). 2009, pp. 69.1–69.11. doi:10.5244/C. 23.69. BIBLIOGRAPHY 123 [Sch+12] Christoph Schmalz et al. “An endoscopic 3D scanner based on structured light”. In: Medical Image Analysis 16.5 (2012), pp. 1063–1072. doi:10.1016/j.media.2012.04.001. [SDY05] Danail Stoyanov, Ara Darzi, and Guang-Zhong Yang. “A Practical Approach Towards Accurate Dense 3D Depth Recovery for Robotic Laparoscopic Surgery”. In: Computer Aided Surgery 10.4 (2005), pp. 199–208. doi:10.3109/10929080500230379. [SFS09] Davide Scaramuzza, Friedrich Fraundorfer, and Roland Siegwart. “Real-Time Monocular Visual Odometry for On-Road Vehicles with 1-Point RANSAC”. In: IEEE Int. Conf. on Robotics and Automation (ICRA). 2009, pp. 4293–4299. doi:10.1109/ ROBOT.2009.5152255. [Smi+09] Mike Smith et al. “The New College Vision and Laser Data Set”. In: The International Journal of Robotics Research 28.5 (2009), pp. 595–599. doi:10.1177/0278364909103911. [SR93] T. A. Santora and J. J. Roslyn. “Incisional Hernia”. In: Surgical Clinics Of North America 73.3 (1993), 557–570. [SS02] Daniel Scharstein and Richard Szeliski. “A Taxonomy and Evaluation of Dense Two-Frame Stereo Correspondence Algorithms”. In: Int. Journal of Computer Vision (IJCV) 47.1-3 (2002), pp. 7–42. doi:10.1023/A:1014573219977. [SSC87] R. Smith, M. Self, and P. Cheeseman. “A stochastic map for uncertain spatial relationships”. In: 4th International Symposium on Robotics Research. 1987. [ST94] J. Shi and C. Tomasi. “Good Features to Track”. In: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR). 1994, pp. 593–600. [Ste+10] Olmi Stefano et al. “Laparoscopic Incisional Hernia Repair With Fibrin Glue in Select Patients”. In: Journal of the Society of Laparoendoscopic Surgeons (JSLS) 14.2 (2010), pp. 240–245. doi:10.4293/108680810X12785289144359. [Tar81] J. C. Tarasconi. “Endoscopic Salpingectomy”. In: Journal of Reproductive Medicine 26.10 (1981), 541–545. [TBF05] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The MIT Press, 2005. 124 BIBLIOGRAPHY [Thr+04] Sebastian Thrun et al. “Simultaneous localization and mapping with sparse extended information filters”. In: The International Journal of Robotics Research 23.7-8 (2004), pp. 693–716. doi: 10.1177/0278364904045479. [TM93] Philip H. S. Torr and David W. Murray. “Outlier detection and motion segmentation”. In: Proc. SPIE, Sensor Fusion VI 2059 (1993), pp. 432–443. doi:10.1117/12.150246. [Tot+11] Johannes Totz et al. “Dense Surface Reconstruction for Enhanced Navigation in MIS”. In: Int. Conf. on Medical Image Computing and Computer Assisted Intervention (MICCAI). Vol. 6891. 2011, pp. 89–96. doi:10.1007/978-3-642-236235_12. [Tri+00] Bill Triggs et al. “Bundle Adjustment — A Modern Synthesis”. In: Vision Algorithms: Theory and Practice. Vol. 1883. Lecture Notes in Computer Science. Springer Berlin Heidelberg, 2000, pp. 298–372. doi:10.1007/3-540-44480-7_21. [TZ00] P.H.S. Torr and A. Zisserman. “MLESAC: A new robust estimator with application to estimating image geometry”. In: Computer Vision and Image Understanding 78.1 (2000), pp. 138–156. doi:10.1006/cviu.1999.0832. [Ved+05] Andrea Vedaldi et al. “KALMANSAC: Robust filtering by consensus”. In: Int. Conf. on Computer Vision (ICCV). Vol. 1. 2005, pp. 633–640. doi:10.1109/ICCV.2005.130. [Wan+08] Hanzi Wang et al. “Robust Motion Estimation and Structure Recovery from Endoscopic Image Sequences with an Adaptive Scale Kernel Consensus Estimator”. In: IEEE Conf. on Computer Vision and Pattern Recognition (CVPR). 2008, pp. 1–7. doi:10.1109/CVPR.2008.4587687. [WKR07] Brian Williams, Georg Klein, and Ian Reid. “Real-Time SLAM Relocalisation”. In: Int. Conf. on Computer Vision (ICCV). 2007, pp. 1–8. doi:10.1109/ICCV.2007.4409115. [WSC07] Chia-Hsiang Wu, Yung-Nien Sun, and Chien-Chen Chang. “Three-Dimensional Modeling From Endoscopic Video Using Geometric Constraints Via Feature Positioning”. In: IEEE Trans. on Biomedical Engineering 54.7 (2007), pp. 1199–1211. doi:10.1109/TBME.2006.889767. BIBLIOGRAPHY 125 [Zha00] Zhengyou Zhang. “A Flexible New Technique for Camera Calibration”. In: IEEE Transactions on Pattern Analysis and Machine Intelligence(PAMI) 22.11 (2000), pp. 1330–1334. doi:10. 1109/34.888718. [LeB07] LeBlanc, K. A. “Laparoscopic incisional hernia repair: are transfascial sutures necessary? A review of the literature”. In: Surgical Endoscopy 21.4 (2007), pp. 508–513. doi:10.1007/ s00464-006-9032-8.