Université De Strasbourg

Paolo Cabras

Rapporteurs M Guillaume

Morel Professeur

M Nicolas Andreff

Professeur Mme Alicia Casals

M Florent Nageotte

M Christophe Doignon

Mario Grazie Al Movimento Dei Focolari E In Particolare A Guido

Gracias

David Goyard

Florent Nageotte

Philippe Zanne

Positionnement Christophe Doignon

Pose Estimation

3D Pose Estimation of Continuously Deformable Instruments in Robotic Endoscopic Surgery

la più adatta o come si scrive... come si dice...

With the advent of Minimally Invasive Surgery (MIS), the gazeo fm e d i c i n eo n the patient health has become wider and start focusing not only on the success of the operation, but also on the post-operative and aesthetic sides. The dimension and number of scars after operation is, now, a sort of evaluation paradigm of the operation: the less invasive the operation is, the scanter the blood loss, the less the surgery and post-operative pain and the shorter the convalescence period are.

The first example of MIS dates back to 1985, when Eric Muhe performed the first LC (Laparoscopic Cholecystectomy). He encountered great skepticism by the German Medical Community, where the general surgical thinking was oriented to the concept that big problems required big incisions [START_REF] Litynski | [END_REF]]. Eric Muhe work on LC was never published and for many years the merit for the firstLCwasattributed to Philippe Mouret who performed it in 1987 [Cuschieri 1991]. However, even in the case of Mouret the work had weak responses and the diffusion of these techniques was more due to private communities or private practice surgeons. Nowadays, on the contrary, Laparoscopy is a diffuse technique and, probably, the most performed among the MIS techniques. In conventional Laparoscopy threeo rf o u ri n c i s i o n sa r e made on the skin of the patient to access the body cavity: one isu s u a l l yf o rt h e camera (laparoscope) and the other for rigid instruments. With the aim of limiting the invasiveness, LESS (Laparo-Endoscopic Single Site) surgery reduces the incisions to a single one: in this procedure all the instruments and the camera are inserted in the body cavity through a single port [START_REF] Curcillo | [END_REF]].

Along the same line, in the last decade, new surgical techniques have been developed with the aim of having no scar at all (or, at least, no external scar) leading to a family of techniques such as endoluminal surgery or natural orifice transluminal surgery (also known as NOTES). The idea underneath both of these techniques is to exploit the natural orifices and tracts to access the wanteds u r g i c a ls i t eb yt h e interior of the body. In endoluminal surgery, the operation is carried out in the interior of a tubular anatomical structure (lumen) such as urethra, colon or esophagus and no superficial incision is necessary since these structure are accessible through natural orifices. In the transluminal surgery, the lumen is used as a via to approach the site of the operation which is finally reached performing an incision in the wall of the tract that has been walked through.

These kinds of techniques are made possible by the utilization of flexible systems. Thanks to their compliance, they can reach the zone of interest in a really noninvasive way. Differently from the rigid instruments, they can enter through the natural orifices and walk through tortuous paths adapting to anatomical constraints and assuring almost atraumatic endoluminal movements. For these purposes, several devices are being developed which, often, are extension or adaptation of the already available technologies (e.g. endoscopes) whose main use wasf o rd i a g n o s t i ct a s k s (see Ch. 2)..

With respect to conventional surgical operations, the techniques using flexible instruments present a higher degree of difficulty. The relationship between the hand gestures and the actual instruments trajectories are not easily intelligible if the surgeon only relies on the endoscopic image and, furthermore, the large distance between operator and end-effectors, which is typical in flexible endoscopy, limits haptic feedback. Maneuverability also decreases due to the narrow space where camera and instruments are constrained. Moreover, positioning and field of view are maintained by a complex coordination of positioning-wheels and locking mechanism which are usually controlled by an assistant, while thes u r g e o no p e r a t e st h e endoscopic instruments which move inside the channels of theendoscope. Thepro ximity of instruments controlling handles and navigation/view control elements and the linkage of camera and instruments movements (due to the fact that the same flexible system houses the camera and the instruments) requires a solid team interaction which is not obvious. Additionally, mental workload will likely be increased by fluctuating visual frames of reference and angles of approach associated with NOTES procedures.

Motivation and Problem Statement

In this context, robotic assistance can provide large advantages in terms of manipulation easiness, ergonomics, accuracy in the gesture and repeatability [Dogangil 2010]. In the majority of the robotic systems for endoscopic (flexible or laparoscopic) surgery, the robot is thought within a teleoperation framework where the surgeon remotely controls the bedside slave system (cf. Ch. 2). In simple Master -Slave configuration, the robot is just another instrument that repeats the gestures performed by the physician with the specific human-machine interface. To upgrade the robot to be an assistant for the surgeon, autonomous movements control must be contemplated in the medical robot scenario. The next challenge, then, is providing the robots with automatic control that can help following a particular trajectory, limit the permitted movement to the zone of interest, pre-positioning the instrument for a better incipit of a specific action during an operation or, later on, accomplishing some tasks autonomously. This could both increase the precision of the surgeon movements and considerably decrease the fatigue of the surgeon. Furthermore, the movements of a specific operation can be recorded and an a-posteriori analysis of them can be exploited for educational scopes.

To accomplish this pro ject, the configuration of the rob ot must be known accurately to compute the appropriate control action. Nevertheless, obtaining the actual configuration and pose of these robotized flexible instruments is difficult. Mainly because of the transmission of the movement inside the flexible body, these systems have often a complex behavior with strong and variable non-linearities depending on several parameters. It is difficult, then, to determine a precise model and, consequently, model-based open-loop control strategies are not satisfactory. To obtain ag o o da c c u r a c yi ni n s t r u m e n tp o s i t i o n i n g ,a ne x t e r n a lm e a s urement seems to be necessary to close the loop.

Objectives

In this context, our main objective is to study the potentialities of using the embedded monocular vision to provide the 3D pose measurement offl e x i b l es u r g i c a l instruments.

Vision-based methods usually rely on particular visual features related to the 3D object whose pose has to be estimated and the precision of the pose estimation is indirectly affected by the accuracy attain in extracting these features. Therefore, the second objective of this thesis is the development of a feature extraction method that has to be effective in in-vivo environment and robust to the peculiar illumination conditions (strong central enlightenment and darker borderz o n e )a n dpo s s i b l ya b l e to manage a partial loss of visual information.

In this work, the attention will be focused on those flexible systems provided by an embedded monocular vision system and at least one working channel where an instrument can slide. The flexible instrument in question is considered to be a conventional surgical instrument currently used in manually performed NOTES or endoluminal surgery characterized by a single bendable section, a small diameter. As a first approximation it is supposed to move within a free space and no contacts nor occlusion are considered.

Thesis Overview

The solutions proposed to attain the enunciated objectives is presented in details in this manuscript which is structured in 5 main chapters.

One introductory chapter (chapter 2)i sd e d i c a t e dt ot h ed e s c r i p t i o no ft h e medical context underlying the role of flexible systems and the robotic solutions existing in this field. This kind of robots is usually inscribed in the continuum robot category and presents strong peculiarities both in the structure and drive. After a brief presentation of their characteristics and the main issues, state of the art works are presented concerning the measurements used for controlling continuum robots.

The four other chapters are arranged in two parts coinciding with two of the aspects of a visual-based pose estimation process: (I) choice and segmentation of visual features for pose estimation and (II) actual 3D pose inferring from a single 2D image visual features.

The beginning of Part I is dedicated to the detailed description of the flexible system taken as specimen, focusing on the visual perception of the flexible instrument by the endoscopic camera.

In Chapter 3,ap r e l i m i n a r yt h e o r e t i c a ls t u d yi sc a r r i e do u tt oi n v e s t i g ate the suitability of some visual features for pose estimation. Thes u i t a b i l i t yo ft h ev i s u a l feature is defined based on the sensitivity of the 3D pose estimation with respect to the error in the segmentation process. Within this scope, the direct and inverse kinematics of a conventional single bending section flexibleinstrumen tarediscussed assuming a constant curvature model. Furthermore, the geometric Jacobian (relating the variation of the robot degrees of freedom to the variation of a point of the robot) and the image Jacobian (relating the velocity of a 3D point with the velocity of its projection in the image) are computed considering, also, the effects of camera lens distortion.

The problem of extracting the visual features suggested by the theoretical study (i.e. the apparent corners of colored markers attached to thebendablesectionofthe instrument) is detailed in Chapter 4.T h ep o s s i b i l i t yo fu s i n gc o l o ri n f o r m a t i o n for feature segmentation is analyzed trying to manage those cases where color information is only partially available (e.g. overexposure regions due to strong frontal illumination). The main proposed idea is to detect the corners points as those points along the superior and inferior apparent borders where colortransition(betweentwo markers) occurs. A continuous representation of the instrument borders is proposed to achieve a subpixel accuracy in extracting these points. The results of in-vivo experiments are finally shown and commented.

In part II two novel methods are described to infer 3D information from 2D images for flexible instruments with the monocular endoscopic camera.

In Chapter 5 we analyze the potentialities of estimating the 3D pose relying on the use of a parametrized kinematic model of the system. This problem can be formulated as an optimization problem intended to minimize the differences between the synthetic (built starting from camera and kinematic model) and the actual image.

The interest of considering additional degrees of freedom inthekinematicmodel is also addressed with the intention of modeling the mechanical play between the instrument and its housing working channel. These plays are essential for the instrument to slide inside the channel (and therefore somehow unavoidable) and influence the relative position and orientation of the instrument exiting the channel and, consequently, the appearance of the instrument.

In a robotic setup, the possibility of exploiting motor encoder data to reinforce the pose estimation is finally explored together with the effectiveness of such proposal in in-vivo environment.

To avoid all the p ossible errors deriving from an uncertain and potentially incomplete model, a model-free strategy has been evaluated in Chapter 6.T h em a i n intent is to learn "from experience" the function relating the 3D position of the instrument tip to the image features.

Two common learning metho ds for regression have b een taken into consideration to study the suitability of such solution in pose estimation of flexible instruments and discuss the main issues usually deriving from high input dimensionality and poor or noisy (and therefore untrustworthy) training set.

Several simulation experiments led to the final learning method which is tested on a robotic flexible system in a laboratory environment.

Labex CAMI Context

This thesis is part of Labex (LABoratoire d'EXcellence, an ANRp r o j e c t )C A M I program "Investissement d'avenir" (french expression for "future investments"). This laboratory is actually a consortium of laboratories that share the common conviction that an integrated approach will strongly increase the quality of medical intervention and the common aspiration of diffusing CAMI (Computer Assisted Medical Intervention) technologies in routine clinical intervention.

CAMI LABEX Network intensively works in 5 areas of interests:

• Augmented Perception. Offering the operator the possibility to see beyond the immediately visible by innovative fusion of multi-modald a t ao b t a i n e db y novel or classical sensors.

• Augmented Decision. Offering assistance to real-time decision-making through high-level planning and monitoring of the intervention.

• Augmented Action. Offering the operator a new dimension in intervention performance with miniaturized robots and solutions for augmented dexterity.

• Augmented Learning. Reducing the learning curve by offering User-centered learning strategies exploiting the educational potentialities of CAMI technologies.

• Innovative Methods for medical benefit demonstration. Developing and validate an adapted methodology for the demonstration of the Medical Benefit of CAMI techniques.

This thesis can be ascribed to the first listed area since the proposed solution can be considered as a "peculiar" sensor that could eventually allows gesture guidance or autonomous positioning decreasing mental and physical burden of physicians with consequent benefit also for the patients.

Medical Context

Despite the great skepticism reserved to the first laparascopy [START_REF] Litynski | [END_REF]], Minimally Invasive Surgery (MIS) is becoming the new paradigm forsurgicaloperations. The strength behind this revolution has been the great benefits for the patient: small scars, reduced blood loss during the surgery, shorten recovery time and decrease post-operative pain convinced the patients to ask for MIS indirectly promoting the development of these techniques as they are known today. Since then, the surgery seems to focus the attention on the minimization of the invasiveness towards, ideally, an o -s c a rs u r g e r y .

In the last decade, flexible systems have acquired a fundamental role in this field due to their capacities of adapting to human body structures allowing the access of even remote surgical sites in the interior of the body throughn a t u r a lo r i fi c e sa n d tracts.

In this direction, NOTES is considered as maybe the most revolutionary concept of surgery between the MIS techniques but it does not find yet a large acceptance among the physicians. In fact, despite the enthusiasm after the first pioneering works on animal [Kalloo 2004]a n dh u m a n s [ Reddy 2004[START_REF] Zorrón | [END_REF], Marescaux 2007], it is still object of a controversial discussion. The transluminal approach appears to have tremendous potential, but several important issues,i n c l u d i n gt h es a f e t y of this approach and whether it will provide significant patient benefit in terms of postoperative recovery compared with other procedures, must be resolved before the new technique is widely introduced into clinical use. As Podolsky stated: "Patients' desire for the cosmetic benefit is important, but the price cannot be increased risk or complication." [START_REF] Podolsky | [END_REF]].

Dhumane et al. suspect that the benefits, at this point in its development, may be limited and the appropriate technology should be waited. They believe that the ultimate form of Minimally Invasive Surgery will be a hybrid form of Minimally Invasive Single Site surgery and Natural Orifice Transluminal Endoscopic Surgery, complimented by technological innovations from the fields ofroboticsandcomputerassisted surgery [START_REF] Dhumane | [END_REF]]. The necessity of an improved instrumentation is also underlined in the white paper on NOTES produced in 2006 bytheworkinggroup of surgeons and gastroenterologists (known as Natural Orifice Surgery Consortium for Assessment and Research (NOSCAR)) promoted by the American Society for Gastrointestinal Endoscopy (ASGE) and the Society of American Gastrointestinal and Endoscopic Surgeons (SAGES). In these documents severalg u i d e l i n e sa r ep r oposed for research to make NOTES a viable clinical application for patient. These areas included development of a reliable closure technique for the internal incision, prevention of infection in addition to the creation of advanced endoscopic surgical tools [START_REF] Rattner | [END_REF][START_REF] Swanström | [END_REF]]. This same necessity gave stimulus to the creation of advanced flexible systems to use for single port orN O -S C A Rs u r g e r y which, in turn, can be either NOTES or endoluminal surgery.

Flexible Systems for NO -SCAR Surgery

The first systems were just a composition of already existing endoscopic and surgical instruments: this composite repurposed equipment were rudimentary for these applications and usually implied the participation of severals u r g e o n si nt h eo pe r a t i o n and totally uncomfortable positions (Fig. 2.1).

Thus, since the first successful cases of NOTES the interest ofh a v i n gm o r e specific and ergonomic instruments increased and led to redesign the endoscopic access device itself [START_REF] Swanström | [END_REF]]. Most prototypes were modified based on the therapeutic endoscopes by augmenting the number and the diameter of the channels.

One of the first platforms to be used in NOTES on animal models was the "R-scope" by Olympus later called NOTES scope. It consists inad u a lc h a n n e l endoscope (DCE) with two bending segments. The primary segment is lockable allowing a better angle of approach and more precise tissue manipulation with the maneuvering of the second segment. The peculiarity of this endoscope were the two working channels which has lifting gates orthogonally position with respect one another allowing simultaneous lifting (vertical) and dissection (horizontal) movements (Fig. 2.2). This endoscope improves the characteristic of a conventional DCE providing a better positioning and a small degree of triangulation of the instruments but, its characteristics were not optimal for NOTES such as the limited field of view or the complexity of maneuvering the two independent sections. From a surgical p oint of view, though, these two movements aren o ts u ffi c i e n t since more dexterity is needed and triangulation must be assured to carry out a complete surgical intervention. With the term triangulation is meant the fact that the surgical tools should form a triangle pointing towards the organ so as to permit traction on tissues to facilitate dissection along normal anatomical planes. If, on the other hand, the instruments are inserted close to each other and almost parallel, the applicable forces in the horizontal direction are weak and the collisions between the two instruments are almost unavoidable complicating them a n i p u l a t i o no ft h e organs.

Another important aspect in MIS is the position of the camera wrt the surgical plane 1 The ideal solution would be to maintain the camera outside thes u r g i c a l plane so as to allow visualization of the surgical plane and prevents the camera to physically interfering with the surgical plane itself.

Am o r es u r g e r y -a i m e de x a m p l eb yO l y m p u si st h eE n d o S A M U R A Id esigned to operate within the flexible-laparoscopic paradigm. This specialized endoscope is manipulated by a remote working station and is provided with a locking overtube and two independent arms in the distal part of the scope. These arms have 5d e g r e e so ff r e e d o m( D O F )a n ds e r v ea sc o n d u i tf o rd i ff e r e n te nd-effectors. In addition to that, there is a third channel that can be used for auxiliary equipment or for suction/irrigation. The triangulation assured by these arms makes them suitable for tying sutures in addition to providing traction( F i g . 2.3). The locking overtube confers to this device more stability than the DCE but always presents the same image-perspective limitations since the arms are married to the camera. The methodology employed by the EndoSAMURAI is a "drive, parka n dm o v e " methodology. Once the target is achieved, the user locks the scope position with the over-tube and proceed to the user-interface: the assistant should solely adjust the image (as in conventional laparoscopic) without worrying of the configuration of the endoscope. The idea beneath this was to avoid the necessity of great physicians coordination which often result difficult.

An alternative to endoSAMURAI is the flexible-laparoscopic platform from Boston Scientific called direct drive endoscopic system (DDES). Theg u i d es h e a t hh o u s e s3 working channels and the user interface is an ergonomic rail-guided drive handles. By these handles the physician can move all the 7 dof of the system. The instruments can be inserted in the lumens and the depth of insertion is independent of the image. The scope is inserted in one of the channels and it can be freely rotated independently of the instruments and DDES, adjusting the horizon. Contrarily to the endoSAMURAI, it does not present a dedicated channel for irrigation/suction (which may not be adequate for intra-abdominal surgery) and the maintenance of the visualisation may require endoscope and DDES adjustments during manipulation of tissues (see Fig. 2.4).

Asimilarphilosoph yistheoneoftheIOP(IncisionOperatingp l a t f o r m )s y s t e m by USGI Medical (Fig. 2.5). It is a flexible device with 4 lumens which are used as guides for the scope (N-scope by Olympus), specialized instruments or for highflow carbon dioxide insufflation. If on one hand the triangulation is improved with respect to the DCE, on the other hand the in-line channel orientation still favors A locking overtube make it more stable thanD CEb u tal w a ys present the same image-perspective limitations since the camera is married with the instrument (photo: http://www.wjgnet.com).

(a) (b) Figure 2.4: Boston Scientific's Direct Drive Endoscopic System. The sheath houses three channel where two instrument and a scope can be inserted. The two handles let the user control all the 7 DOFs of the system. No irrigation/suction channel is present but instrument movements are independent from thec a m e r a . T h e scope is not integrated in the system and, then, it can rotate and translate independently from DDES (photo: http://www.wjgnet.com). parallelism. Work load for the IOP is high and requires skilled assistants as the primary operator shares responsibilities for instrument exchange, device orientation and scope positioning.

In parallel to the NOSCAR, the New European Surgical Academy (NESA) established in Berlin in 2004 the first European working group over NOS (Natural Orifice Surgery) which includes NOTES but also all the surgical procedures performed through all the body natural openings. After the first meetingtheydecidedtofocus their attention to vaginal/douglas route, which they considered more promising for its safety and cost-effectiveness. They claimed that this approach presented several advantages with respect to transgastrical way. Among them: as i m p l ee n t r ya n d straight view for upper abdominal procedures, the flexibility of the vaginal walls which allow inserting wide instruments without causing mechanical damage, an adequate working space (the diameter of the pouch of Douglas is suited to operative intervention) and an increased surgeon comfort which can perform the procedure seated [START_REF][END_REF]]. The NESA drew attention on the lack of adequate instrumentation in this context and develop a new device called TED (Transdouglas Endoscopic Device) conceived to adapt to the pelvic anatomy (Fig. 2.6). Even though it is not a flexible instrument, it is interesting to mention itheresinceitsapplication is no-scar surgery and it represents an alternative to purelyfl e x i b l ee n d o s c o p i ca n d instruments.

TED is a multichannel articulated rigid endoscope which can assume two different operational configurations: S and U shaped. The S-shape was designed to reach the upper abdomen (for cholecystectomy, splenectomy, ...) bending first anteriorly and then posteriorly and the U-shape variant for lower abdomen operations (appendicectomy, hysterectomy, ...). The controllable instruments (pincer and scissors) are hidden in the head of the device and, once the TED is positioned inside the body, the arms with the instruments are deployed. They can be pivoted and partly • specific for one application (not flexible) 3x2 instr.

• Good triangulation and positioning (for the specific operation) Anubis 3 3x2instruments

• Last part of instrument channels deviated → triangulation assured

• Not self supporting 2f o re n d oscopic head

• suction/irrigation channel rotated and also moved forwards and backwards. This enables tissue manipulation with traction and counter-traction in all planes. Besides thei n s t r u m e n t s ,t h eT E D head houses a camera, a light and a channel dedicated to flushing and sucking. A further channel is available as working channel. In 2009 preclinical studies should have started [START_REF] Stark | [END_REF]]b u tn of u r t h e rp u b l i c a t i o na r ea v a i l a b l ey e t .

In 2009 from the synergy of Karl Storz and IRCAD -France2 born the Anubis Scope (Fig. 2.7): a flexible endoscope specifically conceived for NOTES. It isl a r g e r than the conventional endoscope and it houses different channels: in addition to the dedicated channel for irrigation and suction, it has two mainc h a n n e l so ne a c hs i d e of the camera and a third channel beneath it. The two main channels are used to insert two flexible instruments (usually a grasper and an electric tool) which can smoothly slide and rotate. These instruments are endowed with an additional degree of freedom: the distal part can be bent on a plane (+/ -120 o )b yalev erin tegrated on the handle of the instruments (cf. Fig. 2.7 (a)). Finally, the triangulation is assured by the fact that the final section of the main channels is slightly deviated (10 o )w i t hr e s p e c tt ot h ec a m e r aa x i s . M o r ep r e c i s e l y ,t h et i po ft he endoscope is composed by two jaws (Fig. 2.7)t h a t ,o n c ec l o s e d ,c o n f e rt oi ta na r r o w -l i k es h a pe , which eases the insertion of the device through natural orifices and wall incisions.

However, as already pointed out in the introduction, these manual systems often lack of maneuverability and ergonomics, not to mention the difficulties relating to staff coordination when carrying out an operation. Robotics offered and is still offering several solutions to these needs easing the task of the surgeon and, consequently, decreasing physicians' physical and mental burdenandincreasingtheirand their patients' safety. Some of these examples are describedi nt h en e x ts e c t i o n sf ocusing on flexible robotic system after presenting a brief evolution review of medical robotics. Wider than the conventional endoscope, its body houses two main channels (one at each side of the camera) wheres u r g i c a li nstrument can be inserted and an additional channel beneath the camera. The triangulation is assured by the slight deviation of the main channels wrt the camera axis. Thanks to the knobs on the main shaft handle showni n( a ) ,t h e head of the endoscope can be oriented. In (a) also the instrument handles are shown with the integrated lever that allows to bend the tip of the instruments.

Medical Robotics for MIS

Differently from laparoscopy, which can be defined an already mature field, surgical operation using flexible instruments and devices is still an open field which is at the center of many researches. That is the reason why the most numerous examples of commercial medical robots are in the field of laparoscopy or medical operation with rigid instruments. Nowadays, the most known medical robot for laparoscopic surgery is probably the DaVinci platform, but it represents the last step of a longp a t h w a y ,w h i c h is worth mentioning here and which is still in vivid evolutionp r e s e n t i n gn e wl e s s invasive solutions for surgery.

Medical Robotics Evolution

It was 1985 when the first robot was introduced in an operative room to perform biopsies [START_REF] Kwoh | [END_REF]]. But in that case it was an industrial robot (PUMA 560) employed for medical purposes.

The first robot designed for medical application was ROBODOC which was actually capable of autonomous movements, assisting in Total HipArthroplast y(THA). In the same period (1994), Computer Motion got the FDA clearance for AESOP (Automated Endoscopic System for Optimal Positioning): a robotic arm holding an endoscopic camera which could be voice controlled by the surgeon. This robot represented the first try to solve the problem of team interaction and coordination even though voice control may be not always easy and practicalw i t h i nas u r g e r y and operating room (OR) context. An important breakthrough arrived with the first teleoperated surgical platforms for laparoscopy: Zeus by Computer Motion (finally incorporated in Intuitive Surgical) and DaVinci by Intuitive Surgical. The first was composedofasurgeoncon trol console and of a patient cart where three robotic arms were mounted. The right and left 7-dof robotic arms replicate the movements of the right and left hands of the user while the third arm is an AESOP voice-controlled endoscope holder used for visualization. The surgeon is seated upright in front of av i d e os c r e e na n dt h e instrument handles are adjusted to maximize comfort and dexterity. The idea underneath DaVinci is similar to Zeus: the system is composed byt h r e er o bo t i ca r m s (one for the dual camera and two for the instruments) which canb ec o n t r o l l e dr emotely at a console. One of the peculiarities is that the imagei sa3 Di m a g ew h i c h is displayed above the hands of the surgeon so that it gives thes u r g e o nt h ei l l u s i o n that the tips of the instruments are an extension of the control grips, thus giving the impression of being at the surgical site.

The Master-Slave paradigm seems to be one of the most diffuse paradigm for medical robotics for surgery: the robot is just a more precisea n dc o m f o r t a b l et o o l that is always controlled by the surgeon.

Since then, new robots for general laparoscopy were presented and conceived. Two further examples worth mentioning can b e the Raven I I and the European MIRO and ALF-X. The Raven is an open-architecture surgical robot for laparoscopic surgery research from Applied Dexterity. It has two cable-driven 7 DOF arms (6 DOF + grasp) and it is intended to facilitate collaborative research on advances in surgical robot [START_REF] Kehoe | [END_REF]]. An upgrade of the Raven II is the Raven IV which is composed of 4 arms and two cameras allowing the collaboration of two surgeons interacting with the surgical site in teleoperation.

Together with the more known MIRO (DLR, Germany), the TELEALAP ALF-X is one of the newest european proposal for robotic laparoscopy [Gidaro 2012]. ALF-Xw a sb o r nf r o mt h ec o l l a b o r a t i o no fN E S Aw i t ht h eJ o i n tR e s e a rch Center of the European Commission and SOFAR S.p.A. (Milan, Italy). The great peculiarities of this system are the tactile sensing and the eye-tracking system which not only allow to control the camera view just by looking in the wanted direction but also to activate the instruments by merely looking at them. Further advantages are the universality, wide range of application for every-day usage and cost-effectiveness. Preliminary good clinical results have already been achieved [Fanfani 2015, Gidaro 2014].

In the single port approach, two products are about to be launched on the market: Da Vinci's Sp system and Titan Medical Inc's SPORT. The former appears as a rigid tube where three instruments and a stereo camera area l l oc a t e d . O n c ei n the body the instruments exit the tube and thanks to their distal bendable sections can assure triangulation during the operation. The stereo camera can be controlled as well, providing a 3D image from an overhead point of view. Iti sm e a n tt ow o r k with the Da Vinci Console and should be launched in late 2015.

The latter derives from IREP (Insertable Robotic Effector Platform) project by Columbia University who signed an exclusive agreement with Titan Medical. It is ar i g i dt u b u l a rs t r u c t u r et h a t ,o n c ei n s e r t e di nt h eb o d yt h r ough a single incision can be deployed. In the unfold configuration three parts can bed i s t i n g u i s h e d : t w o dexterous flexible arms and an upper view stereo camera. Each flexible instrument is composed of a two-segment continuum robot, a parallelogram mechanism for improved rigidity and a distal wrist for improved dexterity during suturing.

Other examples for single port surgery are ARAKNES SPRINT which is a dualarm robot with six degrees of freedom per arm plus one tool or gripper [START_REF] Piccigallo | [END_REF]] and, very similar to this, the Virtual Incision's new robot for general abdominal surgery.

At o t a l l yd i ff e r e n tp a r a d i g mi nt h i sfi e l d ,i sb r i n g i n gt h ee n t ire robot where the operation is needed. In this direction, the project ARES had the objective to develop modular miniature robots being able to re-assemble once got to the desired position (the main application was the gastrointestinal surgery). The idea is that the patient could swallow different pills (i.e. the different components of the robot) which will self-assemble thanks to magnetic anchoring. Each componenth a si t sp a r t i c u l a rr o l e (image control, communication, diagnostics and others) inserted in a collaborative framework to carry out a specific task. As one can imagine seen the complexity of the concept, this clever idea has several implementation issues and it is still under investigation.

Flexible Robots in Medicine

Differently from robotic laparoscopy with rigid instruments, the market does not offer many examples of flexible robotics for surgery because oft h eg r e a tc h a l l e n g e s that robotizing miniaturized flexible instruments entails.O nt h eo t h e rh a n d ,t h e research in this field is fervid indicating the great interests of both roboticians and physicians on this subject. Many different architectures have been proposed in the last years for medical applications and more specifically forNOTE Sorendoluminal surgery [START_REF] Traeger | [END_REF], Seneci 2014[START_REF] Simaan | [END_REF]]. In the next paragraphs we will describe some representative examples to show the philosophy beneath 3 different categories of conception.

The first category could be defined as those robots being born by the assemblage and automatization of conventional endoscopes with the necessary instruments.

Aw ellkno wnsysteminthiscategoryistheMASTERsystemen tirely developed in NTU (Nanyang Technological University) by the group leaded by prof. Phee (Fig. 2.11). For sake of intuitiveness in using the robot, they decided that the instruments should mimic the arm movements. Therefore, they conceived the attached instruments with 5 dof (plus one additional for manipulatingt h ee n de ff e c t o r )a n d tendon-sheath actuation is used to assure flexibility. The anthropomorphic data of • Sensor in the tip.

• Physician controls the tip trajectory, the system measures tip position and instructs the 16 articulations to follow the leader. FLEX General Endoscopic Surgery 30

• Self Supporting ← location and point of view can be chosen properly.

• 2s i d e -c h a n n e l sf o rM A N U A Li n s t r u m e n t s . • Master console controlled by surgeon arms.

• An endoscopist must hold the camera.

STRAS

General Endoscopic Surgery 10

• Robotization of Anubis. 3x2 instr.

• Not self supporting.

• All DOFs can be telemanipulated.

• Instrument insertion independent from camera.

Heart Lander

Heart Operations

• No lumen constraint needed.

• Adheres to organ walls (inchwormlike locomotion).

• No scope → external imagery system needed for guidance.

Bio-Inspired

Abdominal Single Port Surgery -• Still of big dimensions (wrt STRAS for example).

• Possibility to modify stiffness → alternatively compliant and high forces application. the surgeon's arms are measured by the master console and transmitted to the robot as inputs.

Another example in this category is the STRAS (Single access and Transluminal Robotic Assistant for Surgeons) robot (Fig. 2.12). Also conceived for a tele manipulation framework, the slave robot resulted from the robotisation of the Anubis platform by Karl Storz (which has been presented previously). The master interface is composed by two handles whose degrees of freedom are similar to those of the flexible instruments so as to assure an easy mapping between the two. The shape of this handle is conceived to be familiar to the surgeon and it isa na d a p t e dv e r s i o n of the flexible instruments of the Anubis platform. An additional joystick is added on the tip of the handle for controlling the endoscope movement during operation without the need of switching pedals or any assistant [De Donno 2013]( c f . P a r tI for more details).

University of Twente has also developed his solution consisting in 3 add-on steering modules: a tip steering module, a shaft manipulator and aninstrumen tmodule. Together they allow to control the degrees of freedom (required to position and stabilize the endoscopic tip and the instruments) with a master console (Fig. 2.13).

The unique example of commercial flexible robot for surgery is Flex by Medrobotics (Raynham, Massachusetts), which received the CE mark in 2014a n ds t a r t e dt ob e sold in Europe and only in 2015 the FDA clearance. This robotice n d o s c o p e( F i g . 2.14(a))c a nb em a n i p u l a t e dw i t haj o y s t i c ka n d ,t h a n k st oi t sm o r et h an 30 dof, enhances the physician ability to reach hard to access areas of the inner body. Moreover, it is self supporting and can generate a unique shape in 3D following the user input. The necessity of working inside a natural lumen constraint is avoided and a better point of view or location can be chosen properly. The two side channels can house two flexible instruments (usually to grasp or cut) manipulated by the physician. Conversely, FLEX can also become limp to adaptt ot h es u r r o u n d i n g anatomical structures. Originally conceived for heart surgery, a recent study showed its utility also in transoral surgery [Johnson 2013].

The original idea behind FLEX is HARP, which was developed in Carnegie Mellon by Choset and Degani [START_REF] Degani | [END_REF]]. HARP (highly articulated robotic probe) was conceived to fuse in the same device the advantageso fb o t hr i g i da n d flexible instruments (Fig. 2.14(c)). The probe was actuated by four cables and the distal link was relayed to a camera by a fiber optic endoscope attached to the outer mechanism. The HARP consisted in two concentric tubes that can alternate between rigid and limp states. The self sustainability is assured by alternating the motion and state of the inner and outer mechanisms. The same robot is at the base of the CardioARM developed by the startup Cardiorobotics located in Pittsburgh.

It has 102 dof and can follow whatever 3D curve in the space (A Novel Highly Articulated Robotic Surgical System for Cardiac Ablation).

Enlarging the boundaries of this group including colonoscopy, NeoGuide presented the Navigator Endoscopy System (NES) which allows to avoid the looping problem which occurs when the forces of the scope stretches the colon (Fig. 2.15). Looping is associated with pain and need for sedation and consequent procedural difficulty and procedural time. The physician inserts the scope and controls the tip of the scope, the sensor attached to the tip measure its position and create a 3D map of the colon (actually the map of the path it made) and, based ont h i si n f o r m a t i o n , the system instructs the 16 articulating segments composing the scope to follow the path.

A second category is the one of robots for cardiovascular intervention. No endoscope is employed here since smaller structures are needed ino r d e rt ow a l ka l o n g veins or through membranes. The guidance of these tools is usually performed using an external imagery system (X-ray or others).

The most popular in this category may be Sensei X for electrophysiology operations and the Magellan (both by Hansen Medical, Inc) for intravascular operations. Ab e d s i d er o b o t i ca r mi sh o l d i n gt h ec a t h e t e rw h i c hi st e l e o p erated by the physi- cians acting on the master console. Sensei X catheter (Fig. 2.16)i sc o m p o s e db y two guides with different stiffness and magnitude of deflection. In beating heart operations, the outer stiffer guide is used till reaching the atrium of heart when the inner guide is drawn to allow more articulated movements. Thanks to the accurate force sensor embedded on the tip, Sensei X is capable of keeping the tip in contact with the tissue or applying a desired pressure autonomously. The stability of the catheter and even the pressure it exercises on heart tissues are of great utility, above all in complex ablation procedures such as atrial fibrillation [Di Biase 2009].

The master console can be installed far from the slave cart preventing the physicians of being repeatedly exposed to noxious radiations. Theb a s i cs t r u c t u r ei st h e same for Magellan for which just the catheter changes which are more adequate for intravascular procedures.

Another interesting proposal comes from Catheter Robotics,Inc. withitsAmigo System (Fig. 2.17). It was designed to be easy to use and cost effective. Contrarily to Sensei X, it does not require proprietary catheter: the slavec o m p o n e n ti sa c t u a l l y as i m p l em e c h a n i c a li n t e r f a c et os e l e c t e dc o m m e r c i a l l ya v a ilable catheters whose handle can be placed in the robot docking station. A particular handle replicating the one of the conventional catheter allows the remote control of the catheter as if it were manipulated manually. Thanks to the remote control (up to 30.5 meters ) the radiation exposure of the physician is reduced. Unfortunately, no force sensor is embedded in the tip.

In addition to the mentioned commercial examples of teleoperated catheters, one robot for heart surgery worth mentioning is the Heart Lander born by the collaboration of Carnegie Mellon and Pittsburgh Medical Center. Much bigger than ac a t h e t e r ,i tc a nb ei n s e r t e dt h r o u g has m a l li n c i s i o nb e l o wthe xiphoid process of the sternum, avoiding any intervention to the lung. It adheres to the epicardium In the original idea of [Stilli 2014[START_REF] Maghooa | [END_REF], the body of the robot is a conic-shape air chamber where the TCP (Tool Center Point)i sr e p r e s e n t e db y the cone vertex. Three pairs of nylon tendons are attached on the surface of the cone and with proper actuation on them the robot can assume different shapes (see Fig. 2.18).

Another renown example is STIFF FLOP, a soft robotic arm that can squeeze through a standard 12mm diameter Trocar-port, reconfigure itself and stiffen by hydrostatic actuation to perform compliant force control tasks (Fig. 2.19). The project objective is to design a soft manipulator with a gripper at the tip, distributed sensing, biologically inspired actuation and control architectures, learning and developing cognition through interaction with a human instructor, and manipulating soft objects in complex and uncertain environments.

The idea within the surgical framework is to insert the uninflated robot inside the body and modify its stiffness regulating the internal air pressure or the shape actuating on cables. In fact, the flexible instrument presenta ni n t r i n s i c" c o n t r adiction": they need to be soft enough to be compliant with anatomical structure but hard enough for effective force transmission and weight resistance [Loeve 2010]. In the studied context, then, the possibility of changing this quality can be very valuable .

All the cited robots differ from the conventional rigid link robots in several aspects concerning actuation, dynamic and shape variability. They are usually classified in a specific family called continuum robots.I ft h efi n a la i mi st r y i n gt oa s s i s t the surgeon improving his/her skills by the means of the robot, the characteristics and behavior of continuum robots must be known, so that automatic control actions can be applied to these systems. In the next sections,c o n t i n u u mr o b o t s are presented (Sec. 2.2)a n ds t a t e -o f -t h e -a r ts o l u t i o n sf o rt h ec o n t r o la r ed e s c r i bed, focusing on the surgical environment (Sec. 2.3).

Continuum Robots

From the classification of Robinson and Davies [START_REF][END_REF]], a continuum robot does not contain rigid links or rotational joints and their structures can bend continuously along their length via elastic deformation. Continuum robots must not be confused with Serpentine Robots which combine very short rigid links with a large density of joints. These robots are not very common and the only commercial example are the snake-like arms by OC Robotics [oc 2015]. In this manuscript, though, serpentine robots will be considered as continuum robots in the case they are under-actuated or, more specifically, when the joints andl i n k sa r ep a s s i v ea n d function only as a support structure (the backbone) which is deformed by external actuation.

Within continuum robots, a broad classification is proposed according to where the actuators are placed on the robot. They are named intrinsic if the actuators are on the structure of the robot and form part of the body of the mechanism, extrinsic if all the actuators are separated from the body and the movements is transmitted via a mechanical linkage and hybrid if a combination of the twos t r a t e g i e si su s e d .

As it can be deduced also from the previous examples, extrinsic actuation seems to be more suitable for surgery since it allows obtaining smaller and more lightweight structures which are fundamental characteristics in this field. In this class, two families can be distinguished according toh o wt h er o bo ts h a pei s obtained. The first is formed by the concentric pre-bent tubesw h e r et h es h a p ei s obtained as result of the interaction of the different component of the robot. The other one includes those robots usually composed by a passivebackbonewhoseshape is obtained by an external actuation e.g. using forces (whichcanbetransferredusing either cables or muscular hydrostats) or Shape Memory Alloy (SMA) actuators.

External Actuation

Tendons are mayb e the most direct way of external actuation. Group of tendons are usually routed along the backbone and terminate at different levels along it: all the tendons of a same group terminate at the same level dividing the structure into so-called bending sections.T h ef o r c e sa p p l i e da tt h eb a s eo ft h et e n d o n sa r e translated in torques at the termination points, hence, producing bending. The final shape of the backbone is then resulting by the different shape of the different sections. Tendon driven systems are usually conceived as extrinsic continuum robots and the actuators driving the cables are usually located at the base of the robot. One possibility of realization is using spring backbone [START_REF] Mehling | [END_REF][START_REF] Zhao | [END_REF] but the intrinsic natural compliance increases the difficultyi nc o n t r o l l i n gt h e m : control efforts intended for bending are lost in compression.A s o l u t i o n i s u s i n g incompressible backbones using rigid vertebrae [Bardou 2010, Shang 2012], but this means loosing extension capability. In this design, the use of tendons allow high forces transmission but it implies finding some method to prevent backslash and slack.

Alternatively to cable driven instruments, SMA (Shape Memory Allow) can be used as external actuation for a continuum robot. In [Szewczyk 2001], two SMA springs, mounted in an antagonist configuration, are used to change the relative orientation of two consecutive backbone segments. The idea under SMA is that SMA actuators undergo a micro-structural transformation from their austenite phase to their martensite phase. Heating and cooling the material or applying an external stress, this transformation is activated and the actuator changes its shape and stiffness. However, they may not provide a large enough deflection range and their performances seem to be not sufficient yet when carrying weights or when submitted to strong traction.

Athirdactuationmodalit yisusingm uscularh ydrostats. Inthe Bionic Handling Assistant by FESTO [festo 2015], for example, the 3D printed structure is used as air chamber that can longitudinally extend or compress. In the OctArm by the Penn State research team [McMahan 2006], instead, "air muscle" actuators are used which consist of particular covering latex tube with a plastic mesh sheath conferring to the muscle a large strength. By combining three or more of these actuators and combining the pressure in each of the chambers extension/compression or bending of each section of the robot is achieved. A similar concept wasu s e db yR o s ae t al. [START_REF] Rosa | [END_REF]

Concentric Tube Design

An alternative design is to use a backbone composed by concentric tubes that are free to rotate and translate one with respect to the other. Thes t r u c t u r ec a ne xtend/contract by longitudinal translation and rotate by rotational sliding of the tubes. Backbone shape modification is usually achieved by using pre-curved compliant tubes in combination with directly controllable relative translation and rota-tion. This kind of actuation allows having thin design but, contrarily to the tendon driven system, the bending cannot be actively controlled. They encountered a high diffusion in medical field for their smaller profile and great compliance, even though they provide less forces with respect to tendon-driven mechanism. In this context they are also known as active cannulas and have been used to carry out various MIS operations permitting task oriented designs [START_REF] Torres | [END_REF] ]orMRIimage-guided operation [START_REF] Su | [END_REF]].

Control of Continuum Robots

The design of automatic controllers is a fundamental issue for any robotic system and become more complicated for underactuated systems such as the described continuum robots. Additional difficulties derive from the usual absence of adequate sensors along the structure, the fact that actuators do not coincide with space variables and, above all, the complexity of their dynamic models.

Slack and backlash are two main phenomena which complicate the achievement of a precise model of tendon driven systems. If the cables are not properly pretensioned, there is a delay between the start of the motor movement and the actual deformation of the body of the robot. The initial movement of the motor is used to tension the slack cable in order to transfer the motion to the robot section through the cable. Same phenomena can be observed when a change in the direction of the motor occurs. For example, if antagonistic cable are employed for achieving bending in a plane, when the direction of rotation is changed the cablet h a tw a sp a s s i v ei s slack and the initial movement of the motor is dedicated to recover from this loss of tension. Friction b etween cables and backb ones vertebrae and b etweencablesandthesheath is the other main factor of complexity. In presence of friction, the relationship between traction forces and the shape of the robot body becomes more and slack becomes more severe [Agrawal 2010a].

Active cannulas do not have totally predictable behavior neither and, also suffer of friction between the tube surfaces. This can lead to torsion and traction of the tubes making a precise modeling of the motor to end effector relationship difficult.

To overcome these problems, several control strategies haveb e e na d a p t e df o r continuum robots throughout the years. The employed strategies can be classified mainly in three categories: model-based open loop approaches (1), closed-loop control strategies where the position/shape measurement is performed using attached external sensors (2) or using vision-based methods [START_REF] Broomhead | Multivariable functional interpolation and adaptive networks[END_REF]. Camera is still an external sensor but, given the specificity of the information processing they require, it has been chosen to separate them from category (2).

Kinematic and Dynamic Model-Based Methods

Despite the difficulties in obtaining a precise model, many studies have been made to transfer kinematic and dynamic control from rigid link to continuum robots, investing strong efforts to obtain general kinematic [START_REF] Chirikjian | [END_REF], Jones 2006, Gravagne 2000]a n dd y n a m i c [ Chirikjian 1993, Mochiyama 2003]m od e l s .

In [Ivanescu 1995], Ivanescu et al. proposed a variable structure controller for a tentacle manipulator, whose structure was made of flexible materials in conjunction with active controllable electro-rheological fluids. A non-linear observer, based on as i m p l i fi e dd i s c r e t es p a t i a lm o d e l ,w a sa d o p t e dt ok n o wt h e" shape" state variable on the whole length of the arm. To avoid the complexity of dynamic model, the same authors introduced in [Ivanescu 2004]anenergybasedcon trolla wrelyingonly on those relations of the dynamic model that determines the energy stored in the system. In both cases, only simulation results were presented.

Inverse kinematic and dynamic models are also at the base of the work in [Kapadia 2011], where the first task space controller for continuum robot isp r esented. The trajectory is defined in terms of contact point (position, velocity and acceleration) between the Octarm robot [START_REF] Tatlicioglu | [END_REF] ]andthemanipulatedobject, allowing, then, grasping and manipulation instead of only shape configuration tracking.

Other works in cable-driven continuum robots try to directlyc o m p e n s a t et h e sources of error in the model of mechanical transmission of the movements. Usually, actuator and tip positions are measured and the hysteresis loop, due mainly to backlash and frictions, is determined and, then, used in a feedforward control scheme [START_REF] Kesner | [END_REF], Agrawal 2010b]. Furthermore, decoupled inverse kinematics has been described to allow for independent control of multiple sections [Camarillo 2009].

In medical field, model based approaches (with or without backlash compensation) have been extensively studied and applied for differentd e s i g n sa n da c t u a t i o n modalities: active cannulas [START_REF] Webster | [END_REF][START_REF] Dupont | [END_REF]andtendondrivencatheter or instrument [Camarillo 2008b, Agrawal 2010b, Xu 2006].

The works cited here are just a small panorama of all the model based solutions for the continuum robot control. However, it seems that this approach presents some inherent limitations deriving from the complexity of describing all the phenomena occurring in mechanical transmission. They still suffer froml a c ko fa c c u r a c yd u et o unavoidable kinematic approximation and consequent modeling mismatches (at the time of implementation), actuation coupling between the different vertebrae not to mention the friction, extension and torsion of the actuationl i n e s .M o r e o v e r ,i nt h e case where the continuum robot to control is inserted in a flexible shaft (such as for STRAS) the backlash behavior of the flexible instruments changes with the shape of the housing flexible shaft. Trying to model all these behaviors seems to result in ad-hoc solutions which are not smoothly transferable to other systems.

This suggests the necessity of having an external measurement of the robot tip position or shape configuration to trustfully close the control loop. Some authors who proposed specific model based controllers, showed, in later works, the interest for combining model ap r i o r iknowledge with actual measurements [START_REF] Agrawal | [END_REF]] or trying being independent from dynamic model [Yip 2014].

External Sensors

External sensing of continuum robot shape has also been demonstrated for example using electromagnetic (EM) field sensors. The electromagnetic sensor system is usually composed by a magnetic field generator and several (upt o5 -7 )s e n s o rc o i l s whose position and orientation can be retrieved when immersed in the generated magnetic field.

In [START_REF] Agrawal | [END_REF]], the authors propose an adaptive robust control which combines EM sensors feedback with the actuator feedback. They obtain better results than using pure open-loop approach, but they also denote the necessity of improving the used dynamic model (of cable actuation) to make the results applicable to other flexible instruments.

In [Bardou 2010]a n d [START_REF] Bardou | [END_REF]], the EM sensors are applied in a tendondriven flexible system similar to STRAS: one coil attached at the endoscope head and another at the flexible instrument tip. Thanks to the sensed positions and taking into account the non-linearities, the entire shape oftheflexibleinstrumen tis computed improving the control of such instrument. EM sensors were also adopted in [START_REF] Penning | [END_REF]]t or e t r i e v et h et i pl o c a t i o nf o rc o m p l e t i n gaC a r t e s i a nc o n trol of the catheter prototype. Nevertheless, the reliability of their position and orientation measurements decreases if the sensor coil is outside the center of the generated magnetic field. To overcome this problem Reichl et al. [START_REF] Reichl | [END_REF]]p r o p o s e dt o utilize a robotic arm to move the magnetic field generator so ast ofo l l o wt h ese n so r coil attached to a catheter, showing the feasibility of electromagnetic tracking in an extended workspace.

Despite this artifice, EM sensors still remain very sensitivet oe l e c t r o m a g n e t i c disturbances and are affected by metallic tools, which limitst h e i ru s e f u l n e s si n clinical applications. In addition, mounting an EM tracker in the distal part of a tool which is inserted in the human body is a challenging task. Moreover, some actuated flexible instruments achieve high curvature radius and the continual solicitation due to repetitive deflections may lead to break the filament of the sensor.

The same problem can occur integrating fiber optics into the flexible instruments for pose estimation purposes. It seems that small curvature radius cannot be achieved and that the bulky structure necessary as support isn o ty e ts u i t a b l ef o r surgical applications where the possibility of downscalingt h ei n s t r u m e n ti sf u n d amental [START_REF] Roesthuis | [END_REF]].

An interesting alternative is shape sensing using vision by mono or multi camera systems, which can be external or embedded. In the next section, some examples will be presented, taking into account the medical context and the objectives of the thesis which are focused in founding trustful measurements for controlling flexible instruments in flexible endoscopic surgery.

Pose estimation of continuum robots using vision

One of the first and most cited work where vision is employed to sense the shape of continuum robots is the one by Hannan and Walker [START_REF] Hannan | [END_REF]], where they show their results using the multi-section Elephant's TrunkM a n i p u l a t o r . I nt h i s work, they use the robot as a planar robot moving on a plane perpendicular to the optical axis of the camera. The vertebrae of each section are detected in the image and the total shape of the robot is retrieved by fitting circumferences to the segmented vertebrae: one circumference for each set of vertebrae pertaining to the same robot section.

The same camera-robot configuration is employed in [Yip 2014], where a single bending section planar robot is controlled even in case of contact with obstacles.

Here the camera is used to compute the displacement of a colored optical marker attached to the tip. After initialization, the estimation oft h ej a c o b i a no ft h em anipulator is updated according to the measured displacements from actuators and end-effector sensors.

To estimate the p ose for any 3D configuration (not only planar movement) Camarillo et al. [Camarillo 2008a]in v estigatedthepoten tialit yofav o xelcarvingtec hnique to retrieve the 3D shape of a single section bendable catheter. In this work, they use 3 cameras orthogonally placed and looking at the sames c e n es u r r o u n d e d by a green background to ease the detection.

Ad i ffe r e n tm u l t ic a m e r aa p p r o a c hi su se di n[Web er 2012]wheremicrocameras are mounted on the body of the flexible robot and their relativep o s i t i o n sa r ec o mputed by performing a bundle adjustment over tracked features visible by multiple cameras.

Unfortunately, these approaches would be difficult to directly transfer in surgical application since they rely on hypothesis hardly verifiable in-vivo:u s u a l l yt h eb a c kground and point of view are not as favorable as in [START_REF] Hannan | [END_REF]]a n d [ Yip 2014] or multiple view techniques would mean more laparoscopes insertion in the case of [Camarillo 2008a]. The interesting work in [Yip 2014]s h o u l db ee x t e n d e df o r 3D movements assuring the visibility of the marker whatever the robot configuration is (even outside the plane). For [Web er 2012], the integration of cameras on the flexible instruments would be difficult and relative motionb e t w e e nc a m e r aa n d environment may complicate tracking and features correspondence.

Another possibility is to use stereo-vision to retrieve a more precise depth information. Stereo vision is used by Croom et al. to detect the shape of a concentric tube based on Self-Organizing-Maps [START_REF] Croom | Visual sensing of continuum robot shape using selforganizing maps.I nR o b o t i c sa n dA u t o m a t i o n( I C R A ) ,2 0 1 0[END_REF]]. Also researchers developing the IREP robot have opted for embedded stereo vision to catch a more trustful depth information. In [START_REF] Reiter | [END_REF][START_REF] Reiter | [END_REF], they proposed to learn the relationship between the visual information and the 3D position of the instrument. Some colored rings where attached around the flexible instruments to ease the detection and the computation of the visual information, which results from the composition of 2D descriptors on each of the stereo image.

Shape estimation of deformable instruments is a problem similar to that of estimating pose and shape of threads or tubes. In [START_REF] Padoy | [END_REF]]at e x t u r e dt h r e a di s tracked by modeling the thread as a B-spline directly in 3D and optimizing the 2D error between the actual detected thread and the reprojected B-spline. Here, stereo vision was adopted to discriminate ambiguous configurations. In [START_REF] Caglioti | Reconstruction of canal surfaces from single images under exact perspective[END_REF]], the 3D pose of a tube is retrieved using solely one image and geometrical properties related to the apparent borders of the tube. The image of the tube is thought like the projection on the image plane of a series of circumferences. Flex and bitangent points are taken as "fiducials" points for creating the correspondence between upper and lower border points along all the apparent borders. The position of each couple of points and the tangents at these points are used to retrieve the position and orientation of the associated 3D circumference.

In the domain of endoscopy, where, usually, only monocular vision is available, the works in the literature become more rare. The only example, at the best of our knowledge, is the work by Reilink et al. [START_REF] Reilink | [END_REF][START_REF] Reilink | [END_REF]. They propose to use the monocular camera of ANUBIScope to estimate the pose of the flexible instruments inserted in the lateral channels. Supposing known the mechanical structure of the system, they retrieve the 3D tip positionoftheinstrumen tfrom fitting the virtual model to the actual image. They actually present two methods: one is marker-less and the other one is relying on green markers attached on the instrument to ease features extraction.

Conclusion

Robotics solution for flexible endoscopic surgery are makingi n r o a di nt h eM I S scenario due to the increasing interest on no-scar or single-scar surgery techniques. One common possibility is to imagine the robotic system within a teleoperation framework where the surgeon closes the feedback loop correcting his/her movements according to what he/she observes in the endoscopic image.

To convert the rob ot into a real assistant for the physician (providing gesture guidance, autonomous positioning, ...) an automatic control strategy should be adopted. Due to the complexity in defining a complete and precise dynamic model and the dependency of such model to the particular system (or even configuration), open-loop control strategies are not the most adequate solution for retrieving real robot 3D pose.

The need of an external measurement arises, but the difficulty of attaching external sensors (e.g. EM sensors) and their fragility and possible incompatibility with other OR devices, bring to the idea of using only the embedded endoscopic camera to retrieve the 3D position of the flexible instruments.

Stereo-vision would permit to capture a precise depth information, but, since we are interested in conventional flexible endoscopic systems, which usually mount am o n o c u l a rc a m e r a ,s o m ea -p r i o r ii n f o r m a t i o nm u s tb ec o n s i dered to retrieve 3D pose from a 2D image. Pure geometrical and perspective based approach, though, seems to be very sensitive to noise (as will be explained next)a n dt h eu s eo fafi x mechanical model would fail in case of mechanical plays, which would modify the mechanical model.

Thus, a new method is needed, which must be effective with conventional endoscopic systems configurations and robust even in in-vivo conditions. Two novel solutions are proposed to retrieve the 3D pose of the instrument from a single image. The first proposes to extend the mechanical model so as to take into account possible mechanical plays or model changes during the operation. Given the difficulty to satisfactorily model how the instrument is perceived by the camera, the second solution is a model-free approach to avoid problems related to model uncertainties. As the visual based technique accuracy is strictly related tot h ec h o s e nf e a t u r ea n d the precision with which these features are extracted, we decided to focus in both the aspects of the "pose from image" problem. Therefore, the first part of this thesis is dedicated to the selection of adequate features for 3D posee s t i m a t i o na n dt h e segmentation of these features even in in-vivo scenario. The second part details the mentioned 3D pose estimation solutions.

As described in the introduction. the main objective of this thesis is the pose estimation of flexible miniaturized instruments using monocular vision. However, as also remarked in [START_REF] Azizian | [END_REF]], a good control or estimation require also a good choice and high precision in visual feature extraction. Therefore,b o t ht h e s eo b j e c t i v e s , feature extraction and pose estimation, are equally important and constitute the main contributions of this work. In this part, the focus is seto nf e a t u r es e l e c t i o n and extraction, whilst the second part will deal with the proper pose estimation problem.

In the previous chapter, several systems adopted in flexible endoscopic surgical procedures have been described and their controlling issuesh a v eb e e np r e s e n t e d . Although a common thread can be individuated in the control and pose measurement strategies, the peculiarities that such systems present lead to the necessity of state the problem in details. Stereo vision instead of monocular, different points of view, absence of endoscopic camera (for catheter) are only a few of the reasons that make the problem to slightly change for each situation.

In this work, it has been decided to focus on devices for endoluminal or abdominal single port surgery or NOTES where their particular configuration of camera, lights and instruments are considered as general constraints worth to be studied and taken into consideration to validate possible solutions.

Thus, to comprehend the considered framework and the main contributions, it is fundamental to start with the description of the device taken as reference and present the main issues.

System Description

The device taken as reference is STRAS, a robotic system basedonashortversionof the manual Anubis platform developed by Karl Storz (Tüttlingen, Germany). The Anubis platform is a totally flexible system (in the sense thatt h e r ea r en od i s c r e t e joints), initially developed for NOTES surgery. The short version consists of a main endoscope and two instruments, which can be used simultaneously. Instruments and main endoscope consist of a flexible passive shaft and are equipped with a bendable distal part, which is controlled from the proximal side usingt e n d o n s . E a c ht e n d o n runs inside a sheath inside the shaft of the endoscope (and instruments). The sheath is free to move inside the shaft so as to allow a good flexibilityo ft h es h a f ti t s e l f and it is attached at the beginning of the bendable section of the endoscope (and instrument). At the base of this distal section the cables exit the shaft and are made to pass through dedicated supports attached to the support structure (backbone) before being attached to the tip of the bendable part. The main endoscope has a diameter of 16 mm and it is equipped with a camera at the distal tip and a lighting system composed by two spots one on each side of the camera. The distal part of the endoscope can be deflected along two orthogonal directions and is actuated by two antagonistic pairs of tendons made of braided steel arranged in quadrature.

Three channels are available in the endoscope, which acts as aguide,forpassing surgical instruments. One channel is located at the core of the shaft of the scope, while two channels (called lateral channels) are at the sideso ft h es h a f t . T h ei nstruments can be inserted in the lateral channels of the endoscope and thanks to the mechanical play between them and the channel, they can smoothly translate and rotate. The instruments have long flexible shafts (length9 0 0m m )a n dabe n dable distal part (length 18.35 mm, diameter 3.5 mm). This partc o n s i s t so fh o l l o w cylindrical-shaped vertebrae linked one another in two diametrically opposed points forming a rotational joint (see Fig. 2.20). A pair of antagonist tendons made of braided steel allow bending the distal part in a plane.

As the instruments are hollow, they can receive inserts equipped with distal tools. Tools can be mechanical (grasper, scissors) or electrical (knife, ball, hook).

STRAS Concept

The conceptual idea for STRAS was to design a teleoperated modular platform, which could be easily setup at the operating table side. It wasa l s od e c i d e dt ok e e p as much of the original design of the Anubis platform in order to keep the sealing of the endoscope and the fluid management (water for irrigation and camera washing, air for insufflation and aspiration).

The main scope and the two instruments are held by a cart (see Fig. platform supporting the instruments and the endoscope has two motorized DoFs (up/down and inclination), which allows to position the whole system in a large variety of configurations depending on the access port to the operating area. The entire slave system (main scope + instruments) can be translated and rotated, as one entity, around the entrance direction at the proximal side. Thanks to this combined translation of the endoscope and the instruments, the friction efforts of the instruments at the entrance of the channels of thee n d o s c o p ed e c r e a s e considerably.

All manual wheels and knobs on the instruments and scope have been replaced by small electrical motors (see scheme on Fig. 2.22)a n dt h ew h o l es l a v es y s t e mc a n be controlled from a master console. The head of the endoscopeh a st w oD o F sa n d can be deflected along two orthogonal directions. Both instruments have 3 DOFs: they can be translated and rotated inside the channels of the main scope and their extremity can be bent in one direction. Moreover, mechanicali n s t r u m e n t s( s u c ha s graspers) can be opened and closed conferring an additional DOF.

The overall system has 12 motorised DoFs of whom 10 (those regarding translation, rotation and bending of endoscope and 2 instruments)a r ea l s ot e l e o pe r a b l e .

Modularity is another important concept beneath the design of STRAS slave system (see Fig. 2.21). The endoscope holds the motorization for its deflection and it can easily be attached to / detached from the cart. Two fixed modules contain the motorization for the translation and the rotation of the instruments. Instruments handles have been replaced by cylindrical bull-nose shaped casings containing the motorization for bending and grasper opening / closing. These modules can be easily plugged onto the translation / rotation modules. Thism o d u l a rs t r u c t u r e allows changing instruments during operations, while limiting the weight of the instrument modules to be manipulated and the required numbero fm o t o r sf o ra given set of surgical effectors.

Using visual perception in this context is challenging and the need of new approaches arises to extract the required visual information for pose estimation. In the next section, the main issues and the derived hypotheses are described and compared with state-of-the-art solutions.

Problem Statement

As the instruments bodies are uniformly black, the only element that presents visual features that can be related to 3D physical points is the attached surgical tool. Unfortunately, during the operation, the tool is often hidden since it continuously interacts with tissue for grasping or cutting. The focus, then, must be displaced to the body of the instrument bendable section which is textureless, though. It is not uncommon, then, to employ colored markers to overcome such problems and, in addition, to strengthen the features mainly for in-vivo environments where smoke or other fluids often corrupt the quality of the image making the features to appear less clear or fuzzy.

Additionally, the strong frontal lighting creates three zones in the image: a central zone where the light is very intense, an intermediate zone with soft lighting and ab o r d e rz o n ew h e r et h ei m a g ei sa l m o s td a r k . A so n em a ye x p e c t ,i ft h ei n s t r ument is located in front of the camera, the intense light and its reflective material cause strong specularity effects that modify the color appearance of the instrument, causing a wide white region in the middle of its bendable section (Fig. 2.23). This effect is not very attenuated even using a matte painting for the markers. Only when the instrument is no more exposed to this strong direct light, the specularity disappears.

Finally, the small elevation angle of the camera wrt to the instruments does not allow a clear vision of the surgical plane and increases the difficulty at perceiving the depth of the instruments. This particular point of view, indeed, impedes to clearly capture the shape of the instrument whose deflection is often perceivable only for the sheath (and consequently the markers') surface deformation while the outline stays almost unvaried for certain configurations.

In in-vivo environments further problems arise: the strong reflectanceo ft h e organ surfaces produces mirroring effects of the instrument,increasingtheam biguit y of visual information (Fig. 2.23).

In the solution proposed here, all these remarks are taken into account, the only constraining hypotheses made are to consider that the instruments move in a free space and that no occlusions of the bendable section occur (neither occlusions due to external objects nor self-occlusions).

As shown in the precedent chapter, solutions regarding the pose estimation in such configuration are rare in the literature. However, this problem can be contextualised in a wider framework letting aside the in-vivo or surgical constraints. This framework is therefore the pose estimation of deformable objects using 2D images.

Depth Information from 2D images

The main issue in monocular vision is related to the retrievalo fd e p t hi n f o r m ation which must be inferred from 2D visual measurements. The question is then which visual features contain shape information or how to combine features to recreate the 3D scene. Gibson, in his studies on animal/human visual perception, argued that humans obtain a lot of information on depth and environment structure by moving inside it. The structure from motion framework recalls this concept [Longuet-Higgins 1986]a n da i m st or e t r i e v e3 Di n f o r m a t i o nf r o mas e q u e n c eo fi mages. Usually, some features are tracked through the sequence and, based on them, the motion and the structure can be obtained by a projection model [START_REF] Tomasi | [END_REF] ] or using 2 or 3 views at a time [START_REF] Hartley | [END_REF]]. A final bundle adjustment is made to find the best structure and movement explaining the scene observed in the sequence.

The outline of an object can be also used for shape retrieval even using uncalibrated camera. Different approaches have been developed [START_REF] Cipolla | [END_REF], Wong 2004[START_REF] Utcke | Projective reconstruction of surfaces of revolution[END_REF]]f o ro b j e c tt h a tc a nb ed e s c r i b e db yc i r c u l a rc r o s s -s e c t i o n(of different radius) where the axis goes through the center of the cross-section, and the cross-section is orthogonal to the axis which is usually straight and constant. They exploit revolution surface invariant to calibrate the camera and specific geometrical/projective properties to retrieve the shape from the surface outline [START_REF] Cipolla | [END_REF], Wong 2004]ortheoutlineandt w ocross-section [START_REF] Utcke | Projective reconstruction of surfaces of revolution[END_REF]]. For depth determination, the dimension of at least one section [Wong 2004]o rt h eo u t l i n ed e f o r m a t i o n from several known point of view [START_REF] Cipolla | [END_REF]]m u s tbeg i v e n .

Other authors proposed interactive methods for depth estimation with uncalibrated cameras. In [START_REF] Sturm | [END_REF]], the user is meant to provide 3D constraints in terms of coplanarity, perpendicularity and parallelism which are then used to calibrate the image and perform the 3D reconstruction. In [Criminisi 1999], instead, the proposed solution needs a reference plane and a reference direction not parallel to the plane to compute respectively the vanishing line and the vanishing point. With this information, 3D affine measurement can be computed in terms of distances between planes parallel to the reference plane, area and length ratio on any plane parallel to the reference plane. Given the value of a reference distance or height, the real 3D position of other points can be computed and a 3D model can be created from these points.

For automatic structure determination using only one image, Shape-from-Shading (SfS) is another interesting approach. The original idea by Horn [Horn 1970]w a s to obtain the shape of a smooth opaque object from one view, given the knowledge of the surface photometry (which must be uniform), the position of the light source and other additional information such as the position of the observer with respect to the object and the light source.

Alternatively to shading, if all the possible deformations of the object pertain to a limited set, another solution is to determine the shape asal i n e a rc o m b i n a t i o n of the elements of that set. In [START_REF] Blanz | [END_REF]]t h i sc o n c e p to fmorphable models is applied to faces, where the set of basic shapes is composed by 3D scanned head of 200 people including color information. Obviously, this process is applicable only to as pe c i fi cc a t e g o r yw h o s ed a t a b a s ei sa v a i l a b l e .

If no information on the illumination source or no prior information is available on the kind of deformation, none of the previous framework would work. An alternative can be Shape-from-Template provided that a template is known. For template is meant a prior knowledge over the appearance (texture of theo b j e c t ) ,t h es h a p e (the resting position) and the material of the object (implying the dynamic of the deformation). Feature such as SIFT are extracted both in the template and the actual image and a warping is done based on these features. After that, the possible deformation of the object that would generate the actual image is computed taking into account the deformation law of the template according toitsmaterial. Astrong hypothesis in this process is the isometry of the object even though more complex deformation law can be taken into consideration [START_REF] Bartoli | [END_REF]].

Taking into consideration the problem we want to address (see problem statement section), in structure from motion the observed objects in the scene are supposed to be rigid which is not the case here. Non-Rigid Shape-from-Motion [START_REF] Chhatkuli | [END_REF]] can be considered, but it is still a complicate and open problem. In the aim of performing automatic control, interactive methods are evidently unsuitable and shape from shading can lead to imprecise solutions due to the fact that material reflectance and illumination models are not easy to obtain fort h i sk i n do fs y s t e m s and, especially, in in-vivo environment. It is true, though, that do exist shape from shading methods that need less a-priori knowledge [START_REF] Barron | [END_REF]]. Shading is, then, parametrized as a function of shape and reflectance which are considered unknown. To comp ensate this information loss, plausible constraints( a b o u ts u r f a c es m o o t hness, uniformity of paint and illumination) are added to the original SfS problem to find the final solution. Unfortunately, in our problem, the strong saturation due to specular component of light make the most of instrument white, loosing, in this manner, important structure information.

On the other hand, the use of morphable models would mean to create a sufficiently dense database considering all the possible deformations of the instruments. Doing so, with the will of obtaining high precision, there canbetheriskofcreatinga self-exhaustive set that would convert the approach almost to a look-up table making morphable model to lose their "interpolating role". In addition, the instruments in question are textureless and some new features must be found for the correspondence. Furthermore, the authors need a rough interactive alignment of the model with the actual photo and suppose some strong a-priori: camera distance, surface shininess and light direction remain fixed to the value estimated by the user.

Finally, neither an easy adaptation of shape from template isp o s s i b l ei nt h e studied case due to its strong dependency on clear texture forinitialw arping,whic h the considered instrument does not present.

However, assuming that the intrinsic parameters of the camera are computed and some ap r i o r iinformation about the environment is known, neither the user interaction nor texture or lighting knowledge are needed forp o s ee s t i m a t i o n . I ti s quite easy for a rigid object (with easy recognizable physical features) [START_REF] Lepetit | [END_REF]], but further considerations must be added for solids of revolution (such as cylinders or sphere) [Doignon 2007]o rf r e e l yd e f o r m a b l eo b j e c t ss u c ha st h r e a d so rfl e x i b l e tubes (as described in Sec. 2.3). In [START_REF] Caglioti | Reconstruction of canal surfaces from single images under exact perspective[END_REF]]ac o m p l e t e l yg e o m e t r i c a lb a s e d method is used to estimate the position of a flexible tube in thes p a c e . A f t e rbo r d e r extraction, upper and lower correspondence is settled basedo nb i t a n g e n ta n dfl e x points. Each couple of points (upper and lower border points)isconsideredtobetwo points on the tube section, knowing, then, the 2D tangent to these points (computed numerically) gives information on the vanishing point whichi su s e dt oc o m p u t et h e position and orientation of each section and, finally, composing all these sections, the entire tube. This approach, though, shows its weakness inthosecaseswherethe borders appear as almost parallel lines which is often the case with the ending part of the bending section of the considered surgical instruments. In this case, in fact, strong noise on the detected points could bring to a wrong estimation of tangents and, in turn, to the corresponding section orientation.

In the light of what discussed so far, a dual necessity arises:

1. Find which are the most suitable visual features for pose estimation.

2. Find a manner to obtain such features in the enunciated conditions while, as afi r s ta p p r o x i m a t i o n ,c o n s i d e r i n gt h ei n s t r u m e n tm o v i n gi nfree space.

These two problems are addressed in the next chapters: in section 3.5 the choice of a suitable image features for pose estimation is argued on simulation data and a novel method to obtain the selected feature is presented in chapter 4.

The suitable feature will be defined as that visual feature which, considering a given 2D error in its extraction, allows having a smaller estimation error on the 3D position of the surgical Tool Center Point (TCP). To carry out such study, the relationship between the robot configuration and image features and between the image feature and 3D tip must be known. This means knowing:

• the kinematic model of the instrument wrt the camera, that relates the values of its DOFs to its 3D shape.

• the camera model for being able to create the image relative to a specific 3D configuration of the instrument.

• an extended Image Jacobian that describes the relationship between the instrument joint velocities and feature movement in the image. The inverse of this Jacobian provides the relationship between the feature movements (or uncertainty) in the image and the instrument DOFs variationsa n d ,i n d i r e c t l y , on the 3D tip position variations.

Geometric and Kinematic Model

For computing the mo del of the instrument, the attention wil lb ef o c u s e do nt h e tip of the endoscope and the camera-to-end-effector kinematic chain. This can be divided into two parts: one consists of the properly-said kinematic model defined by the controllable DoFs (marked in blue in Fig. 3.1(b))o ft h ei n s t r u m e n t sa n d the other represents the geometrical relationship between the channel exit and the endoscopic camera, i.e. the position and orientation of F ch wrt F c in Fig. 3.1(a).

For the former part, the 3 DOFs of the instrument are considered (translation λ and rotation φ with respect to their axis and deflection θ)andforthelattertheposition of the exit of the channel (x ch ,y ch ) housing the instrument and its orientation (angles (ψ, µ) marked in green in Fig. 3.1(a))withrespecttotheendoscopiccameraoptical axis are taken into account. The instrument can slightly moveinsidethechanneland to entirely describe the instrument position wrt to the camera these parameters are needed. The only two restrictive yet reasonable hypotheses are that the instrument bends in a plane and that it conserves a constant curvature alla l o n gt h eb e n d a b l e section. To formalize what discussed so far, the reference co ordinatef r a m e ss h o w e di n Fig. 3.1 are defined. F c indicates the camera frame, the frame associated to the end of the instrument channel is called F ch ,whereastheoneassociatedtothebeginning of the bendable part is F b .T h e t o r u s -l i k e b e n d a b l e p a r t c a n b e d i s c r e t i z e d i n sections whose reference frame are F sec i .F o rt h el a s ts e c t i o n ,t h eo n ec o r r e s p o n d i n g to the tip of the instrument, F tip is used. Depending on the attached surgical tool, the distance from the tip to the surgical Tool Center Point (TCP) can vary and the frame associated to it (F TCP )w o u l dr e s u l ti nar i g i dt r a n s l a t i o no fF tip along its z-axis.

Without losing sight of the main objective of using the cameraf o rp o s ee s t i m ation, the camera frame will be considered as the world reference frame and all the other frames will be expressed with respect to it. Thus, F ch can be obtained from F c by a 3D translation and two subsequent elementary rotations:o n eo fa n g l eψ around the camera y-axis and one of angle µ around the resulting x-axis.

The vector defining the mentioned translation is known from the CAD model of the instrument provided by the manufacturer and it will be denoted as t c c,ch meaning the translation from the origin of F c to the origin of F ch (the two ordered subscript elements) expressed in the camera frame F c (the superscript element). A similar notation will be used for the rotation matrix: R c ch express the orientation of F ch with respect to F c coordinate system. The frame of the base of the bendable section (F b ), in turn, can be defined from F ch by a translation along F ch z-axis with λ (first instrument DOF) and a rotation around the same axis of ana n g l eφ (second instrument DOF).

The affine map components between the camera and the base of the bendable part can, then, be expressed as follows:

t c c,b = t c c,ch + λ z c b (3.1) = ⎡ ⎢ ⎣ x ch y ch 0 ⎤ ⎥ ⎦ + λ ⎡ ⎢ ⎣ sin ψ cos µ sin µ cos ψ cos µ ⎤ ⎥ ⎦ (3.2) R c b = R y (ψ) R x (µ) R z (φ) (3.3)
where z c b is the third column of R c b ,i.e. theunitlengthv ector(alsocalledv ersor) of the z-axis of the channel frame (whose direction coincides with theo n eo ft h e base frame), whereas R x (.), R y (.) and R z (.) are the matrix form of rotations around x, y and z axis with angles µ, ψ and φ,r e s p e c t i v e l y . F i n a l l y ,i th a sb e e nc h o s e nt o consider the end of the instrument duct fixed and on the same plane (x, y) of the camera, so as to eliminate a sort of redundancy between λ and z-coordinate of the channel. Consequently, the third component of t c c,ch is equal to zero. Assuming a constant curvature along the bending section and the fact that the instrument axis pertains to the (z, y) plane of F b ,t h er e l a t i o nb e t w e e nt h eb a s e frame and F tip (Fig. 3.1(b))c a nb ee a s i l yd e c o m p o s e da sar o t a t i o na r o u n dF b y-axis with angle θ and a translation defined by the vector:

t b b,tip = ⎡ ⎢ ⎣ R curv (1 -cosθ) 0 R curv sinθ ⎤ ⎥ ⎦ (3.4)
The curvature radius R curv is gathered from the definition of radians: R curv = L/θ where L is the length of the arc corresponding to θ.I nt h i sc a s eL is the length of the whole bending section of the instrument (arc connecting F b to F tip in Fig. 3.1(b))a n dθ the bending angle.

Finally, the orientation and translation of the tip with respect to F c results to be:

R c tip = R c b R y (θ) (3.5)
and

t c c,tip = t c c,b + R c b t b b,tip (3.6)
where

R tip b =[R y (θ)] T .

Camera to i-th section

The geometrical transformation between the camera coordinate frame and the frame F sec i associated to a possible section of the bendable part of the instrument is obtained from F b as

R c sec i = R c b R y (θ i ) (3.7) t c c,sec i = t c c,b + R c b (t b b,sec i ) (3.8)
where

t b b,sec i = ⎡ ⎢ ⎣ L i /θ i (1 -cosθ i ) 0 L i /θ i sinθ i ⎤ ⎥ ⎦ , (3.9)
and L i are the arc length defined by the angle θ i associated to the section in question. Thus, L i /θ i is the curvature radius R curv .

Every point P i of the surface of the torus can be expressed as a point pertaining on the circumference or radius r (radius of the instrument) individuated by a specific section (F sec i )p e r p e n d i c u l a rt ot h ei n s t r u m e n ta x i s . K n o w i n gt h ec o o r d i n ate of P sec i i with respect to F sec i ,t h ec oo r d i n a t e so nt h es u r f a c ew r tt oc a m e r af r a m ec a n be computed straightforward using (3.8): Referring to Fig. 3.2,ar e l a t i o nbe t w e e nt h eb a s ea n dt h epo i n t so ft h es e c t i o n s can be defined. In fact

P c i = R c sec i P sec i i + t c c,sec i θ i z x θ i P i F b O sec i L i
t b Osec i ,P i = ⎡ ⎢ ⎣ P sec ix cosθ i P iy -P sec ix sinθ i ⎤ ⎥ ⎦ (3.10)
and, joining the two results (3.8)a n d( 3.9):

t b b,P i = t b b,sec i + t b Osec i ,P i t b b,P i = ⎡ ⎢ ⎣ R curv (1 -cosθ i ) 0 R curv sinθ i ⎤ ⎥ ⎦ + ⎡ ⎢ ⎣ P sec ix cosθ i P iy -P sec ix sinθ i ⎤ ⎥ ⎦ . (3.11)
Once obtained the expression in the base frame it is easy to findt h eo n ew i t h respect to the camera frame, which is, for the translation component:

t c c,P i = t c c,b + R c b t b b,P i (3.12)
and for the rotation component:

R c sec i = R c b R b sec i . ( 3

Camera to TCP

The extension for the TCP is straightforward. In fact, its reference coordinate frame shares the same orientation as the frame associated to the tip (expressed by the rotation matrix in 3.13)a n dt h ep o s i t i o ni so b t a i n e da sat r a n s l a t i o na l o n gt h e tip z-axis. The resulting orientation and translation wrt the camera are:

R c TCP = R c tip t c c,T CP = t c c,tip + R c b R b tip ⎡ ⎢ ⎣ 0 0 l tool ⎤ ⎥ ⎦ (3.14)
where l tool is the distance between the end of the bendable part (i.e. the instrument tip) and the surgical tool center point (TCP).

Inverse Position Kinematic Model

No closed form solution is available for this problem and numerical optimization approach such as Gauss Newton algorithm using the inverse of the Jacobian would return only one solution. However, for obtaining all possible solutions the following procedure can be adopted.

First, the desired Cartesian position of the TCP is expressedwithrespecttoF ch with coordinates P * =(X * ,Y * ,Z * ):

t ch ch,T CP = ⎡ ⎢ ⎣ 0 0 λ ⎤ ⎥ ⎦ + R ch b t b b,tip where R ch b = R z (φ).
T h e r e f o r e , t h e d i s t a n c e o f t h e p o i n t f r o m t h e i n s t r u m e n t rotation axis can be expressed as:

f d (θ)= X * 2 + Y * 2 = L θ (1 -cosθ)l tool sin(θ) (3.15)
Fig. 3.3 shows the relationship f d (θ) between angle θ and the distance of the TCP from the instrument rotation axis, the maximum is reached for θ sing which is approximatively 101.77 • .L o o k i n ga tt h i sfi g u r e( o b t a i n e dw i t hL =18.35 and l tool =15.8), it can be seen that the previous equation generally has two solutions (θ 1 and θ 2 ), one on each side of θ sing plus their opposite negative solutions. In fact, if a point in the workspace (without taking into account orientation) is attained by a specific deflection θ 1 and a specific rotation angle φ 1 ,i tc a nb ea t t a i n e da l s ob yd e fl e c t i o n equal to -θ 1 and rotation φ 1 + π.F i n d i n gt h ed e fl e c t i o na n g l e si s e q u i v a l e n tt o solve:

θ 1,2 =argmin θ X * 2 + Y * 2 -f d (θ).
This problem can be solved numerically. One solution is obtained initialising the optimization next to θ =0and, for the second, near to θ = θ max where θ max is the maximum deflection that the instrument can achieve (equal to 120 • in this case).

The corresponding rotation angles φ is computed directly from P * 1 coordinates as

φ 1 =atan2(Y * ,X * );
Finally, the translation parameters λ 1 (λ 2 )arecomputeddirectlyfromthedirect kinematic model relations:

λ 1,2 = Z * - L θ 1,2 sin θ 1,2 -l tool cos θ 1,2 .
For the scop e of the preliminary study, only one solution wil lbeconsideredsuc h that θ is positive and smaller than θ sing .

Camera Projection Model

To relate the 3D shap e of the instrument with visual features ,amodelofthecamera is needed so as to know how the 3D points are projected onto the image plane. The pinhole model is a simple way to describe this mathematical relationship. In this model the camera aperture is described as an infinitesimal point where no lenses are used to collect the light and no distortion or blurring are taken into account. The rays of light coming from the environment travel along a single path through the pinhole and intersect the image plane where its upside-down image is formed. The reference frame associated to the camera is usually a right-handed coordinate system where the x-axis is taken as the horizontal direction and the y-axis as the vertical (downward) direction. This means that the optical axis (gaze direction) is the positive z-axis. The image plane is parallel to the camera sensor plane (x, y) and positioned at -f along the camera z-axis where f is the apparent focal distance of the camera. A totally equivalent representation is choseni nF i g . 3.4 which does not pretend to be a physical representation of a camera. The fact of positioning the image plane at +f is just for easing the representation since it avoids the formation of an upside down image. The intersection of the optical axis with this plane individuates the principal point of the image plane marked as (u 0 ,v 0 ).

The camera realizes the projection of a point P i ∈ R 3 of the Euclidean space into a point p i ∈ I 2 of the image plane whose homogeneous coordinates (u i ,v i , 1) are expressed in pixels. Assuming the point is expressed withr e s pe c tt ot h ec a m e r a frame, the projection in metric units of such point (X i ,Y i ,Z i ) onto the image plane can be written as:

ũi ṽi = f Z i X i Y i . (3.16)
The corresponding image coordinates (u i ,v i ) in pixel units are obtained from (ũ i , ṽi ) with the following relation:

u i v i = D u s u ũi -D u cot(ϕ)ṽ i D v ṽi / sin(ϕ) + u 0 v 0 (3.17)
where D u and D v are needed to change from metric units to pixels, s u is the possible scale factor between the vertical and horizontal sizes of thesensorandϕ is the angle between the image reference frame axis (usually ϕ = π/2). The addition of [u 0 ,v 0 ] T moves the principal point to the top left corner. This principal point can be taken as origin of the image coordinate system. These expressions can be summarized in matricial form as:

p i = 1 Z i K [I 3x3 O 3x1 ] P i = K [I 3x3 O 3x1 ] ˜m i (3.18)
where

˜m i = 1 Z i P i = ⎡ ⎢ ⎣ xi ỹi 1 ⎤ ⎥ ⎦ K = ⎡ ⎢ ⎣ α x -α x cot(ϕ) u 0 0 α y / sin(ϕ) v 0 001 ⎤ ⎥ ⎦ , (3.19)
α x = D u s u f and α y = D v f are the apparent focal distances expressed in horizontal and vertical pixels respectively.

To increase the field of view of the surgeon, flexible endoscopies usually mount a wide angle lens which return a highly distorted image both tangentially and, above all, radially. It is denominated radial distortion when the projected point suffers a deviation along the line connecting it to the principal point, whose value depends on its distance with the principal point and it arises because the "thin lens" hypothesis considered in the pin-hole model is not valid anymore.

Another source of image distortion is the fact that centers ofc u r v a t u r eo fl e n s surfaces are not always strictly collinear. This distortiont y p e ,c a l l e dd e c e n t e r i n g distortion, has both a radial and tangential component [START_REF] Slama | [END_REF][START_REF] Heikkila | [END_REF]].

The model just presented then becomes insufficient to describet h ew h o l ep h enomena of image formation, therefore a more accurate model must be developed which derives from the combination of the pinhole model with radial and tangential distortions, obtaining:

m i = ⎡ ⎢ ⎣ x i y i 1 ⎤ ⎥ ⎦ = ⎡ ⎢ ⎣ xi + δx (r) i + δx (t) i ỹi + δy (r) i + δy (t) i 1 ⎤ ⎥ ⎦ (3.20)
The radial distortion contribution is [START_REF] Heikkila | [END_REF]]:

δx (r) i δy (r) i = xi (k 1 r 2 i + k 2 r 4 i + k 5 r 6 i + •••) ỹi (k 1 r 2 i + k 2 r 4 i + k 5 r 6 i + •••) (3.21)
and the tangential contribution is:

δx (t) i δy (t) i = 2k 3 xi ỹi + k 4 (r 2 i +2x 2 i ) k 3 (r 2 i +2ỹ 2 i )+2k 4 xi ỹi (3.22)
where r i = x2 i +ỹ 2 i is the distance of the normalized points from the principal point.

The parameters k 1 , k 2 , k 5 are the coefficients for radial distortion (sixth order is usually sufficient) and k 3 , k 4 are the ones for tangential distortion. To completely define the projection transformation, these parameters mustb ek n o w nt o g e t h e r with the so called intrinsic parameter α x , α y and (u 0 ,v 0 ).I ti sf u n d a m e n t a l ,t h e n , ap r e l i m i n a r yc a l i b r a t i o ns t e po ft h ec a m e r at oe s t i m a t et h a tp a r a m e t e r s . F o rt h i s purpose, the methods of Doignon [START_REF] Doignon | n e method for a low-cost calibration technique[END_REF]], Zhang [Zhang 1999]andHiekkilä [START_REF] Heikkila | [END_REF]] can be used. The last two have been implemented by Yves Bouguet and are available as a Matlab Toolbox [Bouguet 2013]w h i c hh a sb e e ne m p l o y e d here.

Several views of the same known calibration grid are grabbed and camera-grid relative pose (so called extrinsic parameters), distortiona n di n t r i n s i cp a r a m e t e r s are optimized to obtain the least reprojection error defined as the 2D error between the detected grid corners and the reprojected corners using the estimated model parameters.

Knowing the distortion parameters, the inverse of the distortion transformation can be computed and applied to the image obtaining its rectified version. Once rectified, the pinhole model is a good approximation of the camera behavior and the perspective projection relation are valid and can be possibly utilized to infer 3D information from image measurements.

Extended Image Jacobian

The last step is to relate the 3D velocity of given 3D points with the image motion of their projections. The transformation matrix describing such relationship is called Image Jacobian:

˙p i = J I i Ṗi (3.23)
where P i is a point defined in the task space (camera frame: P i = t c c,P i )a n d ˙p i is the feature parameter rate of change in the feature (image) space.

Since camera and endoscope guides are linked together, the relative movement of a point P i on the instrument surface (wrt the camera) depends solely on the movements of the flexible instrument itself:

Ṗi = J g i ṙ .
(3.24)

J g i is the geometrical Jacobian relating the joint velocities (ṙ )t ot h ev e l oc i t yo ft h e chosen point on the instrument. Thus, defining the 3D point

P i =[X i ,Y i ,Z i ]
T and its relative 2D visual feature point p i =[x i ,y i , 1] T and considering equation (3.24), the relationship described in (3.23)c a nb ew r i t t e na s :

˙p i = J I i J g i ṙ (3.25)
where r =[λφθ] T is a specific instrument configuration. The product between J I i and J g i can be considered an extended image Jacobian L i :

L i = J I i J g i (3.26)
which relates the joint velocity of the instrument with the image motion of a point of the instrument itself.

Image Jacobian

Considering only the 3D translational effects on the 2D features, the computation of the image Jacobian is equivalent to find the gradient:

J I i = ∂ ˜m i ∂X i , ∂ ˜m i ∂Y i , ∂ ˜m i ∂Z i .
The final expression, then, can be derived from 3.18 and 3.19 resulting in: To take into account the distortion effect an equivalent gain is computed which expresses a magnification factor between the displacement ontheimageplanewithout distortion with respect to the same displacements on the image plane with distortion. In fig 3.5,t h ep r i n c i p l ei ss c h e m a t i z e df o rt h ex coordinate on the plane (x, z).T h er a yo fl i g h tp a s s i n gt h r o u g ht h eo p t i c a lc e n t e ri n t e r s e cts the ideal (= no distortion effects) image plane at a point p i which can be computed with the projective relationships described in the precedent section with equations (3.16)a n d (3.17)( t oo b t a i nt h er e s u l ti np i x e lu n i t s ) .

J I i = K ⎡ ⎢ ⎣ 1 Z i 0 -X i Z 2 i 0 1 Z i -Y i Z 2 i 00 0 ⎤ ⎥ ⎦ = K ⎡ ⎢ ⎣ 1 Z i 0 -x i Z i 0 1 Z i -ỹ i Z i 00 0 ⎤ ⎥ ⎦ (3.27)

Distortion Effect

P i P ′ i C x z Π I Π I d p i p ′ i p d p ′ d ∆x ∆x d
For the effect of wide angle lenses, though, the ray of light is "bent" before intersecting the real image plane (Π I d ). As showed in precedent section, the projection of P i onto this plane can be computed with (3.18)c o n s i d e r i n gm (from eq. (3.20)) instead of ˜m :

p d = K [I 3x3 O 3x1 ] m i . (3.28)
The distorted projection in metric units m can be computed through (3.21)

G d = ⎡ ⎢ ⎣ ∆x d ∆x ∆y d ∆y 1 ⎤ ⎥ ⎦ (3.29)
and, post-multiplying it by J I ,t h en e wI m a g eJ a c o b i a nJ I d taking into account distortion effects is obtained as:

J I d = G T d J I .
(3.30)

Geometric Jacobian

As explained before, the geometric Jacobian is the matrix transformation which describes the relationship between the joint velocity and the end-effector velocity.

Since the considered robot has 3 DOFs the Jacobian can be thought as a block matrix where each block is the contribution of each joint action ontot h e3 Dpo s i t i o no ft h e chosen instrument point.

Ṗi = A λ A φ A θ i λ φ θ T (3.31)
where A r k = ∂P i /∂r k ,w i t hr k being alternatively λ, φ or θ.T h efi n a le x p r e s s i o n , then, would result by the computation of these three blocks separately. Av a r i a t i o no fλ affects the position of P i according to the channel direction. Indeed, when computing ∂t c c,P i /∂λ the only element depending on λ is t c b i.e. the position of the base with respect to the camera finally giving

A λ = ∂t c c,P i ∂λ = ∂t c c,b ∂λ = ⎡ ⎢ ⎣ sin ψ cos µ -sin µ cos ψ cos µ ⎤ ⎥ ⎦ (3.32)
which is the channel z-axis direction which correspond to z b c (the 3rd column of R c b ). The rotation around the channel axis can be considered as a revolute joint and, as for any revolute joint, the effect of this joint on another point of the kinematic chain is equal to the cross product between the angular velocity screw and the vector joining the center of rotation and the point of interest. Since the rotation is around the F b z-axis the angular velocity screw will correspond to the third column of R c b and the radius is P c i -t c c,b always with respect to the camera frame, obtaining:

A φ = -[(P c i -t c c,b ) × z c b )]. (3.33)
The last block concerns the variation of θ.S i n c e

t c c,P i = t c b,c + R c b P i b = t c b,c + R c b ⎛ ⎜ ⎝ ⎡ ⎢ ⎣ L i /θ i (1 -cos θ i ) 0 L i /θ i sin θ i ⎤ ⎥ ⎦ + ⎡ ⎢ ⎣ P sec ix cos θ i P iy -P sec ix sin θ i ⎤ ⎥ ⎦ ⎞ ⎟ ⎠ (3.34)
the only non constant element with respect to θ is P i b .T h ev a r i a t i o no fP i coordinates with respect to the camera can be computed pre-multiplying the same variation expressed with respect to the base of the bendable part by R c b :

A θ = ∂t c c,P i ∂θ = R c b ∂P b i ∂θ with ∂P i b ∂θ = ⎡ ⎢ ⎣ k θ L/θ sin θ i -P sec ix k θ sin θ i -L/θ 2 (1 -cos θ i ) 0 k θ L/θ cos θ i -P sec ix k θ cos θ i -L/θ 2 sin θ i ⎤ ⎥ ⎦
where

θ i = θk θ being k θ =( i -1)/(n sec -1)
where n sec is the number of section and i =1, ••• ,n sec is the considered section.

Point on the axis and TCP

The computation of the different blocks for a point on the axis of the instrument bendable segment or for the TCP is totally similar to the one described previously. Actually, the block A λ and A φ are equal to the previous ones (substituting P i with a point in the axis), whereas A θ needs to be recomputed or derived from the precedent without considering instrument thickness (i.e. P sec i =[ 000 ] T )a n da further transformation in the case of the TCP. Finally, for a general point on the axis

A θ = R c b ⎡ ⎢ ⎣ k θ L/θ sin θ i -L/θ 2 (1 -cos θ i ) 0 k θ L/θ cos θ i -L/θ 2 sin θ i ⎤ ⎥ ⎦
and for the TCP

A θ = R c b ⎡ ⎢ ⎣ L/θ sin θ -L/θ 2 (1 -cos θ)+l tool cos θ 0 k θ L/θ cos θ -L/θ 2 sin θ -l tool sin θ ⎤ ⎥ ⎦
Both 3.27 and 3.31 still require the knowledge of the 3-D point depth. The only issue is that the considered points do not have a direct physical sense in 3D but are only apparent features depending on the pose of the instrument with respect to the camera. Their 3D position can be computed for a specified robotc o n fi g u r a t i o nr (see Appendix A for details) giving to the extended Jacobian only a local meaning. The dependency of the apparent points on the pose of the instrument with respect to the camera affects also their velocity. This effect has to be taken into account in the geometrical 3D Jacobian and further details are given in Appendix A.

All these relationships between robot (i. e. the robotized instrument) configuration and the image are of fundamental importance for any control or pose estimation approach based on image measurements. This result will have a duple utility: studying the pose estimation problem in simulated data for choosing a proper visual feature to use for such purpose and also providing a modelofinstrumen tvisual perception that will be exploited in the first proposed solution (Sec. 5).

Theoretical Study for Features Selection

The precision of vision based methods is inherently linked totheintrinsicparameters of the camera and with the accuracy of the image features location. To know which precision can be achieved with such methods and, based on that, which visual features to adopt, a preliminary study has been carried out. It consists in quantifying the error on the pose estimation when a misplacement of one pixel is done on the image.

For sampled test p ositions, the Inverse Kinematic Mo del (sees e c t i o n3.2)o ft h e instrument is used to compute the corresponding Degrees of Freedom (DoFs) of the instrument. Then J g tip the Jacobian from the DoFs variations to the 3D position changes of the TCP of the instrument and J I the Jacobian from the DoFs to the image features are computed.

The local variance of the estimated TCP position is computed as

Σ TCP =(J g tip J T I d Σ 2D J I d ) -1 J T g tip with Σ 2D = I 2n feat
assuming a uniform independent one pixel covariance error one a c hc o o r d i n a t eo f the feature location.

Three cases are now studied: (1) Considering markers centroids and 3DOFs (the instrument is totally constrained in the channel, case reported in [START_REF] Reilink | [END_REF]), [START_REF] Gheorghe | Introduction of the radial basis function (rbf) networks[END_REF] Considering instrument borders, (3) Considering the apparent corners of some visual markers which divide the body of the instrument in equally spaced sections. For sake of result visualization, the fo cus has b een centeredo nt h ee s t i m a t i o no ft h e TCP coordinates in the central (x, z) plane of the instrument work-space, which is defined by the origin of F ch and two vectors parallel to the x and z coordinate of F C . Afirstanalysiscanbedoneontheoutlineofthevarianceonthedefinedprincipal (x, z) instrument plane (Fig. 3.6). A sort of symmetry can be observed with respect to the instrument channel axis and, focusing on those configurations where φ ∈ [-π/2,π/2] and θ>0 (in the work-space on the right of the blue line), its tendency shows a loss of accuracy on the tip position estimation for instrument positions farther from the endoscopic camera. This is quite an expectedb e h a v i o rd u et ot h e perspective projection which makes the size of the object in the image to decrease as the distance from the camera increases. In fact, when the object is far away, the resolution on the object given by the image is lower, since onep i x e lr e p r e s e n t sa wider region on the object itself. Thus, a larger movement or deformation would be necessary so that to be visible in the image, at a pixel scale.

As e c o n dl e v e la n a l y s i si st oc o m p a r et h ed i ff e r e n td e s c r i p t o rs in terms of mean and maximum errors on the TCP position estimation. For each robot configuration the direction with the largest variance in the TCP position estimation are taken and sorted (Fig. 3.7(a)). It can be straightforwardly seen that the use of centroids only does not permit a proper discrimination of different configurations, whereas the other considered descriptor seems to provide better results.

Features based on the b orders only do es not seem to b e a robust choice, either. This can be explained by the fact that more than one optimal solution can be found where the estimated borders are inside the actual apparent border of the instrument: in this case, in fact, the image residual is zero even if the estimated border does not really represent the real apparent border. Two points on eachapparen tbordercould be enough to know the scale to correctly identify the configuration. The use of the term scale here is not the most adequate since the instruments are deformable and the relative distances between points vary. Knowing some physical points on the border, actually, gives an information on the shape of the instrument: for example, if the beginning and the end of the instrument positions are known and visible in the image, this information can be used to improve the definition of image residuals, by constraining the estimated border to recover the totalityo ft h ea c t u a lb o r d e r .

To strengthen this shap e information, one can think to mark more than two points on the instruments that can be easily detected in the image and that carry shape information for example using the apparent corners of colored markers. Using markers painted on the whole surface of the instrument is, at the same time, a mean to ensure the visibility of the feature point for any rotation of the instrument (which would be not the case if dots are painted in specific positions). This seems to significantly help the pose estimation as can be seen when zooming on the worst cases of each approach (Fig. 3.7(b)).

Among the available corners, the ones associated to the end oft h eb e n d a b l e part seem to carry an important part of the information about the pose. In fact, observing the results obtained without the consideration ofthesecorners(greenline in Fig. 3.7(b))t h ei m p r e c i s i o ni sh i g h e rw i t hr e s p e c tt h eo t h e rc a s e s . O n ecan therefore think to give more confidence to these points to improve the robustness of the estimation. However, an exaggerate confidence on these points' locations may bring to higher errors (such that obtained with a weight of 10 on the tip, traced green line).

The discussion conducted so far is just a local analysis of sensitivity and it does not allow to discuss if the problem is a ill-posed problemi . e . i ft w oo rm o r e configuration have the same visual representation (whatevert h es e l e c t e df e a t u r e s are). Also the set of possible features is reduced and other features or composition of such features can be imagined; in the next sections, when talking about "best features" for pose estimation purpose is always referred to these 3 cases.

In light of this discussion, the best solution is taking into account reference points on the apparent borders which have some physical meaning and for whom a 3D relationship can be determined.

To increase the robustness, several p oints can b e chosen overt h eb o r d e ra n dt o relate the image feature points and the 3D object positioning, a ruler-like pattern is painted on the instrument alternating blue and yellow markers (Fig. 3.8). This does not only creates salient object related features, it also provides correspondences between upper and lower border points which can be used as trustful features for the pose estimation (cf chapter 5).

The choice of using markers is quite extended in surgical robotics, where robustness of feature extraction and tracking is really challenging especially in-vivo scenarios where the illumination is non-uniform and varying, and where smoke, blood or other fluids may degrade the quality of the image. The choice of the colors allows to augment the discriminance between the instrument and almost any internal tissues and also to get good inter-markers distinction astheyarecomplemen tary colors and, thus, their chromaticity values occupy oppositepo s i t i o n si nm o s to ft h e color representation spaces.

In the next chapter, the algorithm developed to extract the mentioned feature is presented with the studies and theoretical basis over which it is built. As evinced from the precedent chapter, adequate features forp o s ee s t i m a t i o n are the corners of colored markers attached to the bendable part of the instrument. As already pointed out, the choice of locating the markers on the bendable part is due to the fact that this is the most visible part during an operation. Indeed, the surgical tool is continuously interacting with the tissues and, therefore, hidden by them, whereas the instrument section located upstream wrt tothebendablesection is inside the channel most of the time.

The extraction of the apparent corners of the markers is quitec h a l l e n g i n g . T h e conventional approaches (e.g. Harris, maximum curvature oft h eb o r d e r s ,. . . ) a r e not efficient because the object has not sharp vertexes and the images are of low quality and highly distorted. The apparent corners can be defined as the points in correspondence with the color transitions on the upper (orl o w e r )i n s t r u m e n t apparent contours.

To exploit this prop erty, we prop ose to first roughly segment the instrument in the image so as to to define a region of interest where the contours are detected. In order to reject falsely segmented areas and retrieve the whole instrument structure, an interpretation step is performed (see section 4.2), which label the segmented regions according to the marker they pertain. Finally, a continuous definition of the upper and lower ordered border points must be obtained so as tob ea b l et od e t e c t the color transitions with a subpixel accuracy. This processa l s oa l l o w st ol a be lt h e extracted corners according to the markers they pertains, which makes the matching with their 3D counterparts possible.

This chapter describes in detail the method used for the extraction of the wanted features. After a discussion over color segmentation for thea d o p t e dm a r k e r s ,t h e two keyparts of the algorithm are detailed in sections 4.2 and 4.3.F i n a l l y , t h e algorithm implementation is described before showing the results in both controlled and in-vivo environment.

Color model and segmentation

Thanks to the adoption of colored markers, the color itself can be used for the first broad segmentation to retrieve the instrument ROI. In Fig. 4.1 the distribution of yellow, blue and background colors are represented in the RGB( 8 -b i tc o l o rr e p r esentation) space for two common scenarios within the abdominal cavity: intestine and liver. Analyzing these distributions, it can be seen thatthec hosencolorsforthe markers are distinguishable from both the backgrounds. Furthermore, blue and yellow distributions show a large inter-distance between them confirming also visually the complementarity of the two chosen colors.

However, the limits between the three categories (yellow, blue and backgrounddepicted in black in the figure) are not always clear: for example, in the zone of the RGB space representing whiter hues (i. e. R, G and B values nextt o2 5 5 ) . T h i s is may due to the strong illumination which degrades the colorp e r c e p t i o nh a v i n g almost a whitening action and conferring to the same marker different hues including white (see sec. 4.1.1).

In endoscopy, the camera sensor is usually a mono-CCD with Bayes filter and the acquired image is in RGB format. This color space, though,i sn o ti d e a lf o r color-based segmentation since its channels intrinsicallyc o n t a i nl u m i n a n c ei n f o r m ation which makes color perception (and consequent RGB values) sensitive to light [START_REF] Doignon | [END_REF]]. For retrieving a true color information, the chroma component, which is independent from luminance variation, must be extracted.

In the L*a*b color space, the chrominance is separated from lightness information (L)a n di sr e p r e s e n t e db yt w oc h a n n e l sa and b (Fig. for the aforementioned color segmentation. However, as it can be deducted from Fig. 4.3,t h eb l u ep a i n ti sn o tap u r eb l u e and, consequently, only one channel is not enough to represent the entire variability. The color "blue" seems to contain some green components (negative values along the a axis) and this fact obliges the use of the entire bidimensional chrominance space for having the real color representation. Despite the fact that blue is not pure, the two colors still cover two separate areas in the a * b space, which only partially overlap the background gamut in the whiter zone (the same behavior present in the RGB space).

Given the quite large inter-distance between the different distributions, a model for each apparent color content of the markers can be determined, so as to use it for detecting the same color in further video frames. Observing the distribution of the two color clusters, it seems that the density of the point cloud decreases with the distance from the center of the cluster, that can be considered the nominal (or real) paint color. This fact suggests that the probability of one pixel to pertain to one of these two class can be modeled like a Gaussian centered on the nominal value (the barycenter of the point cloud) and whose shape is determined by the variance of the specific color samples. The obtained (blue and yellow) Gaussian Models (GM) (represented as a confidence ellipse in Fig. 4.3)c a nb eu s e dw i t ht h ep i x e l sa * b values of a new image frame providing, as a result, a probability image where the pixel intensity represent the probability of that pixel of being yellow marker (or blue marker).

According to the dataset used in this work, the centers (c)a n dt h ev a r i a n c e matrixes (C)o ft h efi t t e dG a u s s i a n sa r et h ef o l l o w i n g( r e s p e c t i v e l yf o ryellow -ywand blue -bl-): In the literature, assigning a probability value instead of a"hard"labeliskno wn as "soft" segmentation. In our case, the combination of a "soft" segmentation with ad y n a m i ct h r e s h o l dd e t e r m i n a t i o nb a s e do ni m a g ei n f o r m a t i on demonstrated to bring more repeatable and robust results than a conventional" h a r d "s e g m e n t a t i o n where a specific label is assigned to each pixel [Chen 2007].

c yw =[-6.
The soft segmentation step, in fact, impedes to have ap r i o r iexclusion of some pixels that can result to be useful according to a higher-level classification or segmentation criteria. The error due to "hard" decision, instead, cannot be changed and bring difficulties for image analysis.

A hard classifier such as ADABOOST [Schapire 1990]wasalsoexperimentedbut the described drawbacks make it less suitable for such application. This learning algorithm learns the boundary which best divides (with the greatest clearance) the background from the foreground according solely to the training set. If a case was not "seen " during the training, the classification could fails make it necessary to learn the markers color in different scenarios according to the operation which is carried out.

On the other hand, in a probabilistic approach, if the color iss l i g h t l yd i ff e r e n t from the model the response would be weaker but thanks to a dynamic threshold such information can be recovered (and not negatively labeled from the off). However, a minimum threshold should be settled anyway to be able to determine when the instrument is absent.

Ac o l o r -b a s e ds e g m e n t a t i o n ,t h o u g h ,l o o s ei t se ff e c t i v e n e s si ft h ec a p t u r e dc o l o r changes during the video sequence or in cluttered environment where other object can have the same color. In these cases, solutions have been proposed either updating the color model [Raja 1998]o rc o n s i d e r i n gt h es t r u c t u r eo ft h eo b j e c ti t s e l fo r border information [Moreno-noguer 2003].

However, the most critical issue for color-based segmentation is, obviously, when the color information is corrupted or totally absent. As already pointed out at the beginning of this section, the strong illumination degradest h ec o l o ri n f o r m a t i o n even causing strong specular reflectance on the body of the instrument removing any color information.

Effect of Specular Reflectance

When the specular component of the reflected light hitting thec a m e r as e n s o ri so f high intensity, the sensor saturates and a white region is formed in the image at the portion of surface responsible of that specular reflection.

As described before, the light is emitted by two punctual sources very close to the camera which provides high intensity in the center of the observed scene letting the border regions darker. The material of the instrument is very reflective, but even using matte painting for the markers, the high intensity of the light illuminating the center of the scene corrupts the instrument image showing wide white zones on the colored markers.

However, assuming that the bendable part of the instrument isat o r u s ,i t s apparent contours correspond to physical points where the normals to the instrument surface are perpendicular to the line of view. Thus, the rays of light hitting that portion of surface cannot be reflected directly to the camera sensor which will collect only the diffuse reflection component for that portion. It can be stated, then, that the white zones deriving from direct reflectance are only present in the "middle" of the imaged instrument when it is exposed to direct high intensity illumination. As ar e s u l t ,e a c hm a r k e ri sp a r t i t i o n e di n t o ,a tm o s t ,t w oy e l l o w/blue parts.

This decomposition of the instrument body and the issue of distinguishing the instrument region by other regions with the same colors (false positive) brings to the need of a specific classification stage. It has the role of "understanding" each detected region recognizing which of them really pertain to the instrument and, in that case, to what marker they pertain.

Regions Interpretation

Looking at the aspect of the instrument and considering only the color, we decided to described the instrument as a multi-part object formed by the ordered sequence of the colored markers from the base to the tip. However, due tol i g h ts a t u r a t i o n , the appearance of these parts may change due to the division ofas i n g l em a r k e ri n multiple regions.

Ap o s t -p r o c e s s i n gs t a g e ,t h e n ,i sn e e d e dt oc l a s s i f yt h er e a lo b j e c tp a r t sw i t h the right order (based on a topological criterion) and takingi n t oa c c o u n tp o s s i b l e parts variations.

Global optimal solution methods such as Graph-Cuts or hierarchical structure do not fit exactly the problem due the complexity (or impossibility) of defining (and optimize) a global cost function that takes into account order relationship. Furthermore, the shape and the appearance of the instrument is subjected to changes during the operations and, for this additional reason, pictorial structure [Felzenszwalb 2005] based methods cannot be straightforwardly applied to our case and would return high score false positives due to the effect of specularities. We prop ose here to simplify the problem and solve it lo cally, in two steps: firstly, at r e ei sc r e a t e da c c o r d i n gt ot h er e g i o n st o p o l o g ya n d ,s e c o ndly, that tree is processed till obtaining a directed path from the base (the root)t ot h et i pm a r k e r( t h e unique leaf).

To build the tree, a neighb oring connectivity (4-or 8-connected) is not sufficient to determine the correct relationship between the regions and, consequently, the right order. Therefore, a new connectivity is defined, based on two structural characteristics of the bendable section:

1. The markers are adjacent to one another alternating between blue and yellow 2. The centroids of the different markers in the image should lay on an hyperplane whose tangent at each point should not present strong discontinuities.

More specifically, starting from the root, a new node is connected to its parent only if it satisfies these two properties, i.e. assuring the alternation of colors and avoiding strong discontinuities on the orientation of subsequent branches (Fig. 4.4). Building the tree this way allows to order the regions from the base to the tip and to have, on each generation (cfg. Fig. 4.4), all the candidate regions associated to the same marker.

Moreover, for each generation, the nodes are considered 2 by 2a n daputative node is created which represents the region resulting by the possible fusion of the two considered regions, eventually providing the tree in thefi r s tl i n eo fF i g . 4.6. The appearance of each marker can be represented by two factors: one shape factor and a topological factor. The first is defined as the length and width of each marker according to its principal axis (left) and the second as the distance from the considered marker and its parent (right).

The final aim, though, is to obtain the directed path composed by the markers of the bendable part of the instrument. For pruning the tree, at i m ec o n s i s t e n c y criterion is used, assuming that from one frame to the following, the shape of the markers substantially stays unchanged.

To exploit this consideration, two marker descriptors are proposed to be taken into account:

• a shape factor depending on the second order moments of the region (computed along its principal component directions);

• a topological factor depending on the distance of the considered node (real or putative) from its parent node (=region).

These two factors are modeled as stochastic variables. Defining a as the event of being a particular marker A, s the event of having a particular shape S (shape factor) and d the event of having a specific distance D from the parent node (topological factor), the shape/topological frame-to-frame similarityc a nb ed e fi n e di nt e r m so f :

p(A|S, D).
As a first approximation, we can consider both shape and topological events as Gaussian processes which can be defined as:

p(S (1) |A) ∼ 1 2πσ (1) sa exp(- (s i -µ (1) sa ) 2 2[σ (1) sa ] 2 ) (4.1) p(S (2) |A) ∼ 1 2πσ (2) sa exp(- (s i -µ (2) sa ) 2 2[σ (2) sa ] 2 ) (4.2)
for the shape (Fig. 4.5(a))a n d

p(D|A) ∼ 1 2πσ da exp(- (d i -µ da ) 2 2σ 2 da ) (4.3)
for the topology (Fig. 4.5(a)), where µ and σ are respectively the mean and the standard deviation of the observed values of, respectively:

• the euclidean distance from the father region centroid (µ da ,σ da )

• the second order central moment computed along the principalc o m p o n e n t s directions of the specific marker (µ

(1) sa ,σ (1)
sa ) and (µ

(2) sa ,σ (2)
sa ).

These quantities are specific for each marker from 1 to 5.T o t a k e i n t o a c c o u n t the variation of size and shape during instrument movement, these parameters are updated at each frame considered the observations on the last1 0f r a m e s .

Except for the root, the joint probability value of each region (putative or real) of being marker given the shape and topological observation can be considered as the reward associated to each branch of the tree:

p(A|S, D) ∼ 1 2 (p(S (1) |A)+p(S (2) |A)) p(D|A). (4.4)
The tree pruning, then, can be solved generation by generation (see Fig. 4.6)b y selecting the branch with the highest probability. If the chosen branch is associated to a putative node, then, the two involved nodes are deleted and substituted by the putative node where the centroid and shape factor are computed for the region resulting from the merging of the two involved regions. Subsequently, the tree structure and the branches values are updated.

If, in one generation, there are more than 2 candidates then the process is iterated till no better (i.e. with higher probability) solution can bef o u n d .

At the end of this process, the tree will be the sequence of the 5c e n t r o i d s coordinates ordered from the base to the tip of the bendable section (see second step of Fig. 4.7 with the corresponding labeled image).

Bézier Curve Representation of the Borders

Once the ROI and the skeleton (composed connecting the markerc e n t r o i d s )h a v e been determined, the idea is to exploit that information to retrieve the superior and inferior apparent borders of the instrument so as to look for the color transition along them (see Sec. 4.4).

The conventional edge detection methods usually return a cloud of points which usually must be post-processed to eliminate outliers and select only the wanted edges.

Parametric curve fitting can be used to smooth contours extracted from local maximum gradient extraction, thus providing sub-pixel precision, and helping rejecting outliers. According to several test on synthetic data, we decided to use quadratic Bézier curves which resulted to be sufficient to represent the apparent borders of the projection of a torus, at least from the particular point of view of the considered endoscopic system. Therefore, we propose here a complete framework for robustly fitting Bézier curve to bidimensional data using an M-estimator.

A Bézier curve of degree n is a parametric curve defined as follows:

B(t)= n i=0 b i,n β i (4.5)
where the polynomials

b i,n = n i t i (1 -t) n-i ,i=0, 1, •,n (4.6)
are the Bernstein basis polynomials of degree n.T h ep o i n t sβ i are the so called control points of the curve. The polygon formed by connectingt h ec o n t r o lp o i n t s from β 0 to β n is called the control polygon and the resulting curve is contained in the convex hull convex hull of the control polygon.

The best fitting Bézier curve can be defined in several manners depending on the objective function. The objective function must be chosen properly and, especially in computer vision problems, with particular attention to the robustness it confers to the estimation. Different measures of robustness are proposed in the literature, but the most common are the breakdown point and the influence function.T h e former represent the minimum fraction of outlying data that can cause an estimate to diverge arbitrarily far from the true estimate. The latteri st h ec h a n g ei na n estimate caused by the insertion of outlying data as a function of the distance of the data from the (not corrupted) estimate.

The ideal situation for a robust fitting is having a breakdown point of 0.5 and an influence function which tends to zero with increasing distance. The breakdown point cannot achieve higher values because, when more than the half of the data are "outliers", it is impossible to determine which ones are the true data to fit.

The commonly used estimators become, then, insufficient to getarobustfitting, according to these two indicators. Indeed, in the case of least mean square (LMS), as i n g l eb a dp o i n tc a nb r i n gt h es o l u t i o nf a rf r o mt h er e a lfi t t ing (i.e. breakdown point equal to 0)a n dt h ei n fl u e n c eo fap o i n ti sp r o p o r t i o n a lw i t hi t sd i s t a n c e. An alternative (not ideal though) is using least absolute deviation as fitting quality function which presents a bounded influence function (it is the derivative of a piecewise linear function).

Ar o b u s ts o l u t i o ni nc o m p u t e rv i s i o ni sr e p r e s e n t e db yM-Estimators,w h i c h are a generalization of Maximum Likelihood Expectation and Least Squares. We propose, here, a possible formalization for computing the "best" fitting Bézier curve using M-estimators.

M-estimator for Bézier Curve Fitting

Let X = {x i } be a set of data points and let a ak -d i m e n s i o n a lv e c t o ro ft h e parameters to estimate. The M-estimate of a is â =argmin

β x i ∈X ρ(r i,a /σ i ) (4.7)
where r i,a is the error distance or residual function relative to x i and ρ(q) is a robust loss function that grows subquadratically and is monotonically nondecreasing with increasing |q|.I na d d i t i o n ,σ 2 i is the variance (scale) of the scalar value r i,a . The solution in (4.7)i ss o l v e db yfi n d i n ga such that

x i ∈X ψ(r i,a /σ i ) dr i,a da
1 σ i =0 (4.8)
where ψ(q) is the derivative of ρ(q).T h es a m ep r o b l e mc a nb et r a n s l a t e di ns o l v i n g

x i ∈X w(r i,a /σ i ) dr i,a da
1 σ i r i,a =0 (4.9)
where w is a particular weighting function such that w(q)q = ψ(q).T h i sl e a d st o ap r oc e s sk n o w na s" I t e r a t i v eR e -w e i g h t e dL e a s tS q u a r e s "( I RLS), which alternates steps of computing the weights w i = w(r i,β ,/σ i ) using the current estimate of the parameters and solving (4.9)t oc o m p u t ean e wa with given weights. Following the M-estimator approach, an ob jective function can be conceived to fit the Bézier curve on the extracted borders:

n b i b =0 ρ(B x (t b i ,β x ) -p (x) b i )+ n b i b =0 ρ(B y (t b i ,β y ) -p (y) b i ) (4.10)
where β x and β y are the vector of the x and y coordinates of the control points:

β x =[β (x) 0 ,β (x) 1 ,β (x) 2
]

β y =[β (y) 0 ,β (y) 1 ,β (y) 2 ] and p b i =(p (x) b i ,p (x)
b i ) T are the superior (inferior) detected border points, and t b is the parameter of the Bézier curve such that B(t b i ,β) is equal to p b i for some β that must be found. Comparing (4.10)w i t h( 4.7), the vector of parameters a is represented by the coordinates of the Bézier control points a =[ β 0x β 1x β 2x β 0y β 1y β 2y ] T and the residuals are expressed as the distances along each coordinates of corresponding points

r i,a =[B x (t i b ,β x ) -p (x) b i ,B y (t i b ,β y ) -p (y) b i ] T .
As previously stated, this could be solved by means of the IRLSa l g o r i t h mb u t thanks to the adopted formalism with Bézier curves, a closed form solution can be found for (4.9)o n c et h ew e i g h t sa r ek n o w n . Am a t r i xf o r ms o l u t i o ni sp r e s e n t e dh e r et ofi n dt h eb e s tfi ta c c ording to the M-estimator framework.

Rewriting the Bézier curve (4.5)a s

β 0 + t(-2β 0 +2β 1 )+t 2 (β 0 -2β 1 + β 2 )
it can be expressed, for each coordinate (x or y), in a matrix form as:

B x (t)=TCβ x
where C is the matrix of the Bézier coefficients:

C = ⎡ ⎢ ⎣ 1 -21 -220 10 0 ⎤ ⎥ ⎦
and each line i of T is composed by [t 2 b i t b i 1] corresponding to each detected point i. For a quadratic Bézier curve (n =2 ), the lines defining the control polygon are the tangents to the Bézier curve in the first and last controlp o i n t s . A st h e Bézier curve must fit the border points, it is plausible to statet h a ti d e a l l yt h e control polygon must be tangent to the apparent borders at itst e r m i n a lp o i n t s . This property indirectly suggests that the first and last control point must lie on the terminal parts of the apparent superior and inferior contour of the object.

This characteristic allows to define the different t b as the curvilinear coordinate over the curve connecting all the points in the given order. Its value will be 0 for the first (superior/inferior) border point and 1 for the last. A proper way to compute these values is assigning:

t i b = |d i -d i-1 | n b i=1 |d i -d i-1 | where d 1 =0 d i = i j=2 p bj -p bj-1 2
Thus, at each iteration and for each coordinate x and y,t h eβ solving the least square problem is defined as:

β (j+1) x =argmin βx n i=1 w (j) p (x) b i -T i Cβ x 2 =((TC) T W (j) (TC)) -1 (TC) T W (j) p (x) b where p (x) b
is the vector containing all the x-coordinates of the detected border points and W is a diagonal matrix gathering the weights computed at step j for each border point. The problem has a closed form solution and the new weights can be computed after each IRLS iteration.

Considering that the whole a =[β 0x β 1x β 2x β 0y β 1y β 2y ] T is taken into account and permitting an over-definition of the variables, the whole problem can be defined (and solved) in a matrix way by composing the matrices defined so far. Thus, new (2) Creation of the tree using the specified connectivity: Branch 2 is not created since it does not fulfill condition 1, whereas branch 1 does not respect condition 2 because the orientation of this branch strongly differs from the preceding outline. The tree is finally processed to get a directed path. ( 3)

T i is [t 2 i b t i b 1000] for residual along x-coordinate and T i =[ 000t 2 i b t i b 1] for ones along y; the matrix of coefficient C = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ 1 -21 0 0 0 -2200 00
The skeleton is used for searching border points. These are fitted with Bézier curves along which color discontinuities are searched and considered as corners (4).

At each iteration, then, the searched β is

β (j+1) =((TC) T W (j) (TC)) -1 (TC) T W (j) a.
Thanks to this formalization and solution by M-estimator, a continuous representation of the border can be computed which is able to filter the outliers due to image noise of miss-detection (see Sec 4.5).

Overview of the whole process

First Stage: Candidate Regions

For any new frame the L*a*b representation of each pixel is retrieved and its probability of being either yellow or blue is computed according to the built Gaussian Models.

Subsequently, the image is labelled (yellow / blue / background) according to two thresholds (one for blue and one for yellow) iteratively computed to assure a reasonable number of candidate regions according to the considerations on light saturation and markers division: at most 8 yellow and 6 blue regions. The initial thresholds are increased (or decreased) if the number of binarized regions is higher (or lower) of the admitted quantity. Afurthercontrolisperformedtocheckifeachconnectedregion actually pertains to a single marker. The convex hull of each binary region must contain, in prevalence, one color only (it may contain background or neighbor marker little portion). If it is not the case it means that two different markers resulted in only one region which must be divided by an erosion.

Second Stage: Image Interpretation

For building the tree, the ro ot no de must b e firstly detected. Actually, it can be defined as the first yellow marker encountered walking throught h ep r o j e c t i o ni n the image of the instrument tube which precedes the bendable part (cfr. Fig. 4.8). Indeed, in STRAS the axis of the passive part of the instruments wrt the camera is constrained by the mechanical structure and its projection in the image can be coarsely estimated. This defines the entering direction of the instrument in the image which may vary during the operation for example when the tip oft h ei n s t r u m e n ti s pushing and organ. To be robust to these possible entering direction variations, the root position at step i is used to define the direction along which searching the root at step i +1.

Moreover, the marker associated to the base of the bendable section (the root of the tree) is generally well detected because it lies in a part of the workspace that does not receive much direct illumination.

The tree is then built and processed till obtaining the sequence of the 5 centroids coordinates ordered from the base to the tip of the bendable section.

Third Stage: Apparent Borders extraction

At this stage, a local approach is adopted to extract the uppera n dl o w e ra p p a r e n t borders of the instrument. The skeleton obtained by connecting the ordered centroids is evenly sampled and gradient magnitude local maximaa r es e a r c h e da l o n g the normal to the skeleton. The candidate contours points areselectedaccordingto their gradient direction (along the normal to the skeleton) and their position (near the boundaries of the labelled image obtained in the previouss t e p ) .

More in details, for each ordered couple (c i , c i+1 ) of subsequent centroids, the segment connecting them is uniformly sampled and for each sampled point, a line g i,j is built, which passes through this point and which is perpendicular to c i+1c i (see step three in Fig. 4.7). The instrument edges can then be defined as composed by the points satisfying the following three properties:

• Being a local maxima in the gradient magnitude (computed along x and y of each RGB channel) along g i,j .

• The orientation of the gradient of the candidate point shouldb ec l o s et ot h e orientation of g i,j relative orientation between the phase of the gradient of the candidate point and the direction of g i,j should be within a little interval around zero.

• The candidate point should be contained within the crown defined around the final binary image obtained in the previous step. Such crown can be obtained by subtracting the binary erosion of the image obtained in step two (Fig. 4.7) to its dilation: this results in a thick crown where most probably the borders are.

Along each g i,j the outline of the gradient is analysed and the local maxima are recorded both in upper and lower directions.

Once collected all the upper and lower border candidate points, only those respecting the three aforementioned properties are kept. If some ambiguity is still present, the furthest pixel from the c i+1c i segment is chosen. In fact, when walking along g j from the centroid to the outer zone, two peaks in the gradient can be encountered if the marker in question is affected by saturation: from white (saturated) central zone to yellow (blue) marker and from yellow (blue) to the background color. But only the external transition is the one describing the apparent border.

Bézier Fitting

In fitting the borders, the outliers have small residual magnitude, therefore a strong rejection of outliers has been adopted. This can be achieved with the so-called "harddescenders" whose loss functions quickly tend to zero outside a certain threshold. One example is the Beaton and Tukey loss function [Beaton 1974]( F i g . 4.9)b u t , unfortunately, this usually means that the objective function is non convex, implying that the IRLS method can converge to a local minimum. Furthermore, using Beaton and Tukey function still implies a breakdown of 0 due to possible leverage points [Stewart 1999]: outliers positioned far from the remainder of the data in the independent variables as well as far from the fit to the uncorrupted points. However, thanks to a good initialization and, especially, to the formalization with Bézier curves, non-sense local minima can be avoided and higher breakdown values can be achieved. In fact, the order of the extracted edge points and the topological information (where the border begin and where it ends) give furtheri n f o r m a t i o nt h a t have been integrated in the optimization thanks to how the t parameter of Bézier curve is computed.

According to Beaton and Tukey loss function, the updated weights can be calculated as:

w (j+1) = ⎧ ⎨ ⎩ 1 -r/σ c 2 2
, |r|≤c

0, |r| >c

If the scale σ of the residuals is not known ap r i o r i ,i tc a nbee s t i m a t e d( a n d ,i f needed, re-estimated after some iterations) in several manners. The most adopted and less sensitive to noise is taking the median of the residuals. Optimal c must be chosen according to the application (even though suggested values exist). Here the value c =1.5 pixels seems to be the most appropriate.

Fourth Stage: Corner Localization

To finally extract the markers corners, the color transitions along the Bézier curves should be found. Since blue and yellow are complementary colors, the boundaries between two consecutive markers appears on the image as a gradual transition from yellow to blue (or vice-versa), provided the absence of overexposure (where color information disappear).

As a first approach, one may think to use the probability imagesr e s u l t e db y the application of GM and search the local minima along the mentioned curves. Although, the pixels pertaining to the intermarkers frontiers does not fit any of the GM and, therefore, the resulting probability image present wide low value regions in correspondence of these frontiers. Thus, searching the local minimum on this wide flat valley would have no sense and surely lead to corners mislocation.

For this reason, in this step, we consider again the L * a * b image. As yellow and blue are defined in opposite position along the b axis of the L * a * b color representation, the same gradual color transition between two markers should be observable in the b channel of the image. In other words, in correspondence of yellow marker there would be a high b value which smoothly decreases in the blue zone. Considering the origin of the b axis as the frontier between the yellowish and bluish colors, the intermarker frontier points (i.e. the marker corners) can be defined as the zero-cross points of b values along the superior or inferior borders.

To help the detection of such p oint avoiding p ossible outliers, the binary masks of the yellow and blue markers ( If the interpolated line does not overlay the transition (forexamplewhenamore external border is detected) the corners are retrieved in another way. By realizing a weighted interpolation between the two Bézier borders, one obtains a set of similar curves inside the instrument image which cross all the markers (Fig. 4.11(a)). The previously defined blue-yellow transitions are then searched on these lines (Fig. 4.11(c)), hence providing a sampled version of these transition limits (Fig. 4.11(d)). An average tangent direction of the upper-most (resp. lower-most) points of each blue-yellow limit is then computed to estimate the directiono ft h el i m i tn e a rt h e borders (green line in Fig. 4.11(d)). The intersection with the upper (resp. lower) border then gives the upper (resp. lower) corner point for each transition.

For the corners corresp onding to the tip, the same pro cedure described in the third stage is employed. Since no skeleton information is available in the last segment, the g 5,j are built using the same inter-distance and direction of g 4,j .T h i s is repeated till detecting the absence of yellow information. The last point of the superior and inferior border points are taken as the final corners.

Results and Comments

Using Bézier curves has, in fact, a triple purpose: obtaining ac o n t i n u o u sr e p r e s e ntation of the borders with consequent sub-pixel precision, filtering the outliers of the local gradient-based border detection and complete the possible missing infor- mation. These last two properties are not that noticeable in controlled environment but they become more useful in in-vivo environment. To study these capabilities, two artificial cases are artificially generated: the first where 60% randomly chosen detected border points were randomly misplaced within a range of √ 15 2 +15 2 pixels and the other where some randomly chosen misplaced border points (60% of them) were deleted. The results are shown respectively in Fig. 4.12(a) and Fig. 4.12(b).

The analysis of the breakdown point shows, somehow, the plausibility of the constraints on the control points and the effectiveness of thewholealgorithm. E v en though a strong noise (50 pixels) is added to the 30% of the detected points, the IRLS solution (cyan curve in Fig. 4.12(c) )i sn o ta ff e c t e d . T h i si st h a n k st o the synergy between the outliers rejection capacity of the Beaton and Tukey loss function with an added robustness (in terms of breakdown point) thanks to the Bézier formalization and implicit constraint of consideringt h efi r s ta n dl a s tc o n t r o l points to be near to the first and last detected border points.

The same figure also shows the comparison of this method with a classical least mean square solution highlighting the good quality of the proposed solution even with highly corrupted information.

However, this last constraint is the bottom line of such formalization. When the initial and ending points of the detected borders are strongly misplaced, the algorithm needs the other border information to be correct and dense to succeed (Fig. 4.13(a) and (b)).

The same process has been used in in-vivo cases showing good results in free space without occlusion and demonstrate to be quite robust tos t r o n gs p e c u l a r i t y effects (Fig. 4.14)a n dm i r r o r i n ge ff e c to nt h eo r g a n s( F i g . 4.14(c) and 4.14(d)). As it does not take into consideration any template of the object nor multiframe analysis, the evident limit of this approach is occlusion (Fig. 4.14(f) and 4.14(e)) which should be taken into consideration in future work (see 7.2). Once chosen the suitable features, two opposite approaches have been studied to determine strengths and weaknesses of each of them in the purpose of pose estimation.

Firstly, a model-based approach is described which considers some aprioriinformation about the mechanical structure of the system and the shape of the instrument to compute the extrinsic parameters of the endoscopic cameraa n d ,c o n s e q u e n t l y , the entire pose of the instrument.

Secondly, a learning based method is presented which considers the image to pose system like a black box where the relationship between two quantities can be learned provided that they are measurable and a set of this input-output measurement is available. For the mo del-based approaches, the mo del of the ob ject geometry is supposed to be known and the features of interest are usually geometric( v e r t e x e s ,e d g e s , distances between vertexes, ...). A complementary problem,t h e n ,i st ofi n dt h e correspondence between the 3-D model features with all or part of the corresponding 2-D image features. This process is referred to as interpretation or correspondence problem [Doignon 2007]. Once this correspondence has been solved, the best pose is defined in term of a chosen similarity criterion between theprojected3-Dfeatures and the 2-D features.

Chapter 5

Model-Based Pose Estimation

Many works have been presented on this topic with simple shaper i g i do b j e c t s [Doignon 2007]. Fewer solutions, instead, have been proposed regarding articulated [START_REF] Hel | Model based pose estimation of articulated and constrained objects[END_REF]a n ds h a p ev a r y i n go b j e c t s [START_REF] Croom | Visual sensing of continuum robot shape using selforganizing maps.I nR o b o t i c sa n dA u t o m a t i o n( I C R A ) ,2 0 1 0[END_REF][START_REF] Padoy | [END_REF]e v e nt h o u g h , nowadays, this topic has acquired a superior interest also because of the increasing use of flexible tools in many applications.

In our work, the pose estimation problem is seen as the dual problem of an image based visual servoing [START_REF] Marchand | [END_REF]]. Assuming that the kinematic and camera models are known, for a given instrument configuration, a synthetic (virtual) image of the flexible instrument can be computed. The aim, then, willbet ofi n dt h epo s e that minimizes the visual measurement error between the virtual and actual feature points. This has not a closed-form solution but can be solved iteratively by moving the virtual camera towards the real one along the (locally) best direction given by the minimization process.

In part I the correspondence problem has already been addressed and int h i s chapter the attention will be seeking a solution as an optimization problem.

As a first hypothesis, the geometrical mechanical model of thes y s t e mi sc o nsidered to be known (Sec. 5.1). However, since the system is designed with a play between the channel and the instrument so as to allow the instrument to smoothly slide inside it, a second model is used so as to improve the poseaccuracy(insec. 5.2). This extended model takes into account the described variability on the parameters defining the instrument position and orientation.

With a single view, the accuracy of the extrinsic parameters estimates relies on the ability to extract the perspective effect in a reliable fashion from the imaged 3-D objects. Sometimes, the noise or the partial view of the instrument (and consequently partial visual information) can affect the correctness of the estimation because the visual information is insufficient or ambiguous. To overcome such issues, multi-frame continuity and coherence with the motor data can be taken into account (sec. 5.4).

The approach is firstly validated with synthetic data and thent e s t e do no u r robotic experimental cell and on a manual version in in-vivo scenario.

For the rest of this chapter, the following notation is used: P i indicates any 3D point coordinates of the i-th section in the camera frame, P i,u and P i,l are the 3D upper and lower points of the i-th section, where i =1is the base and i =6 is the end of the bending part. Furthermore, P =[ P 1,u •••P 6,u , P 1,l •••P 6,l ] T , P u =[ P 1,u •••P 6,u ] and P l =[ P 1,l •••P 6,l ] T .T h es a m en o t a t i o nb u tw i t hal i t t l e p will be used to express the coordinates in pixels of the same points projected in the image. The hat symbol (•)i su s e df o rt h ee s t i m a t e dv a l u e s .

Fixed Mechanical Model Parameters

As announced, a first solution is to consider that the mechanical structure parameters are totally known and that the only movements of the instrument are due to its 3 DOFs (λ, φ, θ).T h i sa p p r o a c hi si n s p i r e do ft h a to f [START_REF] Reilink | [END_REF]], where they adopt a visual measurement error based on the centroid of 4 markers attached to the instrument (3 on the bendable part and 1 on the instrument).

Translating this very approach to our case (no markers on the tip and corner related features), the problem would be to find:

r =argminχ 2
where the considered cost function χ 2 can be written, for N image points, as :

χ 2 = 1 2 N i=1 w i [p i -ˆp i (r )] 2 .
(5.1)

For each feature p oint, the following well known relation canb ew r i t t e n :

˙p i (r)=L i (r)ṙ , (5.2)
where L i is the extended image Jacobian as defined in the precedent chapter. Once stacked all the L i into what can be called the global extended jacobian matrix L, the optimization problem can be solved iteratively by Gauss-Newton approach as proposed in [START_REF] Reilink | [END_REF]]. From given initial conditions, the correction to apply to r at iteration n to decrease the visual measurement error is calculated as follow:

δr = c L † W e = c ( LT W T W L) -1 W T We (5.3)
where e is the visual measurement error (a column vector containing the 2D errors p i -ˆp i (r )), L W † is the weighted Moore-Penrose pseudo inverse of an approximation of the global extended jacobian matrix ( L), c is a fix gain and W is a weighting matrix. The interest of having a weight associated to the measurement error is the possibility to weight the confidence associated to some feature wrt to others.

In [START_REF] Reilink | [END_REF]], they empirically determined that the information associated to the tip worth higher confidence (somehow confirmed by our preliminary sensitivity study) and, therefore, W is a diagonal matrix with all ones except for those entries corresponding to the coordinates of the upper and lower points of the tip where it is set to the value 4.

In simulation, where the model used to create the reference image is the same as the one used for the optimization process, this approach has very nice results either using marker centroids (as in [START_REF] Reilink | [END_REF]]) or the corner based features proposed in chapter 4.

The limits of this approach show up when applied in real contexts: observing the results in Fig. 5.1(g),i tc a nb en o t i c e dt h a tt h ei m a g ec o r r e s p o n d i n gt ot h e best configuration (in the terms of minimizing the cost function) is not coinciding with the original endoscopic image. To confirm that the obtained solution were not local minima, several starting configurations have been considered. Taking into account the last row case (cf Fig. 5.1), using the different initial conditions listed in table 5.1 the optimization process leads to exactly the same result both in terms of residual error (root mean square error computed consideringt h ed i s t a n c e sb e t w e e n extracted corners and estimated corners) and robot configuration. This suggests that the found solution is not a local minimum1 .

As one can expect, the bad convergence in the image is confirmedb yh i g he r r o r s in the estimation of the 3D position of the tip of the instruments: in a test sequence of 116 images where the Ground Truth (GT) was made with a electro-magnetic (EM) tracker attached to the end of the bendable part, the resulting RMS deviations are 3.16, 4.54 and 5.45 mm respectively for x (horizontal), y (vertical) and z (depth) coordinates in the camera frame. These visual and numerical results suggest that

Initial Conditions 2D error Estimated Pose ( λ [mm],φ[deg],θ[deg]) [pixels]

[λ, φ, θ] [40, 0, 50] [10, 10, 0] [20, -90, 90] 13.12 [24.78 79.66 5.44] [30, 90, 30] [50, -20, 15] Table 5.1: This table shows, in its first column, the configurations used to initialize the optimization algorithm (described in section 5.1)f oraspecificcase( cf . lastro wof Fig. 5.1). The initial condition, in this case, seems not to affect the final solution both in term of image residuals (2nd column) and estimated configuration.

some of the hypotheses made are not reliable either on the geometric or on the camera model. The low reprojection error obtained during the camera calibration (RMS error of [0.48 0.49] pixels over 15 different points of view) suggests that the large error in the image space cannot be linked only to a poor camera calibration. The main reason, then, seems to be an incorrect geometrical model and, consequently, we decided to extend this model by incorporating new "spurious" DOFs corresponding to those mechanical parameters whose real values are difficult to measure correctly or can vary during the manipulation due to the described mechanicalp l a y s . T h e s eD O F s , that will be called mechanical DOFs, are the position of the instrument channel exit ([x ch ,y ch ])a n di t so r i e n t a t i o n( a n g l e s(ψ, µ)), both wrt to the camera.

Variable mechanical parameters

With the will of including the four mechanical DOFs, a new vector of DOFs can be defined as r e =[x ch ,y ch , ψ, µ, λ, φ, θ] T (cf. Fig. 3.1(a) and (b)). The z ch coordinate is considered to be constant and equal to the nominal value. Inf a c t ,c o n s i d e r i n g the possible movements along z ch would be redundant with variation of the λ DOF: variations of z ch (ceteris paribus )o n l yr e g u l a t e sh o wm u c ht h ei n s t r u m e n te x i ti t s housing channel which results to be similar to the effect achieved with variation of λ.

When the number of parameters to estimate increases, the probability of run into local minima increases as well. In addition to that, evenw h e nt h e r ea r em o r e matches than unknown free parameters, it is often the case that some of the matches have some relationships which lead to an ill-conditioned problem. In this case, the considered parameters are not totally independent in terms of effect on the image features: one needs only to consider, as example, x ch and ψ parameters which both produce an horizontal apparent displacement in the image.

Within this context, Gauss-Newton's method as described above can present The reference image features are the markers apparent corners (red spots). On the left the result considering the theoretical CAD parameters. On the right the solution with some tolerances on the mechanical parameters.

some problems of convergence. Thus, to assure convergence and speed up the optimization process, a Levenberg-Marquardt (LM) approach has been chosen instead of the described Gauss-Newton.

The formalization is exactly the same with the only differencethattheproposed optimal step is a combination of the Gauss-Newton solution with a gradient steepest descent step. The combinatorial weights ∋ are varying according to the cost value at each iteration: if, with the computed step, the cost χ2 decreases ∋ is divided by af a c t o rg r e a t e rt h a n1 ,w h i l s t ,o nt h ec o n t r a r ys i t u a t i o n ,t he weight related to the gradient descent is increased. The equation ( 5.3)b e c o m e s :

δr = c ( LT W T W L + νI) -1 LT W T We .
(5.4)

Managing the mechanical parameters

In addition to the adoption of LM, with the aim to improve the results and limit local minima due to partial redundancy on some parameters, prior constraints can be introduced to define the default to use in case of absence of further information [Lowe 1991]. Lowe proposes to add these constraints to the previous system of equations (5.2), obtaining 2 :

I L ṙ e = d e
where the identity matrix I add one row for each parameter whose displacement can be specified ap r i o r iand the vector d collects all the desired default displacement values of each parameter. The problem in such solution is thatt h e r ei sn os p e c i fication on the trade-offs between meeting the default values ort h ea c t u a le x t e r n a l constraints coming from the image. The solution proposed in [Lowe 1991]i st o weight each row so that each element on the right-hand side hast h es a m es t a n d a r d deviation:

W L ṙ e = Wd e
where W is a diagonal matrix whose elements are the inverse of the standard deviation of each parameter. Doing so, during the optimizations t e pe a c hc o m p o n e n t will contribute in proportion to the number of standard deviation from its nominal value.

For the p ose computation of the flexible instrument, no prior information is available for the 3 DOFs of the instruments, but it can be reasonable to assume that the actual values of the mechanical parameters are close to their nominal values defined by the CAD model furnished by the manufacturer (x ch = -13.3,

y ch =6.2, ψ =10 • and µ =0 • ).
As already mentioned, the variation of the mechanical parameters is mainly due to the necessary mechanical play between instrument and housing channel. Constant weights as in [Lowe 1991], then, would not model this phenomena properly: amec hanicalpla yletsalmostafreemo v emen tinasmallin terval around the resting position whereas the constraints get stronger as it gets far from its resting state (described by the nominal value furnished by the manufacturer).

Ap e c u l i a rw e i g h t i n gf u n c t i o n ,t h e n ,i su s e di nt h ec o s tf u n c tion (cf. eq. ( 5.1)) to maintain the estimation of these parameters (r

m =[ x ch ,y ch ,ψ,µ] T )c l o s et o their nominal values r * m =[ x * ch ,y * ch ,ψ * ,µ * ] T .
T o r e p l i c a t e t h e m e c h a n i c a l p l a y description, this weighting function should have almost a "dead-zone" around the parameters' nominal values and an increasing steepness as getting far from them. Furthermore, it must b e C 1 in its whole domain (R in this case) so as to be used in the optimization process.

The function we propose is the modulus of a cubic function where dead-band and steepness are parametrized:

ρ(u)= k w 3a 3 |u| 3 ,a∈ R + ,k w ∈ R + (5.5)
Its corresponding weighting function is:

ζ(u)= k w a 3 |u|,a∈ R + ,k w ∈ R + (5.6)
where k w determines the steepness of the function outside the dead-zone whose width is defined by parameter a.G o i n gb a c kt ot h eL o w e ' sf r a m e w o r k ,i tw o u l d result in:

⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ζ(x * ch -xch )0 0 0 0 ζ(y * ch -ŷch )0 0O 4×3 00 ζ(ψ * -ψ)0 00 0 ζ(µ * -μ) L ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ ṙ e = ζ(r * m -r m ) T (r * m -r m ) e 2d where ζ(r * m -r m )=[ζ(x * ch -xch ),ζ(y * ch -ŷch ),ζ(ψ * -ψ),ζ(µ * -μ)].
From the p oint of view of the optimization pro cess, the similarity criterion also changes so as to take into account the weighted distances of the estimated mechanical parameters from their nominal values. The new cost function to minimize is:

χ 2 = 1 2 N i=1 w i [p i -ˆp i ] 2 + ρ x ch (x * ch -xch )+ρ y ch (y * ch -ŷch )+ρ ψ (ψ * -ψ)+ρ µ (µ * -μ)
(5.7) and the new visual measurement error vector is re-defined as:

e = (r m * -r m ) T e 2d
T where e 2d is the row vector containing the errors committed on the visual features (upper and lower corners points) along each image coordinate:

e 2d =[ ( p u,1 -p u,1 ) x (p u,1 -p u,1 ) y ••• (p u,n -p u,n ) x (p u,n -p u,n ) y (p l,1 -p l,1 ) x (p l,1 -p l,1 ) y ••• (p l,n -p l,n ) x (p l,n -p l,n ) y ]
Since the variable parameters have increased (vector r now include the mechanical parameters), an extended version of the geometrical Jacobian must be considered. Equation (3.31)n o wb e c o m e s( s e eA p p e n d i xC for the computation details of the different blocks A i ):

Ṗ = J g ṙ e = A x ch A y ch A ψ A µ A λ A φ A θ ṙ e .
(5.8)

According to (5.8), the global geometrical Jacobian will have at most 24 rows (it may be less according to the number of the extracted visual feature points) and 7 columns. Considering the same ordering as in the error vector, the first 4 rows relate the variations of the mechanical parameters r m to the variations of the deviation from their nominal values (r * mr m ).S i n c et h e s e q u a n t i t i e sd e p e n d o n l y o nr m itself, the top left corner of the global J g (4 × 4 matrix) will be the opposite of the identity matrix. The rest of this matrix is made up of the composition of the J g i calculated for each upper (J i,u )a n dl o w e r( J i,l )bo r d e rpo i n to ft h ei-th section of the bending part of the instrument. The corresponding weighting matrix is:

W = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ζ(e 1 )0 0 0 0 ζ(e 2 )0 0O 4×20 00 ζ(e 3 )0 000 ζ(e 4 ) O 20×4 W 2d ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ .

Expressing the error wrt the Bézier curve

Observing more closely the images and the extracted features, it has been seen that the fitting of Bézier curve gives precise results whereas the exact placement of the corner point along the Bézier curve turns out to be more complicated. The strong frontal illumination seems to modify how the color is received by the endoscopic camera, consequently the inter-marker color transition in the image becomes wider (or even absent in case of saturation) and, therefore, a sharplimitismorecomplexto detect. Therefore, it can be stated that the misplacement error of the corner points location along the direction of the real border is greater than the one along the perpendicular direction. This aspect has been included to improve the optimization result. The idea is to express the 2D error vector (p i -pi ) wrt a new frame whose axes are parallel (s ∥ )andperpendicular(s ⊥ ) to the Bézier curve at the corresponding feature point (p i )a ss h o w ni nF i g . 5.2. This allows to weight differently the two new coordinates according to the experimental considerations explained earlier. This improvement can be smoothly added to the optimization framework explained so far. Indeed, (3.25)i se q u i v a l e n tt o :

T i ˙p i = T i J I i J g i ṙ (5.9)
where T i is the 2D homogeneous transformation between the image framea n dt h e local frame associated to the i-th feature point. Thus, for each point p i it can be written: 5.10) where ẽ = T i e i is the error expressed in the Bézier -based coordinate frame and W i will have different weights for each component. The generalization considering more than one corner point p i is straightforward.

δr = c ( Li T T i T W i T W i T i Li + νI) -1 LT T i W i T W i T i e i (
To compute T i , the tangent to the points of the Bézier curve must be known. This can be computed from the curve derivative

B ′ (t)=2(t -1)(F 1 -F 0 )+2t(F 2 -F 1 )
provided that the value of t corresponding to the considered p i is known.

Simulation Study

Before applying the algorithm to real video sequences, some simulations tests have been executed. The aim of these test was simply to verify if theproposedmechanical parameters tolerances could cope with the uncertainties on the model. Therefore, no Bézier reference frame is employed. For these tests, two models have been considered: the first model (correct model) is considered to be the real one and it is fixed and used to create the virtual reference images (synthetic images substituting the real endoscopic images) and the supposed model which differs from the reference model by some error (offset) on the mechanical parameters and which is used in the optimization process to estimate the 3D pose (scheme of Fig. 5.3). Obviously, the proposed weighting functions are centered around the supposed mechanical values.

C (J T WJ) -1 J T W Supposed Kinematic Model Camera Projection (r m , p) (r m , p )
To decouple the effects of the errors, the offset was added either on the channel position or on the channel orientation with respect to the camera (the cases are listed in table 5.2,t h ec o n s i d e r e dc o r r e c tv a l u e sa r e(x ch ,y ch )=( -13.3, 6.6) and ψ = -10 • )a n dµ =03 .F o re a c ho ft h el i s t e dc a s e s ,t h eo p t i m i z a t i o na l g o r i t h mw a s tested on 500 robot configurations and using two different settings for the weighting function ( 5 d)). On the other hand, the proposed method seems to be able to mitigate the counteracting effect deriving by model uncertainties. A fundamental role on this method, though, is played by the weighting policy. The dead-band (DB), indeed, must be chosen according to the expected error on the mechanical parameters: in the case where the DB is smaller than the error on the model, the optimal solution is far from being the correct one (red line in Fig. 5.4(a)). On the other hand, if the dead band is correctly centered, the convergence

uncertainty [mm] -5 -4 -3 -2 -1 0 1 2 3 4 5
[mm] to the optimal solution is faster and slightly more precise (given an equal number of iterations) when the dead-band is narrow (Fig. 5.5(a) and (b)).

Although, choosing an arbitrarily large DB would probably increase the number of local minima (increasing the risk of getting no optimal solution at all) due to the already mentioned redundancy between different DOFs. In fact, the variations of some components of r e have similar effect on the image and, since the optimization criterion is based on image features only regarding the bendable part, a compensation between two or more parameters may occur leading to only slightly changes or no effect on the cost. This is the case, for example, of x ch and ψ which do not have the same effect in 3D but several combinations of the two can lead to the same image. This is somehow confirmed by the results in Fig. 5.6(a):e v e nt h o u g h the model error is introduced only on ψ (cases 12-22 of table 5.2), the estimated channel exit position is farer from the actual value when a larger DB is used. In this case, the algorithm cannot identify the actual mechanical value and both ψ and the channel position seem to loose their physical meaningi nf a v o ro ft h eg l o b a l criteria. The risk of such behavior would be more evident in a larger scale. Let us consider, for example, an horizontal and bent instrument, very similar images can be obtained with the nominal values (x ch = -13.3 and ψ = -10 • wrt the camera coordinate reference frame) but also with an x ch far from the camera on the left and ap o s i t i v eψ,a si ft h ei n s t r u m e n tw e r eo u t s i d et h ec h a n n e la n dp o s i t i o n e da l m o s t parallel to the (x, y) camera plane. If the mechanical parameters are not limited around their nominal values, such solutions would be valid even though they would not have any physical meaning. This could be solved if the non-controllable part of the instrument were taken into account, but this is often not possible since, most of the time, only the bendable part is outside the channels during operations.

The beforehand mentioned behaviors are amplified when noise is added to the visual measurements. The results in Fig. 5.7(a) and 5.7(b) underline again the importance of the ap r i o r iknowledge for the centering of the dead-band. With the effect of noise, though, this aspect acquire more importance.I nf a c t ,i nc a s eo fs m a l uncertainty, the adoption of a large DB policy provides lowera c c u r a c yr e s u l t sw i t h respect to a narrow DB policy. However, large DB is applicablew h e nt h eap r i o r i is really untrustworthy or when large movement are expected to occur during the operation. Even though, in this case, would be better to update the center of the weight function (according to some data) and use a strict DB policy. Also the forementioned parameters compensation issue is more evident in presence of image noise: when a large tolerance is admitted, the estimation error on the mechanical parameters consistently increases (channel position in Fig. 5.7(c) and ψ in Fig. 5.7(d))l o o s i n gt h e i rp h y s i c a lm e a n i n g .

Since, for biomedical applications, high accuracy is needede v e nw i t hn o i s yd a t a and considering that we can quite precisely centering the weighting function (i.e. assuming that the mechanical parameters nominal values furnished by the provider are sufficiently precise), the most adapt weighting policy is using a narrow DB.

Motor Sensor Data Fusion

Earlier it was stated that no ap r i o r iwas available for the real DOFs of the instrument, but this is not totally true. In fact, if on one hand no prior hypothesis or considerations can be done on the configuration, on the other hand robot sensor data can be exploited to have an initial guess of the instrument position. Actually, a good confidence can be given to the rotation and translation encoders values whilst encoders value of the deflection are difficult to relate to real instrument deflection value due to all the phenomena in the transmission chain (friction, cable extension or slack).

As pointed out previously, it is difficult to have a high precision on TCP position looking only at the encoders values, but it can be thought to exploit their information to discriminate those cases where the visual measurements are only partially available or in those positions of the visual features space where the uncertainty on the estimation is high (cf. sec. 3.5). In other words, the idea is to fusion the encoder data (or better said the instrument pose variation deriving by the encoder data variations) with the visual measurement. For this scope, a Kalmanfi l t e rf r a m e w o r ki s used where the increment of encoder value is used to update thes t a t ep r e d i c t i o n and the visual-based pose estimation is used as the measurement.

Thanks to this formalization, wrong estimations based on thei m a g ec a nb ed etected and mitigated. Furthermore, this allows to inherently consider a smoothness and continuity in the movement, although the precision on thee n c od e r si sn o ts u c h to allow an accurate estimation in case of hidden instruments( a n dc o n s e q u e n c el o s t of all the visual information).

For applying the Kalman framework, the state space mo del and the measure-

uncertainty [mm] ([deg]) -5 -4 -3 -2 -1 0 1 2 3 4 5
[mm] ment model of the process are supposed to be known and both corrupted by noise which is usually considered to be additive, white, Gaussian and with zero mean. In mathematical terms, considering a linear, discrete-timed y n a m i c a ls y s t e m ,t h e process equation is: 5.11) where F k+1,k is the transition matrix bringing the state x from time k to k +1 and w k is the process noise such that

x k+1 = F k+1,k x k + w k (
E[w n w T k ]= Q k for n = k 0 for n ̸ = k .
The measurement can be described by a similar equation:

y k = H k x k + v k (5.12)
where y k is measurable at time k and H k is the measurement matrix. For measurement noise v k the same hypothesis hold and

E[v n v T k ]= R k for n = k 0 for n ̸ = k .
In the case an external action has to be consider, the system model ( 5.11)c a n be extended as follows:

x k+1 = F k+1,k x k + Bu k + w k (5.13)
where u k is the aforementioned external control action and B describes how u k acts on the state evolution.

The presented framework can be adapted to our purpose of data fusion in the following manner:

• The process can be considered like a time-discrete process whose sampling time coincide with the camera frame rate.

• It can be expressed as a linear process if the DOFs vector is chosen as state (x k =[ λ, φ, θ, x ch ,y ch ,ψ,µ] T )a n dt h em e a s u r e dv a r i a b l ei st h es t a t ei t s e l f . Fo cusing on the "natural" DOFs (λ, φ, θ), it reasonable to assume that the state changes only if a motor action occurs. On the other hand,themec hanical parameters do not seem to follow any deterministic evolutionla w,thereforew e preferred not to perform any update to this part of the state either. For these reasons, we decided to consider the transition matrix equal to the identity and take as measurement the state estimated by the visual information, this means: F k+1,k = H k = I, ∀k.

• The motor velocity can be considered as the external action u k which updates the state (i.e. the DOFs values). Consequently,

B = ⎡ ⎢ ⎢ ⎢ ⎣ 10 0 01 0 O 3×4 001 /r O 3×4 O 4×4 ⎤ ⎥ ⎥ ⎥ ⎦
The mechanical parameters, although estimable from visual information, are not real controllable DOFs and, thus, no motor velocity is available for them and neither their variation has a clear relationship with thee n c o d e rd a t a variation. Thus, the last four component of u (encoder data) and the last four rows of B (r e l a t i o n s h i pb e t w e e nm o t o rv a r i a t i o na n ds t a t e )w i l lb ee q ual to zero. Anyway, considering true the supposition of the mechanical play and the movements in free space, the robot configuration (measured byt h ee n c o d e r s ) and the estimated values of the mechanical parameters shouldb er e l a t e di . e . the position of the instrument inside the channel depends on the particular configuration.

Since the bending is obtained by actuation on antagonist cables (cf Fig. 2.20 and 3.1(b)), the related encoder returns the difference between the two cables lengths. Dividing by radius of the instrument bendable section r,thedeflection angle in radians is obtained.

• The measurement and prediction confidence trade-off can be made vary in function of the available quantity of the visual information. Logically, if the visual information is only partially available (only few corners are detected), the confidence of the model should be increased to the detriment of the confidence on the measurement.

To obtain a linear system, we have considered the estimated s tate as measurement. If on one hand, this choice allows to have an easy formalization of the Kalman filter on the other it threatens the hypothesis over the measurement noise. In fact, even though the image noise can be considered white Gaussian and zero mean, nothing can be told about the noise associated to a measurement (like the one proposed) that depends, in turn, of the image noise level. Therefore, itc a n n o tb ec e r t a i n l y stated that the measurement noise is still additive, white, Gaussian and with zero mean.

Experimental Results

To evaluate the accuracy and relevance of the prop osed metho dforposeestimation, as e q u e n c eo fi n s t r u m e n tc o n fi g u r a t i o n sw a sg e n e r a t e ds p a n n ing the whole visible left instrument workspace. At each configuration (defined by three values of the 3 DOFs) an image was acquired simultaneously by the endoscopicc a m e r aa n db yt h e two cameras of a stereo system. The sequence of configurationsw a sc h o s e ns u c h that the instrument makes a continuous movement in the picture too as if it were ar e a lt r a j e c t o r y . T h a n k st os t e r e ov i s i o nt h e3 Dp o s i t i o no faw h i t eb a l li n -b u i l t with the tip of the instrument is determined and used as GroundT r u t h( G T )( s e e appendix B for more details on the set-up for creating the 3D GT). The accuracy is, then, evaluated as the 3D distance between the estimated white ball position (based on instrument model and image features) and the GT.

To obtain some meaningful statistics on the accuracy of the proposed method, as e q u e n c eo f295 poses is considered. Over the corresponding images the segmentation process failed on 14 images which gives a useful set of 281 images. A strict Dead-Band policy (cf table in sec. 5.3)i su s e df o rt h ea d m i t t e dt o l e r a n c e so ft h e mechanical DOFs using for µ the same weighting function as ψ. Following the Bézier -based reference frame, the weight associated to all the parallel components of the 2D errors is 8 and the one associated to the perpendicular components is 10.

Moreover, the analysis of several videos of surgical operations using the Anubis platform shows that the instruments displacements can be considered very small between successive frames. Thus, the solution configurationf o rt h ei m a g ef r a m ej is used as initial guess for the optimization problem in frame j +1,w i t ht h er e s u l t of speeding up the computation time for the pose estimation.

The good quality results over these images (Fig. 5.8)a r es u p p o r t e db yt h e round mean square (RMS) errors over each coordinate (in mm): 2.03 ± 1.65 on x, 2.07 ± 2.07 on y and 2.78 ± 2.57 on z axis.

To obtain these results, all the images have b een considered including those where the features where only partially visible. In fact, when the instrument is very near to the camera the initial part of the instrument bendablesectionishiddeninside the channel or is outside the image. Moreover, in order to use projective geometry in pose estimation, the reference image must be rectified. It is a well known issue that for high-distorsion lenses the reliability of the camera model is weak near the border of the image. In the rectification process, then, the "reliable" field of view is limited and, usually, a part of the border is not taken into consideration causing a partial exclusion of the painted bendable section from the image.

When the validation 3D point is near a set of visual features (the corners associated to the tip in this case) used in the similarity criterion, a good convergence on this feature is likely to correspond to a good estimation of such point (3D position) no matter the estimated orientation is. In this case, a good 2Dc o n v e r g e n c eo nt h e corners associated to the tip of the instrument would suffice toh a v eag oode s t i m ation on the x and y 3D coordinates of the instrument central axis terminal point (the tip of the instrument). Furthermore, since the instrument diameter is known, aq u i t eg o o de s t i m a t i o no nt h es c a l ea n d ,i n d i r e c t l y ,o ft h ed epth of the tip can be obtained. However, performing the validation on such point does not give any information on the orientation of the end-effector and, consequently, on the robot higher is the error). In this graphic, it can be noticed that some of the highest errors are located on a crown which correspond to those instant wherea ni n v e r s i o no ft h e deflection movement occurs. In this case, as already explain in precedence, the lack of the antagonist cable must be recovered before appreciating an actual movement of the instrument. This means that the motor is moving and, consequently, the state is update whereas the instrument stays still. Therefore, a high error is accumulated which, inevitably, affects the pose estimation in the successive frames. Indeed, this "crown" of high errors is not visible in Fig. 5. 10(a) where the Kalman filter is not employed and therefore the real behavior of the proposed model based method can be analyzed. Here, the highest errors seem tob ec o n c e n t r a t e d in a specific region corresponding to those poses where the instrument is deflected, pointing upward/downward and near to the camera. The same behavior can be noticed even if the motor data are considered. This behavior can be explained by the fact that, in these configurations, the instrument appears in the left border of the image where the distortion is higher. This may make the visual measurements less reliable indirectly compromising the estimation pose.

Method

In-vivo Qualitative Results

The same process has been applied on in-vivo video sequences, acquired in the abdomen of a pig during single port surgery realized with the manual short Anubis platform. No ground truth was available in this case and the proposed analysis is only qualitative. In Fig. 5.11 some examples are shown, where the instrument is first detected (as shown in the precedent chapter) and subsequently its 3D pose is computed based on the extracted corners (green crosses). The corresponding virtual image of the instrument is then reprojected in the image with the estimated 2D position of the center of the grasper (green dot).

These results seems to confirm the capability of the proposed method to estimate the pose of flexible instrument basing on the corners of colored markers.

Furthermore, the consideration of mechanical plays in the model allows the pose estimation process to converge even when the instrument is pushing on the organs and its position inside the channel is consequently modified (Fig. 5.11(a) and (b)). In this case the estimation of the position of the tip is biasedb e c a u s eo ft h ed e f o rmation of the instrument.

The adaptability of the proposed pose estimation scheme is also demonstrated by the fact that the flexible system used for in vivo experiments is different of the one used in the laboratory. Nonetheless the pose estimation works without any modification of the algorithm.

Finally, as also shown in the precedent chapter, the feature extraction algorithm fails when most of the instrument is hidden (Fig. 5.11(e)), since it does not take into consideration any template of the object nor multi-frame analysis. However, even with partial information the computed 3D pose is acceptable in terms of reprojection, indirectly demonstrating the interest of using a model in terms of segmentation.

Conclusion

This chapter has presented the comparison between two model based methods: the first considers the geometrical model known, whereas the second takes into account the possible mechanical plays between the instruments and the housing channel.

Given the evident difference between the two approaches' results, it can be stated that the introduced tolerances on the mechanical parametersa l l o wt oa c c o u n tf o r the mentioned mechanical plays and help compensate uncertainties in the kinematic model (including those regarding the real position of the camera axis wrt to the axis of the channel) or, going a step further, the instrument deviation due to contact with organs (even though, in this case, the weighting function parameters should be adapted or modified along the operation such that the dead band could admit larger displacement either in term of translation or orientation).

Discrete qualitative results have been obtained also in in-vivo environment using adifferentsystemwrttheoneusedinlaboratory . Theseresults shows the adaptability of the proposed method to different (although similar) systems and, even, when the geometric structure is modified e.g. when the instrument is slightly pushing on the organ. Moreover, from a visual point of view, the same framework could be adapted to recover the instrument outline in case of partial occlusion.

Nevertheless, the observed errors are still large for a surgical application and they are probably the sum of intrinsic issues this approach presents. In addition to segmentation errors which obviously affect the pose accuracy, a part of the error may be explained by the fact that Kalman hypothesis are not totally satisfied. Other errors, then, can arise from considering an incorrect model (e.g. uniform curvature for the bending part, bending on a plane, ...) or can be partly explainable from imperfections in the registration between the camera and theStereosystemGround Truth.

Alearningapproac hcanhelpa v oidingman yofthesemen tionedp r o b l e m s ,s i n c e it learns the model from the input and output data without making any hypothesis on the system model. The next chapter presents a study of the potentialities of two learning approaches in the context of position estimation from a single 2D image.

Chapter 6

Learning Based Pose Estimation It can be said that a machine learns when it can improve its performances through its experiences.N o w a d a y s , t h e r e a r e m a n y a s p e c t s t h a t c a n b e l e a r n e d conferring to learning a wider and wider variety of meanings according to its applications: model identification, image segmentation, interpretation of data (scenes, situations,..), etc. However, two main categories of learning problems can be distinguished based on the kind of the output variables: in regression the objective is to learn a function and return a numerical data output, whereas classification aims at inferring a quality or category based on some input features/characteristics/quality.

Learning algorithm are usually divided into two phases: the machine is first trained on a specific set of known samples to learn the rules which it will apply to new samples to determine its output numerical value or its class.

For example, let us imagine that a function f has to be learned and denote h the hypothesis that can be made on the shape of f and x =(x 1 , ••• ,x n ) the vectorvalued input. Thus, h can be thought as being implemented by a device that has x as input and h(x) as output (which, again, can be either a numerical value or a class according to the problem that has to be solved). The onlyn e e d e da-priori is on the class of function H to whom h pertains. The best h is selected on a training set Ξ of M input vector samples which is usually built according to experiments.

In this framework, there are two major settings in which the function can be learned. In supervised learning,bo t hi n p u t( x)andcorrespondingoutput(y)v a l u e s of each sample of the training set are known. The optimizationc r i t e r i a ,t h e n ,c a n be defined by comparing h(x) and the associated output y.

In unsupervised learning the training set is composed only with input vectors without function values. The problem in this case is usually to find the best partitioning of the training set, obtaining some meaningful categories of the input data. The output values in this case will be the category name or the pertaining cluster of the input data.

Once the best guess among the hypothesis H is learned (based on the training set), this can be used with new samples to estimate the output value.

An important role in learning is played by the training set andh o wm u c hi t is representative of the general problem. It can be asserted that the higher is the cardinality (M )o ft h et r a i n i n gs e ta n dt h ed i v e r s i t yo fi t ss a m p l e s ,t h eb e t ter this algorithm performs in terms of estimation accuracy. This fact is quite intuitive: in fact, the more representative cases are considered in the training set the higher is the probability that the new unseen sample is near (or similar) to those already "seen" during machine training. On the other hand, learning method would loose its sense if the training set contains all the possible cases thatc a noc c u r s : i nt h i sc a s e , in fact, the approach would be more similar to a look-up table and no "estimation" is needed.

In the context of pose estimation, the basic idea is to learn the function relating an image feature with the 3D position of the TCP. As enunciateda tt h ee n do f the previous chapter, the principal aim of adopting a learning approach is to try avoiding all those sources of error deriving by an incorrect system model. Achieving high accuracy in the geometrical model can become hard in the case of flexible instrument which can present several behaviors difficult to detect and, consequently, describe mathematically (e.g.: non constant curvature, no planar curvature, different behavior according to the shape of the rest of the body, ...).

In this case, though, the class (H)oftheregressionfunctionishardtobekno wn apriorifor this kind of problem. Moreover, since the input vector derive from image features, the training set is affected by noise.

Taking into account these characteristics, the searched solution should avoid noise over-fitting and should be able to easily adapt to any shape, allowing local variations fitting without affecting the whole approximation. For this reason, polynomials or linear fitting are unsuitable. A good choice in these terms is the use of a network of Radial Basis Function (RBF) with Gaussian Kernels [START_REF] Broomhead | [END_REF]], [START_REF] Moody | [END_REF]]w h i c hp r o v i d e st h ed e s i r e dl o c a la p p r o x i m a t i o n( a c c o r d i n gt oa ni n itial cluster computation) and, thanks to the chosen kernels,c a ne a s i l ya p p r o x i m a t e any smooth function.

Although, the not satisfying quality of the RBF results and theparticularshape of the considered function brought us to investigate better approaches both for the clustering (Sec. 6.2)a n df o rp o s i t i o ne s t i m a t i o n( S e c . 6.4). The proposed methods are first tested with synthetic data with a particular attention to input noise effect on the learning process (Sec. 6.5). Finally, the experimental results are presented and analyzed.

Radial Basis Functions Network

Principle

The idea underneath this method is very simple: it proposes toa d d r e s s ,i nalocal way, the global problem of approximating an unknown function. Basically, the desired function is expressed as a combination of local functions (named Kernels) which are responsible of the shape of only a portion of the original function domain. These kernels are defined radial functions since their influence diminishes with the increasing distance from their centers.

Therefore, the regression result is a Network of Radial Basis Function (RBF) which, in turn, can be thought as a 2-layers artificial neural network (ANN) that employs RBF as activation functions.

Thus, given

x =[x 1 , ••• ,x 2n ]
T the 2n dimensional input vector, the estimation of the output (ŷ) set up by RBF network is a weighted sum of K kernel functions ϕ k :

ŷ = h(x)= K k=1 γ k ϕ k (x)+γ K+1 (6.1)
where γ k are the learned coefficients which describe the contribution of the k-th kernel to the estimation of the output. In the case of Gaussiank ernels,ϕ k takes the form [START_REF] Bors | [END_REF], Lendasse 2003]:

ϕ k (x)=Y k e -1 2 (x-C k ) T Σ -1 k (x-C k ) (6.2)
where C k is the center of the Gaussian kernel, Σ k its variance and Y k its amplitude.

As any other learning approach, a training phase is needed forbothlayers: firstly the location and shape of Gaussian kernels are computed through unsupervised learning (first layer) and, secondly, the best linear combination of these kernels is computed by supervised learning to approximate the function. Before giving the implementation details for the case concerning image-to-3D-position, an onedimensional example is presented to clarify and visualize what the different layers mean and how they are built.

Example

Let say that we want to approximate with a RBF Network the function in Fig. 6.1(a) which describes the behavior of the system that has to be learned. Obviously, we do not know the continuous outline of that function, what we do know, instead, is a set of measurements each of whom is composed by the input value andt h ea s s o c i a t e d system response to such input. This collection of data represents the training set which is visualized in Fig. 6.1(b).

As previously described, the first stage of training is to compute the location and shape of the Gaussian kernels. A technique of unsupervised learning (e.g. k-means) is used to cluster the input data of the training set into subsets containing "similar" data (indicated by the different colors in fig 6.1(c)). Several similarity criteria can be used also depending on which unsupervised learning methodi sg o i n gt obeu s e d . Once obtained the clusters, the barycenters of the data composing each cluster are taken as the centers of the Gaussian kernels (C k )w h e r e a st h e i rc o v a r i a n c em a t r i x is taken as Σ k .T h e a m p l i t u d e Y k ,i n s t e a d ,i sc o m p u t e da st h ea v e r a g eo ft h e corresponding output values pertaining to each cluster k.

Thus, to each cluster a Gaussian kernel is associated which represents a local model of the corresponding data (Fig. 6.1(d)). From the same picture it can be observed the local influence of each Gaussian which decreasesi nr a d i a ld i r e c t i o n with the distance from the center.

The second stage is to compute the optimal combination (i.e. compute γ k )o f these local models according to a global criteria, such that it minimizes the residual error wrt the collected output values (supervised learning)o b t a i n i n gt h efi n a lr e s u l t in Fig. 6.1(e) (green curve). Now, an approximation of the searched function is known and it can be used to compute the value of any new sample even if it did not pertain to the training set. What is evident, though, it ist h ef a c tt h a tt h e approximation is poor in that portion of the input space wheref e wo rn ot r a i n i n g samples are available.

Learning the 3D position from image

In the case of 3D position estimation, the input vector x is composed by some image descriptors of the instrument and the output values will be one of the 3D coordinates of the tip.

The input space is composed by salient visual features (as state vector components) coming from the visual measurements.

To simplify the learning pro cess, the entire problem has b eens u b d i v i d e di n t o three sub-problems aiming to learn three single-output functions describing the relationship between the image features and each of the coordinates of the 3D tip position. More precisely, the objective is to compute the function relating the 2n dimensional column vector x composed by the 2n (x, y) coordinates of the n chosen salient image features to, alternatively, one of the three coordinates of the 3D TCP position i.e. X, Y or Z (depending on the considered case).

More formally, defining y =[ X, Y, Z],t h ef u n c t i o nh i of each sub-problem is responsible of computing the estimation of the i-th output variable ŷi (alternatively

N k Number of elements in each cluster: k N k = M C k,i
Center of gravity of each cluster k in the estimation problem of the i-th 3D coordinate.

γ k,i
Coefficient weighting the influence of the k-th kernel for the estimation of the i-th 3D coordinate. X, Y or Z):

ŷi = h i (x).
Consequently, equation (6.1)c a nber e w r i t t e na sf o l l o w sf o re a c h3 Dc oo r d i n a t e ŷi :

ŷi = h i (x)= K k=1 γ k,i ϕ k,i (x)+γ K+1,i (6.3)
where, this time, γ k,i are the learned coefficients that describe the contribution of the k-th kernel to the estimation of the i-th output. The Gaussian kernel (6.2), ϕ k,i takes the form:

ϕ k,i (x)=Y k,i e -1 2 (x-C k,i ) T Σ -1 k,i (x-C k,i ) (6.4)
The training process must, then, be repeated three times to solve the three subproblems. Therefore, three training sets are needed: each oft h e mc o n t a i n st h eM samples composed by the 2n dimensional input vector x m (with m going from 1 to M )a n dt h ec o r r e s po n d i n go u t p u ty m,i that must be learned.

The steps described in the one-dimensional example can be straightforwardly adapted and employed to this case. Firstly, the input data x m are divided into clusters: one cluster for kernel. For this aim, K randomly selected x m have been taken as initial centers for a k-mean-like algorithm. Once the algorithm converged, for each of the found K clusters the center of gravity, the variance and amplitude are calculated from the points composing the specific clusters. These values are respectively used as centers (C k,i ), amplitude (Y k,i )a n dv a r i a n c e( Σ k,i )o ft h eG a u s s i a n kernels (cf. equation (6.4)):

C k = 1 N k N k m k =1 x m k Y k = 1 N k N k m k =1 y m k Σ k = X T
k X k where x m k and y m k are the x m and y m,i samples pertaining to the k-th cluster, N k is the number of elements of each cluster ( k N k = M ), whereas X k is the matrix containing the different x m k in its columns:

X k = x 1 |•••|x N k
Secondly, the K coefficients γ k,i (cf. (6.3)) are computed so as to minimize the difference between the estimation ŷi and the corresponding training set values y m,i :

γ k,i =argmin M m=1 (y m,i -ŷm,i )
For each 3D co ordinate, the problem can b e translated in mini mizing the following cost function:

χ 2 i = 1 2 M m=1 y m,i - K k=1 γ k,i ϕ k,i (x m )+γ K+1,i 2
which, in matrix form, can be written as:

χ 2 i =[Y i -Φ T i Γ i ] T [Y i -Φ T i Γ i ] (6.5)
where Y i is the vector containing all the M 3D i-th coordinate output values of the training set, Γ i is the vector of the K +1 regression coefficients

Γ i = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ γ 1,i . . . γ k,i . . . γ K,i γ K+1,i ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (6.6)
and

Φ i = ⎡ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎣ ϕ 1,i (x 1 ) ••• ϕ 1,i (x M ) . . . . . . . . . ϕ k,i (x 1 ) ••• ϕ k,i (x M ) . . . . . . . . . ϕ K,i (x 1 ) ••• ϕ K,i (x M ) 1 ••• 1 ⎤ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎦ (6.7)
contains the kernels (computed for the i-th 3D output coordinate) evaluated in the distinct input samples of the training set x m .E a c hr o wi sc o r r e s p o n d i n gt oo n e kernel k.

Since the cost function (6.5)i sl i n e a rw r tt ot h ec o e ffi c i e n t s ,t h em i n i m i z a t i o n has a closed form solution given by:

Γ i =(Φ i T Φ i ) -1 Φ i T Y i (6.8)
Such a function approximation is efficient as long as enough points are available so as to finely follow the function variations (consider Fig. 6.1(e) in the region where no training point are available). Indeed, the reconstructed function drops to zero as soon as there is no more points to support it [Benoudjit 2003]. Counterbalancing this behavior by increasing the number of kernels would result in less smooth functions with, possibly, large oscillations (with the risk of overfitting). Since RBF are efficient locally and since linear approximation better fits slow and global variations, a combination of both is proposed. Thus, the role that the RBF network assumes is to refine the linear estimation. The training process is totally similar to what described so far with the only difference that the new fitting data for the RBF network are expressed with respect to the linear estimation result. So the new training output data for the RBF network will be:

ỹm,i = y m,i -α T x m
where α is the vector of the global linear regression:

α =(X T X) -1 X T Y i
where X is the matrix containing the training input samples in its rows.

Gaussian kernel has great versatility in function approximation but, given its shape, it seems to provide a better fit for continuous functions with smooth outline, whereas it seems to be less adapt for piecewise linear or discontinuous functions.

To obtain a go o d lo cal representation of the data, it must b e assured that the clusters used for computing the Gaussian kernels gather samples whose output outline (i.e., in this case, one of the three 3D coordinates of theTCP)issmoothenough to be adequately approximated by a Gaussian. In other words, to achieve a good approximation when performing the clustering on the input domain, we should ensure that proximity and smoothness in the output domain (3D space) is reflected in the input domain (visual feature space). However, this is nota l w a y st h ec a s ew h e n the input samples derive from the perspective projection andt h i si se v e na m p l i fi e d due to the peculiar deformation of the instrument.

Let us consider, for example, the case where the instrument isd e fl e c t e da n d rotating around the channel axis: the depth wrt to the camera plane only slightly changes, whereas the input vector deriving from visual features coordinates related to the instrument body (such as the markers centroids or corners) would be subjected to a substantial variation. This leads to the following Consideration 1. Proximity in the output domain (3D space) is not always reflected in the image feature space.

With the aim of managing this kind of situation and group together all those data pertaining to a smooth 3D manifold, the output should be taken into account in the clustering step. A new clustering domain is then defined and it will be described in next section.

Improved Clustering Domain

In the previous section the conventional procedure to built a network of RBF has been presented pointing out the risks of a clustering performed only on input domain and the consequent necessity of considering the output valueinformingtheclusters.

Indeed, if we retake the example of a bent instrument rotatinga r o u n di t sp r i ncipal axis and we consider the x coordinate of the last marker as the input of our function and the distance of the instrument tip from the camera as the output, we would obtain the outline described in Fig. 6.2(b).T h i sb e h a v i o u rw o u l db ec o r r e c t l y described by a very wide Gaussian as shown in Fig. 6.2(d).U n f o r t u n a t e l y ,i ft h e clustering algorithm take into account solely the input domain, there can be the risk of obtaining a result more similar to the one of Fig. 6.2(c).I nt h i sc a s e ,e v e nt h e combination of the radial basis functions cannot properly describe the tendency of the function in this portion of the domain.

We then prop ose to p erform the clustering algorithm in a jointi n p u t / o u t p u t domain so that the distance among the samples is influenced also by the output values. The ob jective is to promote results similar to those in Fig. 6.2(d) where the data which are near in the output domain are gathered together in the same sub-set. In this case, the gaussian describing the cluster will take into account the variance of those new data assuming a wider shape.

Before going on with the discussion, we should distinguish, atthispoint,between clustering and training domains. With clustering domain is intended the set where the k-means-like algorithm is applied and its dimensionality will be indicated with q cl ,w h e r e a st h etraining domain will be the one containing the input samples used to retrieve the 3D coordinate estimation and it will be of dimension q tr .

To have also in the clustering domain the same dimensionalitya st h ei n p u t , PCA analysis can be carried out so as to take into consideration only those principal components necessary to complete (or not) the output information.

The transformation of the data to this new clustering domain is easily achieved by taking the first q ′ cl principal components (according to the system DOFs or a more exhaustive cross validation on the training set) and stacking them to the three Let us suppose that the instrument is bent and turn around its main axis (a) and that we want to approximate the relationship between the x coordinate of the tip of the instrument and its distance from the camera z (b).

If the clustering algorithm (k-means like) is applied considering only the input data (x coordinate), it would lead to a bad estimation (c).I f ,o n t h eo t h e r hand, we take into account also the output data (the z coordinate describing the depth), it will be more probable to obtain a final clustering result similar to the one shown in (d) which leads to a Gaussian whose shape better describes the tendency of the x-to-z relationship.

coordinates (q out =3 )o ft h eT C Pp o s i t i o n . T h u s ,q cl = q ′ cl +3.I nt h en e x tt a b l e and section this case will be referred to as q ′ cl (+3):f o re x a m p l e ,i ft h ec l u s t e r i n gs e t samples are composed by the first four PCs and the 3 output variables, then it will be denoted as 4(+3).

An alternative could be to take into consideration solely thec o n c e r n e di-th coordinate of the TCP position (q out =1 ). In that case, q cl = q ′ cl +1 and, in the next sections, it will be referred to as q ′ cl (+1) as described in the precedent paragraph.

To pro ceed with the clustering in this joint-space, some precautions should be taken. Since the measurement units between the input and output spaces are different, recentering the data wrt their means and normalizing thec o o r d i n a t e sb yt h e i r standard deviation (computed over all the samples) helps to even out the clustering space in the sense that each sample will contribute with the number of standard deviation from its mean.

The new samples, then, can be defined as

x ′ m =Σ x PCA x PCAm y ′ =Σ y (y -µ y )
where Σ . is a diagonal matrix whose elements are the reciprocal of the standard deviation of each component (PC or output variable) computedo v erallthesamples and µ y is the column vector of the means of each output coordinate.

The new clustering set is composed by the samples formed as follows:

x clm =[ x ′ 1 , ••• ,x ′ q ′ cl input first q ′ cl principal component ,y ′ 1 , ••• ,y ′ qout output variables
] T m (6.9) therefore q cl = q ′ cl + q out .O nt h eo t h e rh a n d ,t h ei n p u tt r a i n i n gs e ti sc o m p o s e db y samples composed by solely input data parameters:

x tr =[x ′ 1 , ••• ,x ′ qtr ] T m . (6.10)
where q tr can be chosen no matter the value assigned to q cl . The k-means-like algorithm is applied to the clustering set of samples x cl which corresponds to a clustering domain of dimension q cl .T h ei n d e x e so ft h es a m p l e s composing each cluster (m)a r er e t a i n e da n du s e dt or e c r e a t et h es a m ec l u s t e r si n the training space (made by samples x trm ))w h e r et h ec e n t e r so fg r a v i t ya r efi n a l l y computed.

For sake of clearness, it may b e useful to reiterate that we want to learn the relationship between the visual measurement (input set) ando n eo ft h ec o o r d i n a t e of the 3D TCP position (output set) whose value is not directlya v ailable. Theinput training set must be formed, then, by the same kind of samples of the wanted input set (i.e. visual measurement) and x trm cannot contain output information which will be unavailable when testing. For the same region, the cluster computed in the joint space, must be reformed in the training input space taking into account only the visual measurements.

RBF: Simulation Result

With the aim to reduce the input domain dimensionality so as toeasetheregression process, we consider, as input, the vector formed by the coordinates of the apparent 2D centroids of the colored markers.

Given the difficulty of imagining the shape of such learned function, a simulation study is done to interpret the role of each parameter, thei n fl u e n c eo ft h ei n p u t dimension and to retrieve more details on the centroids-to-3D-TCP-coordinates relationship.

For this simulation, a synthetic training set has b een built .T h es p a c eo ft h e flexible instrument DOFs ([λ, φ, θ])i su n i f o r m l ys a m p l e da n d ,u s i n gk i n e m a t i ca n d camera model, the coordinates of the markers centroids with the corresponding 3D TCP position is recorded. Atestsethasbeenbuiltinthesamew a ybutfordifferen tinstrument configurations so as to test the learned function over a set of "unseen" positions. In other words, the intersection between the set of the tip 3D position of the training set with that of the 3D positions of the test set is empty.

However, a special attention must be conferred in choosing the training test according to the expected test set so as to avoid extrapolation.I n d e e d ,a sa l r e a d y mentioned, RBF network tends to zero in absence of training samples and, therefore, good estimation results cannot be attained for those test samples that fall in ar e g i o no ft h ei n p u td o m a i nw h e r en ot r a i n i n gs a m p l e sa r ep r e sent. Let us define the visited workspace as that portion of the workspace visited by the TCP 3D positions given a set of robot configuration [λ i ,φ i ,θ i ].T oa v o i de x t r a polation, then, the test visited workspace must be included int h et r a i n i n gv i s i t e d workspace which must be dense enough to prevent from the presence of wide empty regions 1 .

More specifically, both for training and testing set used in simulation, the same visited workspace is considered and defined by

λ ⊆ [25, 45] φ ⊆ -π 2 + π 10 , π 2 -π 10 θ ⊆ π 10 , π 2 -π 10 .
The training set is composed of 280 samples whose corresponding configurations are recovered by taking respectively 5, 8 and 7 equally spread values inside the described intervals. For the test set, the considered robot configuration were defined by taking 5equallyexpatiatedv aluesforλ,5f o rφ and 6 for θ leading to a set of 180 samples. Building the sets in this way allows to obtain a test set with no common elements with the training set assuring, though, that the test visitedw o r k s p a c ei si n c l u d e d in the training visited workspace (which means that no extrapolation problems will be considered). Afi r s tb a t c ho fs i m u l a t i o nt e s t sw e r ec a r r i e do u tc o n s i d e r i n go n l yt h r e eD O F s for the instruments (so no variability in the mechanical parameters) and, as visual features, either the three centroids of the yellow markers (set 1 and 2 table 6.2)o r all the five centroids (set 3 and 4 of the same table). Thus, two input spaces were considered: one of six and the other of ten dimensions. Set 1 and 3 in table 6.2 are both derived by exactly the same instrument configurations, the only thing that changes is the input dimension. Fors e t1t h ep o s i t i o no f the 3 yellow centroids is considered, whilst for set 3 the input data is the vector of the 5 centroids coordinates. The same discussion is valid fors e t2a n d4 .

With the scope of presenting results independent by the random initialization, the RMS errors committed over each 3D coordinate are computedc o n s i d e r i n gt h e estimation results over 20 different random initialization as a unique sequence. This means that the whole training and test process is repeated 20 times with 20 different initialization and the RMS error (given in mm) is computed over 20×180 estimations (in the case where the validation set is either set 2, 4 or 6). For each simulation test, in addition to this RMS error, the worst 3D error norm for the best initialization (in terms of RMS error) will be given (Worst 3D err column in the tables of the next sections).

Cluster Initialization

Due to the particular manner of building the training set, theinputsamplescomposing it do not uniformly cover the input domain. Indeed, the uniform discretization used over the robot DOFs does not correspond to a uniform allocation of the marker centroids in the image. It can occur, then, that portions of the input space do not contain any sample. That is the reason why the initializationo ft h ec l u s t e rc e n t e r s for the k-means is done by randomly choosing K training set samples rather than opting for a uniform sampling in the input domain with the risko fh a vi n ge m p t yo r not representative clusters.

Choice of K

Afi r s tc l e a rr e s u l tt h a tc a nb ee x t r a p o l a t e df r o ms i m u l a t i o nconcerns the proper choice of K value which was not discussed so far. The best value for K (in terms of estimation error on the test set) seems to strongly depend on the density and the quantity of samples composing the training set. The choice of K value can be described by the number of elements per cluster, which resulted to be between 15 to 30 per cluster. Analyzing the results, fewer samples per cluster are not enough to have a good generalization and representation for building the Gaussians, whereas too many samples results in an exaggerate smoothing effect.

The study of the best K value has been carried out using set no. 1 for training and the no. 2 to test and the results are shown in table 6.3).

As an alternative solution, a k-fold cross-validation step can be used to determine the best number of clusters according to the available training set. Before starting the training process, the same training set can be divided into, for example, 4 subsets. One of these subsets is retained as the validation data for testing the model trained over the other 3 sub-sets. The cross-validation process is then repeated 3t i m e s( t h ef o l d s ) ,w i t he a c ho ft h e3s u b s e ts a m p l e su s e de x a ctly once as the validation data. The 4 results from the folds can be combined together and the Case trained on tested on q cl q tr 3D RMS Worst 3D err advantage from the additional information. The observed behavior suggests a first conclusion:

Conclusion 1.
Other things being equal, the use of 5 centroids brings about some piece of information useful for position and, mainly, for depth estimation.

Furthermore, comparing cases 20 and 21 one can observe that 3 components are not sufficient to get the best position estimation result. This is may due to the fact that PCA is not the right choice to reduce dimensionality in this case. Indeed, directions with high variance become a principal component even though such direction has no influence or relation with the output.

Mechanical Parameters Variations

As already analyzed in the precedent chapter, considering only 3 DOFs does not allow to represent all the kinematics of the instrument. Mechanical play between instrument and its housing channels must be taken into account since they influence the appearance of the instrument itself in the image.

If the effect of the mechanical plays want to be considered, thet r a i n i n ga n dt e s t sets must be modified taking into account 7 DOFs.

There are many possibilities to extended the proposed 3 DOFs sets. The first can be to consider that the mechanical parameters variationsaretotallyrandom. In such a case, to create the synthetic data set, it is sufficient toperturbthemechanical nominal values before computing the 3D instrument pose with the kinematic model.

If. instead, the mechanical DOFs are considered as real "controllable" DOFs, a new workspace should be defined based on these 7 DOFs. The training and test sets, then, would be composed as set 3 and 4 by uniformly sampling the 7 DOFs.

At h i r dp o s s i b i l i t y ,t h eo n ee m p l o y e dh e r e ,i st oc o n s i d e rt h at the position and orientation of the instrument in the channel depends on the particular instrument configuration. In this case, similarly to the first option, thev a l u e so ft h e4m e c h a nical parameters are computed before retrieving the entire pose with the kinematic model (and, finally, the synthetic image by the camera model).T h u s , t h e n e w training and test set (indicated respectively as set 5 and 6) are computed in the exact same manner as for sets 3 and 4, the only different aspect is that the mechanical parameters are assuming variable values depending on thes pe c i fi cc o n fi g u r a t i o n [λ, φ, θ]:

• x ch = x * ch +3 * sin(φ) • y ch = y * ch +2 * sin(φ) • ψ = ψ * + π/90 * sin(θ -π/4)
• µ =0:a sf o rt h em o d e l -b a s e ds i m u l a t i o nt e s t s ,i sc o n s i d e r e dfi xa nd equal to 0.

Case trained on tested on q cl q tr 3D RMS Worst 3D err 30 6.5: Tendency of RMS and worst 3D estimation errors using RBF Network according to the clustering (q cl )a n dt r a i n i n g( q tr )d o m a i nd i m e n s i o n a l i t y . T h eh i g h e s t accuracy is achieved when exploiting the entire visual information q tr =1 0 (which corresponds to the 2 × 5 2D coordinates of the markers' centroids). Obviously, we have no guarantee that these functions describe the real relationship between the instrument configuration and the mechanical parameters, but they allows to implement a more realistic set that takes into account the mechanical plays.

Consideration 2 may be referred also to this case with 6 DOF3 .T h e r e f o r e ,3 PCs does not seem to be sufficient to have a good estimation. Thisi sc o n fi r m e d also by simulation results (cf. table 6.5), where six or even better seven PCs (cases 31 and 32) already give an acceptable result even though augmenting the clustering and training domain dimensionality (i.e. using more visual information) seems to increase also the estimation accuracy.

However, the achieved precision is not satisfying considering that no noise has been added yet to the data. The possible reasons could be that the shape of the kernel is not correctly computed (maybe due to an imperfect clustering) or that the chosen kernel are not suitable to approximate the function. These two problems are now addressed and possible solutions are proposed. considering the first 3 input PCs and the whole output (q cl =3(+3))orthefirst5 input PCs and the output variable that is wanted to be estimated (q cl =5(+1)).

These strategies seem to slightly benefit the average accuracy above all in Z (wrt to case 31) supporting the hypothesis that such clustering domains help gathering together those data whose output are better represented by a Gaussian.

Clustering the data

To improve the computation of the Gaussian shap es, consideration 1 has been taken into account and, consequently, decided to carry out the clustering algorithm in the input-output joint space obtained as described in Sec. 6.2. The adoption of this alternative clustering space seems to bring only small improvements in terms of 3D estimation accuracy (cf. tab. 6.6). In fact, the clustering and training domain dimension being equal (case 40, 41a n d4 2h a v ea l l q cl = q tr =6 ), the 3D RMS error and even the worst 3D err are comparable. Nevertheless, the result over Z seems to improve when considering the output (either the 3 coordinates or the specific coordinate), confirming somehow the adequacy of such clustering domain.

This leads to the following consideration Consideration 3. It seems that the output carries some information that can substitute (or even improve) the input principal components.

This fact can be exploited in those cases where the confidence on the output of the training set is higher than the confidence on the input samples (the image features in this case). One can think to substitute the input information with the output without any substantial loss of accuracy in the estimation.

Consideration 1 does not only suggest a clustering strategy but, also, that the RBF Gaussian kernel can be inadequate to describe the 2D-to-3D Z-coordinate relationship. In fact, considering again the case of the instrument rotating with a fix deflection, the function describing the relationship between the centroids and the Z 3D coordinate would appear as an hyperplan in the 2n +1 dimensional domain.

That is why another approach has been investigated where another kernel such as hyperplanes is used instead of Gaussians. Directly substituting the Gaussian kernels by planes would result in a piecewise linear functionw h i c hp r e s e n t ss t r o n g discontinuities in correspondence of the inter-clusters frontiers.

Another approach, then, could be investigated. We propose tou s eL o c a l l y Weighted Regression which are describ ed in the next section.

Locally Weighted Regression (LWR) Method

To understand how LWR works, a step backward is necessary to introduce linear regression. Assuming, without loss of generality, that the input x and output y i are zero-mean, the model function in linear regression is:

y i = γ T x + ε y (6.11)
where x is the q tr -dimensional input vector, y i is the output value, γ are the regression coefficients and ε y is a noise variable independent of x.

Furthermore, let M be the number of training data points such that X tr = {(x m ,y m,i )} M m=1 .T h e c o e ffi c i e n t s γ can be obtained by minimizing the squared error χ 2 :

χ 2 = 1 2 M m=1 ||y m,i -γ T x m || 2 (6.12)
which results in the conventional least square solution:

γ =(X T X) -1 X T y i
where the rows of X are the input vector of the training set (image centroids coordinates) and y i the corresponding output values (one of the three space coordinate).

Locally Weighted Regression is funded on the principle that each data of the training set should contribute to the computation of the linear regression proportionally to its distance from the point where the regression has to be computed (also called query point ) [START_REF] Atkeson | [END_REF].

Even though all the data of the training set are used to computesuc hregression, the term "locally" stays for describing the fact that each term in (6.12)i sw e i g h t e d according to the distance from the query point x q .T h e r e f o r e ,t h en e wc o s tf u n c t i o n has the form: 6.13) and the weights can be expressed as

χ 2 = 1 2 M m=1 w m (x q ) ||y m,i -γ T x m || 2 (
w m (x q )=exp - 1 2 (x m -x q ) T D(x m -x q ) (6.14)
where D is a positive semi-definite matrix computing a distance metric that determines the region of influence of the so-defined local model (butt h a ts t i l ld e pe n do n all the x m ).

In this case, the solution for the regression coefficients is:

γ =(X T WX) -1 X T Wy (6.15)
where W is a diagonal matrix containing the w m on its diagonal.

Recalling the one-dimenional example used for RBF, in the caseo fL W R ,w h e n the estimation for a new sample has to be computed (red line in Fig. 6.6(a)), a weight is assigned to all the data of the training set according to (6.14)( r e s u l t i n g for example in the distance metrics of Fig. 6.6(b))soastofindthe"local"v ersionof the linear regression coefficients (cf. eq. ( 6.15)) obtaining the local approximation (green line of Fig. 6.6(a)). This kind of learning method is also named Memory-Based learning since it needs the entire training set to estimate a new sample.

The two main problems of such approaches lie

• in the inversion of the covariance matrix eq. ( 6.15), which cannot handle redundant information,

• and the high computational cost due to the weights computation, which has to be done for all the data of the training set.

To solve the describ ed problems, the dimensionality of the training domain can be reduced with PCA (to avoid the use of redundant informationw h i c hm a yb r i n g to singularities) and the weights computation may be limitedt ot h efi r s tj nearest neighbors of the query points to lighten the whole process.

Taking into account considerations 1 and 3,o u rp r o po s a li st ot a k ei n t oa c c o u n t the output in computing the nearest neighbors. Since no information on the output is furnished for the query point, no input-output joint distance can be computed for deriving the weights. A way to consider the output is to pre-cluster the training data in the input-output joint space as described in sec. 6.2.

In the training space (defined as (6.10)), a distance metric can be defined and, consequently, the distance from the query point (expressed wrt to its principal components) to the clusters centers can be computed. The 2 nearest clusters are selected and the weights are computed only for the sample composing such clusters. The objective of this pre-clustering is to group the data according also to the output values and transfer, somehow, this output influence ind e fi n i n gt h eq u e r y point neighborhood where no output information is available.

However, consideration 1 was more related with the Gaussian shape and the necessity of clustering together smooth data which should bebe t t e rr e p r e s e n t e db y the local Gaussian models. The choice of using planes for the regression has its origin in trying to deal with cases similar to consideration 1 and more specifically the case of centroids-to-Z 3D coordinate relationship. If this is the case, the effect of considering the output would not be as much important as for the RBF Network. Output consideration, though, can be useful in the perspective of consideration 3 to "substitute" noisy data with the possibly more trustful output data.

Simulation Results

As before, the LWR-based method is tested in simulation and actually the achieved accuracy is better than that achieved with RBF Network. The same tendency as before reappears here showing that the higher the dimensionality the better the performance is (see Tab. 6.7 and Fig. 6.7).

The results here seems to be in contradiction with the previous assertion about the need of dimensionality reduction. However, in case of higher redundancy (using 3D O F sa n dat r a i n i n ga n dc l u s t e r i n gs e to fd i m e n s i o n1 0 )t h edescribed singularity problems arise more often.

We can observe, though, that considering output in the clustering domain does not improve the estimation precision as in the other case.

Counteracting the noise

The effect of noise deserves a different section since it changes the results observed so far above all for RBF and conclusions are less intuitive and difficult to generalize.

Afi r s tr e s u l ti so v e ra c c u r a c y . I n d e e d ,a so n em a ye x p e c t ,w h e nc o n s i d e r i n gi nput noise (which is unavoidable in image), the position estimation accuracy drops. Case trained on tested on q cl q tr 3D RMS Worst 3D err Moreover, with the introduction of noise, augmenting the dimension does not coincide any more with increasing the estimation precision, on the contrary it decreases when passing from 3 to 6 PCs to 10 (cf. cases 50, 51 and 52). This fact is appreciable above all looking at the worst error in Tab. 6.8.

In this simulation tests (where both image features and TCP 3Dp o s i t i o na r e computed using camera and kinematic models), a uniformly distributed random noise between 0 and 5 pixels was added to each feature point (both to train and test set input) whereas no noise were added to the TCP position.

To counteract the effect of noise we prop ose two strategies. The first one is to increase the trustworthiness of the clustering data by relying on more trustful data and the second one is to "learn" the effect of the noise.

Improved Clustering

For the first solution,c o n s i d e r a t i o n3 provides already a possible trustful information to use i.e. the output value. In fact, the output value is am e a s u r e m e n ti s s u e d from a stereoscopic system which usually furnish very trustful results (provided that the cameras intrinsic parameters are known). Therefore, using the same framework described beforehand, the clustering space would be composed by the first principal component of the input (image feature) vector and the output (3D coordinate) variables. Moreover, the fact of using PCA relegate the noisec o n t r i b u t i o nt ot h e last component allowing to eliminate a part of the noise by taking only the first components (cf. cases 50 and 52).

This correction improves the results for RBF when consideringa l lt h eo u t p u t values. On the other hand, the same case (q tr =3 ( + 3 ) )i ts e e m st oh a v ea l m o s t no effect for LWR, even though a slight improvement in the maximum error is noticeable (cf. cases 60, 61, Tab. 6.9 and Fig. 6.9(a)). This capability of noise reduction when using the output is confirmed also when comparing the results obtained using the output space as clustering with those obtained using the 3 input PCs (Fig. 6.9(b) and Tab. 6.10). Also in this case the effect is more appreciable for the RBF approach leading to the conclusion that planes may better represent the image-to-3D function being capableo ffi l t e r i n go u t l i e r s .

The results deriving from the consideration of the output alone imply suggest that the benefits go beyond the noise counteraction especially for RBF. It seems that the output information is better for clustering purposet h a nt h ei n f o r m a t i o n carried by input principal component. The simulation results show that the accuracy that can be achieved with a lower clustering and training dimensionality (case 70, Case trained on tested on q cl q tr 3D RMS Worst 3D err 60 5 6 5(+1) 6 RBF 1.0931 1.6832 3.3868 LW R 0 . 9 1 8 9 1 . 3 9 7 8 2 . 7 1 1 Figure 6.9: Sorted 3D error norms for the cases listed in table 6.9 in (a) and table 6.10 in (b).

Case trained on tested on q cl q tr 3D RMS Worst 3D err 8 6 8 5 1 . 4 2 1 0 2 . 4 4 1 6 6.7114 7.1330

Training Set Enrichment

The second proposal is a kind of training set enrichment.T h ei d e ai st o" t e a c h "t o the regression machine that the same output can be obtained with different input configurations due to noise effect. For this aim, the cardinality of the training set can be increased by adding for each original training sample an e ws a m p l ew i t ht h e same output value (as the original one) but with the input value corrupted by the expected noise e.g. the error in extracting the visual feature. This average error can be determined empirically analyzing a sequence of images. The benefit of such enrichment is evident comparing result in Tab. 6.11 with cases 50, 51 and 52. The positive effect is more evident for the RBF Network (cf. Fig. 6.10(a))ev enthougha (more slight) improvement can be noticed also for the LWR (Fig. 6.10(b) and ( c)).

The other objective of the training set enrichment is to adequately add samples in the case where the training set is not dense enough. At the beginning of the chapter and also in Sec. 6.1 has been underlined how local learning approaches need a representative and "dense" training set to avoid to drop to zero if no data are present. Cases 100 and 101 (Tab. 6.12)c o n fi r mt h i sa s p e c t . I nc a s e1 0 0 ,a n uniformly sampled subset of set 5 is used for training (subseto fc a r d i n a l i t y1 4 0 ) whereas the test is always done on test 6 (180 samples). In case1 0 1 ,d o m a i n enrichment is used to "complete" the training set finally obtaining a slightly better result (cf. table 6.12).

The simulation results show that the two proposed improvements increase the average accuracy of both RBF and LWR methods even when trainingsetisaffected by noise. However, the two methods do not response in the same way to the same action.

According to simulations where noise is taken into account, joint cluster domain brings a higher benefit on RBF than LWR-based method (cf cases 52a n d7 0 ) . T h i s may be explainable reminding consideration 1.I nf a c t ,L W Ri sa l r e a d ya d e q u a t e to fit (hyper-)planes to the data, whilst, adding output shapei n f o r m a t i o no b l i g e s , somehow, the Gaussian to take a flatter shape in the case of depth estimation.

These simulation experiments suggest three interesting cases to test with the data retrieved from the robotic experimental cell:

• q cl =3(+3),q tr =6 • q cl =6,q tr =6
• q cl =0(+3),q tr =3 always considering the training enrichment.

Experimental Results

As emerged from the preceding discussion, the needed data set for the RBF network training is composed of the image coordinates of the five markers centroids and the corresponding measurements of the 3D position of the tip of the instrument.

The method described in Ch. 4 can be directly applied for extracting the 5 centroids of the markers by using the nodes of the resulting directed path (without the need of border and corner detection). The GT of the 3D position is obtained using the same experimental setup as detailed in appendix B.

With the aim to compare model-based and learning-based methods the same set is used here to perform the experimental validation.

In this case, a 4-fold cross validation process is adopted foro b t a i n i n gt h eq u a ntitative results of the estimation error. The available dataa r ep a r t i t i o n e di n t o4 subsets whose components are randomly chosen. The train/test process is repeated 4t i m e s : e a c ht i m e75% of the data (three packages) are used for the training and the remaining 25% for performing the test. The RMS errors are then computed considering the errors over all the 4 testing packages (i.e. over the whole available data set).

In the precedent chapter the mechanical parameters were modeled as additional DOFs. However, provided that no contacts occur between instruments and environment, these are "passive" DOFs and, consequently, one can think that their values are associated to the particular configuration of the robot. The simulations were carried out along the line of this reasoning and, based on that, the best result parameters have been chosen for the experimental tests i.e. q cl =3(+3)and q tr =6.

With such choices, the result is poor in terms of RMS estimation error (computed over 237 values): 7 7 4 9 0 . 7 6 3 4 1 . 6 5 6 9 8.6384 5.3356 Table 6.13: RMS and maximal errors computed for the two different strategies (RBF and LW R -l i ke a p p r o a ch e s ) f o r a n i n -l a b o r a t o r y s e q u e n c e o f 2 3 7 c onfigurations with input domain enrichment. The best results for RBF are obtained considering the output in the clustering domain (q cl =0(+3)), whereas LWR is capable of approximating the function only with input information (q cl =6).

q cl q tr RMS
• for (Pre-Clustered) LWR: [0.815, 1.03, 2.4] ± [0.54, 0.73, 1.57] • for RBF: [1.02, 1.98, 4.15] ± [0.65, 1.28, 2.57] but an important improvement is noticed when the training sete n r i c h m e n ti sp e rformed by adding 3 (other values can be used as well) samples for each original sample. Doing so, the obtained RMS errors are:

• (Pre-Clustered) LWR: [0.7, 0.74, 1.74] ± [0.43, 0.49, 1.08] • RBF: [0.92, 1.2, 2.49] ± [0.61, 0.72, 1.49] As also explained before, the choice of the value of parameter K changes according to the cardinality of the training set. The results shown for the original training set are obtained with K =10and for the case of the enriched training K =18.

As announced in the simulation section, other experiments has been carried out which are summarized in table 6.13.T h e s el a s tr e s u l t sc o n fi r mt h ee x p e c t a t i o no ft h e simulation tests. Indeed, LWR is definitively better than RBF in approximating the 2D-to-3D function above all in terms of depth retrieving. Even if output information is not considered in the clustering domain (q cl =6 ), a good approximation of the Z coordinate is achieved thanks to LWR endorsing, somehow, thec o n s i d e r a t i o n1 concerning the particular almost planar shape of the 2D-to-Z-coordinate function.

Comparing the experimental results with those obtained in simulation, the tendency of the RMS error in the different cases (clustering domain, training set enrichment, ...) is almost maintained (Fig. 6.12)w h e r e a st h eR M Sv a l u ea r el o w e r for the experimental RMS. This difference in value can be explained by the fact that, maybe, the hypothesis made over noise magnitude (both for input or output measurement) or over the modeling of the mechanical DOFs are only partially true and maybe exaggerate wrt to reality. Comme montré en figure 6.12,c e sd e u xm o d i fi c a t i o n sa p p o r t e n td e sb é n é fi c e s aux deux approches en améliorant la précision de l'estimation de la position 3D du bout de l'instrument.

3D Err Norm [mm] configurations

Comparison with Model-Based Method

Comparing the root mean square errors of the model-based and learning-based methods suggests that the latter approach seems to be more capableo fc a p t u r i n gt h e intrinsic nature of the image-to-TCP relationship. The samet e n d e n c yi sc o n fi r m e d by the maximum error which is almost 3 mm worse in the case of model based (Fig. 6.13(a)). In terms of maximum error, though, both approaches are still far from the possibility of being used in the envisaged biomedical applications.

Differently of what observed with the model based approach, here, there is not ac l e a rp a t t e r ni nt h ed i s t r i b u t i o no ft h eh i g h e s t3 De r r o r si nt h e3 Ds p a c e . T h e largest error are on isolated points and are usually derived from the fact that no dense information is available in that specific portion of thes p a c e .

The substantial advantage of learning approach is that it does not require to model all the different relationships of the considered system. This allows to reduce the errors deriving from incomplete models, which often result to be incorrect due to uncertainty or identification issues. This can also avoid camera calibration step and camera distortion estimation since these procedures arei n h e r e n t l yc o n s i d e r e d in the learning process. In fact, the coordinates of the features extracted in the original images and used during the training do not need any rectification. The 2D-to-3D function, then, is retrieved directly with the distorted information. This does not imply any problem as far as the image distortion is thes a m ef o ra l lt h e sequences where the trained regression is wanted to be applied. Furthermore, the needed features (markers centroids) are easier (and faster)toextractwrttomark ers corners.

On the other hand, learning-based approaches are strictly dependent on the ground-truth they have been trained on: the GT must be trustful and complete to achieve satisfactory precision and its obtaining is oftenl a b o r i o u sa n dt i m edemanding. If, during test, a "not seen" sample shows up the trained machine cannot give any trustful estimation. This is also confirmed byt h ef a c tt h a tt h e largest errors are on the frontier of the training set i.e. where the information is less dense (cf. Fig. 6.13(b)). In this aspect, a model-based approach beats the learningbased because it does not need to see any specimen and, nevertheless, thanks to model hypotheses, can infer pose information even if the information is only partially available. Furthermore, it shows a more adaptabilityw i t hd i ff e r e n ts y s t e m s (in Ch. 5 the same model worked for experimental cell and manual flexible system used in-vivo), whereas in the learning based method another training set should be created. Considering a scenario where the instruments interact with the environment (tissues, organs, ...) a model-based approach would allow tostillin terpretthescene, with the only necessity of upgrading the model accordingly tot h ed e f o r m a t i o nt h a t have to be taken into account (e.g. considering more DOFs for being able to estimate non-constant curvature configurations or additional bending sections). From the learning point of view, a complex training action should be performed if all the possible deformations have to be taken into account.

Since the model-based approach estimates the best joint configuration, it allows to estimate the entire pose of the instrument which could be useful for motion planning purposes or if the TCP orientation has to be controlled. This is an important aspect in surgery, since cutting or sewing action generally requires a particular tool orientation to perform the most adequate gesture and, then, attain the optimal result. The approach adopted here in the learning-based solution only allows to have the TCP position estimation. It could be thought to extend thep r o p o s e ds o l ution considering orientation data. However, a precise orientation GT seems difficult to obtain. With respect to this aspect several attempts were made with electromagnetic sensors but their reliability is limited and even drops when interact with metallic parts (which are difficult to avoid in endoscopic surgical contexts).

Chapter 7

Conclusion and Future Work

Conclusion

The control of continuum robots is a challenging task and it becomes even more challenging in the context of surgical operations carried out with flexible miniaturized instruments. Without accounting for the difficulties linked to in-vivo operations, the complex models needed to describe their dynamics lead to the necessity of external measurements of the instrument pose to close the feedback position control loop. With this final aim, we have proposed and compared in this manuscript, two vision based methods for measuring the 3D position of flexible instruments.

The problem has been tackled in all its stages: from camera andfl e x i b l ee n d oscopic system modeling, through the selection and segmentation of suitable visual features and to the inferring of 3D information from such 2D features.

Chapter 3

The whole kinematic chain from the camera till the TCP of the instrument has been identified and expressed in the camera reference frame. Based on this formalization, at h e o r e t i c a ls t u d yh a sb e e nc a r r i e do u to v e rd i ff e r e n td e s c r iptors to find the most suitable (among them) to infer 3D information from monocularv i s i o n . I ns p i t eo f the fact that the described study has only a local validity, ita l l o w e dt oq u a n t i f y the sensitivity of certain features and, consequently, maket h ed e c i s i o no fu s i n gt h e corners of some colored markers located on the body of the bendable section of the surgical instrument.

Chapter 4

Although these features are always visible whatever is the orientation of the instruments, they resulted to be difficult to automatically segment in in-vivo environments. We proposed, then, a novel segmentation algorithm that is easily adaptable to any marked bendable instrument with few a-priori information on the object. It showed its robustness to strong specularities present in in-vivo environment and false-positive reflection of the instrument on glossy surfaces contiguous to the instrument. The core of this algorithm is composed by two stages. The first is a graph-based method which allows defining the relationship between regions of interest for selecting solely the true positive and combining them according to some visual criteria. In our case, topological and shape criteriah a v eb e e nu s e d ,b u tt h e same formalization can be used for other criteria. Thanks to this method, the partial markers regions can be recomposed till obtaining the entire instrument body in the image. The second stage takes advantage of the skeleton of the segmented instrument (defined as the piecewise linear curve connecting the marker centroids) to search its upper and lower borders points in the proper directions. Our proposal of using M-estimator to fit Bézier curves to such points allowed a two-fold action: to filter border outliers out and to get a continuous representation oft h eb o r d e rt oa c h i e v e sub-pixel precision in corner extraction. Moreover, it alsoh e l pt or e c o v e rp a r t i a l l y missing information. The method, though, is not robust to external or self occlusions, yet.

One model-based and one model-free approaches have been advanced in this report to tackle the 3D pose estimation starting from 2D information and some a-priori information either represented by model knowledgeo rt r a i n i n gs e t .

Chapter 5 and 6

The model-based approach improves the state-of-the-art solution, by considering an extended kinematic model with additional "spurious" DOF.Thisfactorallo wsto model the mechanical plays that the considered system may have and, consequently, enhance the adaptability of such method, providing good results even when the geometrical model (camera-to-instrument-channel chain) changes during the surgical operation.

It has been also shown how, in a robotic context, motor encoderinformationcan be exploited (within a Kalman filter framework) to strengtheno rc o m p l e t ev i s u a l information throughout an image sequence.

On one hand this method provides the complete pose information, but, on the other hand, it showed accuracy limits in terms of RMS TCP position estimation errors, whose main cause seemed to be due to model uncertainties (for example a non constant curvature).

To overcome mo del uncertainties, a learning based algorithmhasbeendev eloped to approximate the image-to-TCP-position relationship. Given the difficulty of obtaining an a-priori information on the nature of such relationship, local regression approaches have been preferred over regression methods based on global costs, since they can easily fit local variability without affecting the outline of the function in the whole domain. Two methods were conceived inspired by Radial Basis Function (RBF) and Locally Weighted Regression (LWR). The first one approximates the function with a linear combination of Gaussians fitted on pre-clustered data. The second one fits a plane on the training data weighted accordingt ot h ed i s t a n c ef r o m the query point. Differently from conventional LWR the neighborhood is composed by those samples pertaining to the two nearest clusters of theq u e r yp o i n t s .

To mitigate the known issues of lo cal approaches when dealingwithhigh-dimensional domains, the choice of performing the clustering (needed by both methods) in the space defined by the first k principal components of the input-output joint space showed to be effective. The proposed training set enrichment showed an important effect on counteracting the noise.

Comparing the RMS error results, the learning approaches arem o r ea c c u r a t e (in average). Moreover, since the regression is based solelyo ni n p u t / o u t p u td a t a , easier-to-extract (but still coherent) visual features, such as the centroids, can be chosen wrt the model-based approach. However, the difficulty here lies in creating ar e p r e s e n t a t i v ea n dd e n s et r a i n i n gs e t ,w h i c hc a nbel a bo r i ous.

The choice of one method over the other, though, still dependso nt h em e a n t application. In endoscopic surgery, for example, the orientation of the surgical tool (TCP) is as important as the position to ensure the right gesture and perform the operation faster. In other cases where the trajectories of the tool need to be controlled or its workspace to be limited, only the position is sufficient and the learning method proposed here can be utilized as it is.

Both model-based (thanks to the modular way in which the Jacobian is written) and learning based (for its intrinsic independence from the model) can be adapted to other camera-to-instrument configurations, e.g. conventional endoscopy where as i n g l ew o r k i n gc h a n n e li sp r o v i d e du s u a l l yl o c a t e du n d e r n eath the camera or to flexible unarticulated instruments by fixing the parameter θ.

Future Work

Alistofcomplemen taryw orkscanbedra wnup,startingfromthe current identified limits of the presented solution.

Feature Segmentation

The algorithm for extracting visual information, for example, can be made more robust to occlusion by introducing a tracking algorithm. Them o r ed i r e c ts o l u t i o n would be to consider the model of the instrument itself and useaKalmanorparticle filter [START_REF] Isard | [END_REF]]t op r e d i c tt h ep o s i t i o no ft h ef e a t u r e si nt h ei m a g e . I nt h i scase, of 3D tracking, a good 2D measurement or, alternatively, a good drift detection should be individuated. An example for such measurement can be the distance of the estimated outline to the actual apparent contour of thei n s t r u m e n tb o t h labeled according to which markers they pertain. This allowst oh a v es o m e3 D instrument related features which can be employed as harder constraints to possibly help limiting tracking drifting. Using a model, though, always presents the same limits related to the quality of the model and, consequently,c a nj e o p a r d i z et h e tracking accuracy. An alternative would be to consider the Bézier curves that can adapt at any shape even though loose the 3D meaning. Another approach could be to use 2D feature tracking (Bézier or others) for a preliminary raw estimation that can be refined with dense information (such as Mutual Information). However, in this case, a precise model of instrument appearance shouldb ek n o w n . I na n y case, as a first approximation, a constant velocity model can be considered or, in alternative, take the motor encoder data increment to compute the prediction.

Regarding the segmentation, it would be worth keeping on working on the formalization of the graph-based method described in Sec. 4.S u c hs c h e m ec o u l db e inscribed in a pictorial structure framework (or the more general deformable part model)whichispresentlyused,forexample,todetectpersonskeleton [Felzenszwalb 2005]. Firstly, the image is processed with a specific detector for each body part and, subsequently, a global cost function is used to combine the "candidate" body parts in one person skeleton. The difference with our problem is the difficulty of having characteristic features for each marker (for example, the first and last markers are not distinguishable between them, except for their relative position wrt the instrument) and the fact that the structure of the skeleton may changes (inf a c t ,s a t u r a t i o n does not always affect the instrument and, therefore, the instrument body can be composed by 5 or more elements). Therefore, two aspects should be enriched: a suitable characteristic to train the markers detectors (or put a particular color for each marker) and a global cost function allowing the right ordering and (re-)composition of the candidate regions.

Model-Based Approach

The considerable error resulting from the model-based approach can be mainly imputed to the imperfect modelling of the instrument. As futurew o r k ,t h ep o s s i b l e improvement deriving from the employment of a more complete model should be verified. The weakest hypothesis seems to be the one regarding thec o n s t a n tc u r v a t u r e of the instrument in a plane. One solution would be considering all the vertebrae as independent joints, but this would dramatically increaset h en u m b e ro fp a r a m eters over which perform the optimization. An alternative is to study, thanks to the stereoscopic system used for Ground Truth, the specific curvature of the instrument so as to describe it with a limited number of parameters.

Another improvement would be considering, in addition to corners points, the entire apparent contour of the instrument or the computed Bézier curve to reinforce the visual information and help convergence to the right solution following the works in [START_REF] Colombo | [END_REF]]. Also a more clever image-motor data-fusion could be foresee.A ne x a m p l ec o u l d be, at least, detecting by vision the histeresis characterizing the deflection: avoiding state update when no motion on the image is perceived even though a motor increment is detected.

Learning-Based Approach

On the other hand, the learning approach lacks of the orientation of the TCP. Given its acceptable precision on 3D position estimation, the result of the learning-based method could be used, in turn, as a new constraint for the optimization process in the model-based method. This should improve the local-minima avoidance in addition to provide an idea of the orientation of the tool.

One can also imagine to learn solely the shape of the instrument, separating it from the translational (and scale) component of the base oft h eb e n d a b l ep a r t . The idea would be to learn the relationship between the image features and the two parameters (φ and θ)r e s p o n s i b l ef o rm o d i f y i n gt h ea s p e c ta n do r i e n t a t i o n( i n the image) of the bendable part. In this way, the shape can be learned even on synthetic data and, consequently, it could be easier to insert in the training set some non-conventional poses that can be assumed in case of interaction with the environment. Bézier curves parameters can be a good input vector for such new learning approach, since the control points inherently contain the apparent shape and orientation information.

In this learning based method there is the space of another improvement related to the way the domain dimensionality is reduced. Indeed, PCA is not the best way to choose efficient projections: components with high dispersion will be considered as principal component even though they have no effect on the output. One alternative could be PLS (Partial Least Square) which recursively performs single variable regression along input projection and residual errors of thep r e v i o u ss t e p . T h ek e y ingredient of PLS is on the choice of such projection direction, which coincides with the direction of maximal correlation between the residual errors and the input data. In the same perspective used in our work of considering outputd a t af o rc l u s t e ring, PLS assures that the main input projections are those that have the greatest influence on the output.

Other clustering initializations can also be studied maybe based on the output outline (or variation), for example, locating the initial centers according to the gradient of the training samples output. Alternatively, an incremental clustering can be adopted to avoid agglomerating initial guesses in one region of the space with the risk, though, to create clusters with almost no data.

Simulation Experiments

To strengthen the conclusions derived from results and simulations, further tests and theoretical studies can be envisaged. For example, otherf e a t u r e so rd e s c r i p t o r s can be considered in the sensitivity preliminary study or additional tests with other state-of-the-art learning algorithm (e.g. Random Forest) could be performed to see the actual potentiality of the proposed solution. From the segmentation point of view, the strengths and weaknesses of graph-based segmentation and Bézier fitting of the borders could be tested on additional video sequences to verify its robustness in cluttered scenes or in different in-vivo background. It will be interesting, as well, applying the framework developed here to other robotic or manual systems used in general endoscopy or, even, concentric tubes (single bending section concentric tube at a first instance) to test its inter-class applicability.

Instrument Control

Although, the real flowering of this work (already evoked in the title) would be the exploitation of pose estimation for controlling the flexibleinstrumentwithvisualservoing. The most direct implementation would be an Image Based Visual Servoing (IBVS), where the reference can be a picture of the instrument in a given configuration. At each time step a control action can be computed in as i m i l a rm a n n e r to the optimization process used in pose estimation and the resulting increment for the DOF values can be used as velocity reference for those motors actuating the 3 DOF of the flexible instrument.

Possible Further Application

Another really interesting application of such work would becon tactdetectionwith the environment or force sensing. If the force applied on the tip causes movements of the bendable section which can be attributed to the conventional movements (i.e. that can be explained with the kinematic model), the contact can be detected looking at the difference between the encoder incremental values and the increment of the DOF revealed from the image. More precisely, if the estimatedD O F se x p e r i e n c ea variation which is not reflected by encoders data, one can attribute such movements to external forces acting on the instrument.

As the reliability of deflection encoders is poor for retrieving the real instrument deflection, the data referring to this DOF would not be trustful enough to associate af o r c ee s t i m a t i o n . Nevertheless, in all the cases, the resolution and accuracy reachable with such technique is not clear ap r i o r iand probably would not be sufficient for high-precision applications, but would only give an idea on the force magnitude or increment. which gives (considering, without loss of generality, only the lower apparent point):

κ = -cos(κ)ẋ c -sin(κ)ẏ c y c cos(κ) -x c sin(κ) .
Noting that sec ω A/sec =(0, 0, κ) T ,a n dt h a t

cam V A A/sec = cam R sec sec V A A/sec = cam R sec ( sec V S A/sec + sec ω A/sec ∧ sec SA) = cam R sec ⎛ ⎜ ⎝ -κrsin(κ) κrcos(κ) 0 ⎞ ⎟ ⎠ one gets cam V A A/sec = cam R sec M ⎛ ⎜ ⎝ ẋc ẏc żc ⎞ ⎟ ⎠ with M = 1 y c cos(κ) -x c sin(κ) ⎛ ⎜ ⎝ rsin(κ)cos(κ) rsin(κ) 2 0 -rcos(κ) 2 -rsin(κ)cos(κ)0 00 0 ⎞ ⎟ ⎠ .
One also has (ẋ c , ẏc , żc ) T = sec V c cam/sec = -sec R cam cam V C sec/cam . We note cam V C sec/cam = J g_gen ṙ where J g_gen is the Jacobian relating the joint velocities to the velocity of point C,c o n s i d e r e da sap o i n ta t t a c h e dt oF sec .T h i s Jacobian is close to the one of eq. 3.24,a l t h o u g hi ti sd i ff e r e n tb e c a u s eC does not generally belong to the plane of the considered section.

Actually, the effect of λ, φ, ψ, ẋ ch , ẏ ch and μ are not modified by this fact. Only the effect of θ

t b S,C = ⎡ ⎢ ⎣ x c cosθ i + sin(θ i )z c y c -x c sinθ i + cos(θ i )z c ⎤ ⎥ ⎦ (A.3) t b b,C = ⎡ ⎢ ⎣ R curv (1 -cosθ i ) 0 R curv sinθ i ⎤ ⎥ ⎦ + ⎡ ⎢ ⎣ x c cosθ i + sin(θ i )z c y c -x c sinθ i + cos(θ i )z c ⎤ ⎥ ⎦ .
(A.4)

A θ = R c b ∂C b ∂θ with ∂C b ∂θ = ⎛ ⎜ ⎝ k θ L/θ sin θ i -C sec x k θ sin θ i + C sec z k θ cos θ i -L/θ 2 (1 -cos θ i ) 0 k θ L/θ cos θ i -C sec x k θ cos θ i -C sec z k θ sin θ i -L/θ 2 sin θ i ⎞ ⎟ ⎠ Finally cam V A A/cam = J A gapp ṙ with J A gapp = -cam R sec MJ C g_gen + J A g .
The 3D geometric Jacobian of the apparent point A can hence be obtained from the usual 3D Jacobian of the same point but considered as a physical point attached to the section, the generalized Jacobian expressed at the origin of the camera C (considering C as part of the instrument), from the rotation between the camera and the section the point A belongs to, and matrix M . M depends on the position of the apparent point on the section and the position of the camera with respect to the section.

When assuming tolerances on the mechanical parameters (x ch ,y ch ,ψ,µ), their variability effect on the image has to be taken into account in the optimization process of the model-based method. For that reason, the geometrical Jacobian (that is used to compute the extended image Jacobian) must include the relationship describing the 3D point velocity due to the mechanical parameters variations.

We just list here the blo cks A i related to the mechanical parameters which are employed in equation (5.8).

The variation of the channel position directly modify the position of any 3D point pertaining to the instrument obtaining:

A x ch = ∂t c c,P i ∂x ch = ∂t c c,b ∂x ch = ⎡ ⎢ ⎣ 1 0 0 ⎤ ⎥ ⎦ (C.1)
and

A y ch = ∂t c c,P i ∂y ch = ∂t c c,b ∂y ch = ⎡ ⎢ ⎣ 0 1 0 ⎤ ⎥ ⎦ . (C.2)
For ψ and µ the same principle used for φ is employed here, obtaining:

A ψ = ∂t c ch,P i ∂φ = -[(P i -t c c,ch ) × y c ch )]. (C.3)
and Ces marqueurs permettent la segmentation des primitives quelle que soit la rotation de l'instrument. Grâce à cette définition continue des bords apparents, une précision sub-pixelique de la position des coins peut être attendue.

A µ = ∂t c ch,P i ∂µ = -[(P i -t c c,ch ) × [cos(ψ)0 -sin(ψ)] T )]. (C.4) φ λ F ch F b x z C θ F TCP ψ µ z y F c x y y z x F ch F c Tip of STRAS. F C F ch φ λ θ ψ R curv θ i F sec i F tip

Interprétation de l'image

Le coeur de ce procédé est la deuxième étape, qui permet de recomposer le corps de l'instrument dans l'image, en assignant à chaque région candidate une étiquette représentant le marqueur auquel elle appartient.

Cette phase se base sur le concept qu'un objet peut être représenté comme un ensemble de parties connectées entre elles. Dans notre cas, la partie pliable de l'instrument peut être vue comme la séquence ordonnée des cinq marqueurs. Toutefois, en raison des fortes spécularités, le résultat de la segmentation basée couleur présente plus de régions candidates que de marqueurs (cf. stage 1 fig. 5).

Cette étape vise donc à résoudre ce problème en réassemblant les régions dans les parties originales de l'objet de sorte à obtenir l'objet complet. Elle est composéed ed e u xp h a s e s:l ac o n s t r u c t i o nd el ' a r b r ee t son traitement.

Pour mener à bien la première phase, il n'est pas suffisant de considérer une connectivité classique à 4 Cependant, l'objectif final est d'obtenir un chemin dirigé composé par les marqueurs de la partie pliable de l'instrument. Un critère de cohérence temporelle est utilisé pour traiter l'arbre et en extraire ce chemin.

En supposant que, d'une image à la suivante, la forme de l'instrument reste substantiellement la même, l'apparence de chaque marqueur peut être décrite par deux facteurs :

-u nfacteur de forme dépendant du moment de second ordre de la région (calculé selon ses axes principaux) : s (1) et s (2) ; supérieur. L'arbre peut être traité niveau par niveau (cf. Fig. 8) en sélectionnant la branche avec la probabilité la plus haute. Dans le cas d'un noeud artificiel, les facteurs deformeettopologiquesontcalculésenconsidérant une région résultant de la fusion des deux régions candidatesi m p l i q u é e s .S il ar é g i o na v e cl ap l u sh a u t e probabilité est celle associée à un noeud artificiel, les deux noeuds impliqués sont éliminés et remplacés par le noeud artificiel.

Si le niveau présente plus de deux régions, le même procédé peut être appliqué itérativement jusqu'au moment où aucune meilleure solution ne peut être trouvée.

Al afi nd ec ep r o c é d é ,l ' a r b r ee s tu n es é q u e n c ed e5c e n t r e s ,o r donnés de la base au bout de la section bequillable de l'instrument (deuxième étape en fig. 5).

Ce traitement définit une sorte de squelette de l'instrument,q u is e r te n s u i t eàd é t e c t e rl e sp o i n t sd e s bords supérieur et inférieur comme décrit auparavant. Ces points de bord sont ensuite approchés par des courbes de Bézier , puis les coins des marqueurs sont extraitse na n a l y s a n tl e sv a r i a t i o n sd ec o u l e u r sl el o n g de ces courbes.

Cette méthode a fourni de bons résultats, même en conditions in-vivo, lorsque l'instrument se déplace dans un espace libre et sans occultation. La méthode est robuste aux fortes spécularités et est capable de discriminer l'instrument de ses éventuels reflets sur les organes (Fig. 11(c)).

Comme cette méthode ne prend pas en compte de modèles de l'instrument, l'algorithme est peu robuste aux occlusions, ce qui pourrait être pris en considération dans des travaux futurs.

[mm] configurations

où γ k sont les coefficients appris qui décrivent la contribution du k-ème noyau à l'estimation de la sortie.

Dans le cas de noyaux gaussiens, ϕ k est défini comme [START_REF] Gheorghe | Introduction of the radial basis function (rbf) networks[END_REF][START_REF] Lendasse | A p p r o x i m a t i o nb yr a d i a lb a s i sf u n c t i o n networks -application to option pricing[END_REF] : selon sa distance à la nouvelle entrée (query point) x q .E na p p l i q u a n tc ec r i t è r e ,l af o n c t i o nd ec o û tq u ie n résulte est :

ϕ k (x)=Y k e -1 2 (x-C k ) T Σ -1 k (x-C k ) (8)
χ 2 = 1 2 M m=1 w m (x q ) ||y m,i -γ T x m || 2 (9)
où les poids peuvent être exprimés comme w m (x q )=exp -1 2 (x mx q ) T D(x mx q ) (10) où D est une matrice semi-définie positive, qui définit une métrique de distance determinant l'influence de chaque element de l'ensemble d'entraînement dans le calcul du model de regression.

Figure 2 . 1 :

 21 Figure 2.1: First flexible system for surgical intervention were a composition of existing tools which implied the participation of several surgeons during the operation (photo: IRCAD France).

Figure 2 . 2 :

 22 Figure 2.2: Tip of the R-Scope by Olympus. The two working channel has lifting gates that are orthogonally position wrt to each other allowing simultaneous lifting and dissection movements. The head also present two bending sections where the primary one is lockable (photo: http://www.wjgnet.com).

Figure 2 . 3 :

 23 Figure 2.3: EndoSAMURAI by Olympus endoscopic head with instruments andm a s t e r consoles (right). A locking overtube make it more stable thanD CEb u tal w a ys present the same image-perspective limitations since the camera is married with the instrument (photo: http://www.wjgnet.com).

Figure 2 . 5 :

 25 Figure 2.5: Incision Operating Platform by USGI Medical. It presents 4 lumens that can be used to house a scope in addition to specialized instruments or for carbon dioxide insufflation. The in-line channel orientation favorsp a r a l l e l i s m( p h o t o : http://www.wjgnet.com).

Figure 2 . 6 :

 26 Figure 2.6: Transdouglas Endoscopic Device in the S-shap e op erational configuration. It is 3r i g i dl i n k sd e v i c ew i t h4w o r k i n gc h a n n e l s . O n c ep o s i t i o n e d, the arms with the instruments are deployed as shown in (b). Moreover, the head of TED houses also a camera, a light and a dedicated channel for flushing and sucking (image: http://www.nesacademy.org).

Figure 2 . 7 :

 27 Figure 2.7: Anubis Scope by Karl Storz. Wider than the conventional endoscope, its bodyhouses two main channels (one at each side of the camera) wheres u r g i c a li nstrument can be inserted and an additional channel beneath the camera. The triangulation is assured by the slight deviation of the main channels wrt the camera axis. Thanks to the knobs on the main shaft handle showni n( a ) ,t h e head of the endoscope can be oriented. In (a) also the instrument handles are shown with the integrated lever that allows to bend the tip of the instruments.

Figure 2 . 8 :

 28 Figure 2.8: Example of robots for performing laparoscopic surgery. The open-architecture robot Raven II in (a) (photo:www.washington.edu) and the two European robots: ALF-X (photo:alf-x.com) by NESA and SOFAR S.p.A in (b) and MIRO by DLR -Germany in (c) (photo:tilo-wuesthoff.de).

Figure 2 . 9 :

 29 Figure 2.9: Example of robots for single port surgery. The Sp system by DaVinci in (a) (photo: medgadget) and SPORT by Titan Medical in (b) (photo: twitter.com/simnovate) which derives from IREP project by Columbia University (c) (photo: linazeldovich.com).

Figure 2 . 10 :

 210 Figure 2.10: In (a) another example of robot for single port surgery (ARAKNES SPRINT, photo: sssa.bioroboticsinstitute.it). In (b),t h ec o n c e p to fA R E Sp r o j e c ti s shown: a miniature-modular robot able to reassemble once gott ot h ed e s i r e d site inside the human body (photo: Scuola Superiore Sant'Anna).

Figure 2 .

 2 Figure 2.11: Scheme and photo of the tip of MASTER system. A gripper and a hook are located beneath the endoscopic camera and telemanipulated (photo from [Phee 2009]).

Figure 2 .

 2 Figure 2.12: STRAS system global view (a) and tip (b). It is the robotic version of theAnubis scope where all its DOFs can be tele-manipulated.

Figure 2 .

 2 Figure 2.13: TeleFLEX system by the University of Twente. The (green) steering modules are attached to the conventional flexible system (e.g. Anubis) (photo: demcon.nl).

Figure 2 .

 2 Figure 2.14: The FLEX system in (a) and (b) is composed by an endoscope and two working channels where specific instruments can be inserted (photos: medrobotics.com). The scope is self supporting and take inspiration from the HARP system in (c) (photo:carnegie mellon.)

  Figure 2.15: NES system used in colonoscopy (a) (photo: intechopen). A follow-the-leader strategy make the endoscope follow the path described by the tip (b) which is controlled by the physician (photo: nature.com).

Figure 2 .

 2 Figure 2.16: Sensei X system and associated catheter. The force sensor integrated in the tip allows to maintain the tip in contact with the tissue increasing the efficacy of the therapy (photo: Hansen Medical).

Figure 2 .

 2 Figure2.17: The Amigo System is a support where commercial catheter can bepositioned.The particular handle in (b) is employed to control the movements of the catheter (photo: catheter robotics).

Figure 2 .

 2 Figure 2.18: Examples of possible configuration of the bio-inspired manipulator: actuation of the middle and tip sections (a), the base and middle sections (b), the tip section (c), all sections (d), and elongation/shrinkage capability (e)-(h) (photo from [?]).

Figure 2 .

 2 Figure 2.19: STIFF FLOP prototype. The robot can extend and compress and cable actuation allows to determine the shape of the two flexible sections(photo: National Museums Scotland -https://vimeo.com/97913172).

  t op e r f o r mc o n f o c a le n d o m i c r o s c o p y . T h r e eb a l l o o n sc a t h e ter, positioned with 120 deg between them, were used to orient the endomicroscopic laser probe in the space.

Figure 2 .

 2 Figure 2.20: Mechanics of the bendable section of the instrument. A sequence of cilindrical vertebrae are linked together to form the backbone whose shape can be changed by two antagonist cables (in red) attached to the end of the last vertebra

Figure 2 .

 2 Figure 2.21: Overview of the system with the motorization of the different modules. Each color represents one module: endoscope, instruments, wholes y s t e m( m a i n endoscope + instruments) and cart. With the same colors are marked the movement that each motor of each module confer to the corresponding part of the system. Excluding the cart inclination and vertical translation (which are controlled by a wire remote-control), all the other DoF can bet e l e -o p e r a t e d and controlled by a master interface.

Figure 2 .

 2 Figure 2.22: Motorization scheme of STRAS.

Figure 2 .

 2 Figure 2.23: Example of in-vivo image grabbed by the endoscopic camera in the abdomen of a porcine model. In this example, the main characteristicso fs u c hk i n d of images are visible: distortion, strong central enlightment and consequent specularity effects in the middle of the instrument and mirrore ff e c to nt h e shiny surface of the organs.

Figure 2 . 1 7 3. 4 . 1 7 3. 4 . 2 8 3. 4 . 3 9 3. 5

 2174174284395 Figure 2.24: Scheme of the relationships between instrument pose and image projection. Number one indicates the kinematic model wrt the camera relating a point on the body of the instrument or the TCP 3D position to the worldr e f e r e n c e frame (i.e. the camera origin). Number two: with the camera model the projection of the 3D point of the image is computed. Finally (number three) the image Jacobian relates the 3D point velocity to the 2D image variation. Composing 1 and 2, an extended image Jacobian can be computed relating the robot DOFs velocity with the apparent velocity of the corresponding features in the image.

Figure 3 . 1 :

 31 Figure 3.1: In (a) the picture of the tip of the endoscope with the instruments and in (b) the corresponding scheme. For sake of easiness and completeness in the representation, the scheme shows the instrument in the particular configuration with φ =0,t h i si m p l i e st h a tt h eb e n d i n gp l a n eo ft h ei n s t r u m e n ti sp a r allel to the one of the (x, z) camera. In (b) it can be seen that the estimated position of the end of the instrument duct does not coincide with its actual end, in fact it has been chosen to consider the end of the duct on the same plane (x, y) of the camera, to eliminate a sort of redundancy between λ and z-coordinate of the channel.

Figure 3 . 2 :

 32 Figure 3.2: Scheme showing the geometrical relations between the different elements in a section.

Figure 3 . 3 :

 33 Figure 3.3: Graphic of the numerically computed relationship between the deflection angle θ and the distance of the TCP wrt to instrument rotation axis z b .

Figure 3 . 4 :

 34 Figure 3.4: Scheme of the adopted pinhole model and image formation. For easing the representation of the image formation the plane is considered positioned at +f with respect the optical center reference coordinate frame.

Figure 3 . 5 :

 35 Figure 3.5: Scheme for the computation of the Distortion Gain. The plane Π represents the image plane as if no distortion ocurred whereas in image plane Π ′ the image formation is done taking into account radial and tangential distortions. The Distortion Gain is computed as the relative image displacement seen on the two image planes.

Figure 3 . 6 :

 36 Figure 3.6: Result of the variance propagation in the representative plane parallel to (x, z) of F C and passing through the origin of F ch .T h ec o m m o ne x p e c t e dt e n d e n c y is a variance growing with the distance to the camera. The depicted blue line is representing the direction of the housing channel.

  corners), weight on tip 10 5 mark (corners), weight on tip 2 5 mark (corners), weight on tip 1

Figure 3 . 7 :

 37 Figure 3.7: For each rob ot configuration the direction with the largest variance in the tip position estimation are taken and sorted. The result for different chosen descriptors is shown in (a).I n(b),az o o mo nt h ew o r s tc a s e si sd o n e : t h eb e s t solution seems to be the one considering the corners of some markers (5 markers of equal dimensions in this case).

Figure 3 . 8 :

 38 Figure 3.8: Anubis tip with the camera, the flexible surgical instrumentsw i t ht h ev i s u a l markers.

  4.2): along axis a are represented the red -green colors and along b blue -yellow colors.This color space provides a pure chrominance representationa n d ,o nt h e o r y , carries all the needed color information on the same coordinate b.M o r e o v e r ,b l u ea n d yellow are represented on the extrema of this channel and therefore they should be easily distinguishable using this representation making this color space very suitable

Figure 4 . 1 :

 41 Figure 4.1: The distribution of yellow, blue and background colors are represented in the RGB (8-bit color representation) space for two common scenarios within the abdominal cavity: intestine (a) and liver (b).

Figure 4 . 2 :

 42 Figure 4.2: Chromatic representation for L*a*b space. Along axis a are represented the red -green colors and along b blue -yellow opponent colors.

Figure 4 . 3 :

 43 Figure 4.3: Color distribution in L * a * b space. The yellow and blue values are depicted by the same colors whereas black is used for the background. The two ellipses represents the iso-contours of the Gaussian distributions corresponding to a confidence of 90%.

Figure 4 . 4 :

 44 Figure 4.4: Tree building with the sp ecified connectivity. In case 1 the branch is not created since it does not fulfill condition 1, whereas cases 2 and 3 doesn o tr e s p e c t condition 2 since the orientation of the possible branch strongly differs from the preceding outline. This prevents connecting outliers (case 2) and forming internal loop which correspond to physically unfeasible configurations (case 3).

Figure 4 . 5 :

 45 Figure 4.5: The appearance of each marker can be represented by two factors: one shape factor and a topological factor. The first is defined as the length and width of each marker according to its principal axis (left) and the second as the distance from the considered marker and its parent (right).

Figure 4 . 7 :

 47 Figure 4.7: Steps of the feature extraction. (1) GM are used to select bluea n dy e l l o w regions.(2) Creation of the tree using the specified connectivity: Branch 2 is not created since it does not fulfill condition 1, whereas branch 1 does not respect condition 2 because the orientation of this branch strongly differs from the preceding outline. The tree is finally processed to get a directed path. (3)The skeleton is used for searching border points. These are fitted with Bézier curves along which color discontinuities are searched and considered as corners (4).

  entering direction

Figure 4 . 8 :

 48 Figure 4.8: The green line represent the estimated instrument entering direction. The first yellow marker encountered walking along this line is considered to be the root (i.e. the first marker) of the tree.

Figure 4 . 9 :

 49 Figure 4.9: In blue the loss function known as Beaton and Tukey and in red the corresponding weight function.

  Fig. 4.10(c))a r eu s e dt ol i m i tt h er e g i o nw h e r et o search the color transition points (Fig. 4.10(d)). Finally, the outline of the absolute value of the b signal along the border (blue line in Fig. 4.10(d))i sr e t r i e v e d( F i g . 4.10(e))a n dt h eg l o b a lm i n i m u mi sc o m p u t e d( w h i c hi se q u i v a l e n tt othe zero-cross point) leading to the final result in Fig. 4.10(f).

Figure 4 . 10 :

 410 Figure 4.10: Detection of transition points for a particular region in (a).I n(b) the absolute value of the b channel of this region is shown. The space between two consecutive segmented markers (shown in (a))i su s e dt ol i m i tt h es e a r c h i n g zone of the corner as in (d).T h ec h r o m i n a n c ev a l u e so nt h em a r k e dp r o fi l e are shown in (e) where the global minimum is taken as the limit between the two markers obtaining the result in (f).

Figure 4 . 11 :

 411 Figure 4.11: Detection of transition points for a specfic region when a moree xt e rn alu p pe r boarder is detected. In (b) other Bézier curves are computed interpolating the upper and lower borders and the blue/yellow profile along themarecom p u t ed in (c).T h ei n t e r s e c t i o n sb e t w e e ny e l l o wa n db l u ep r o fi l ea r ec o m p u ted over these lines. An average direction is computed (green line in (d))a n dt h e intersection of this straight line with the outer border is considered to be the corner location.

Figure 4 .

 4 Figure 4.12: Bézier curves efficiency in presence of outliers or non dense information. In (a) arandomerrorw asaddedto60% of the detected border points (showed in red). In (b) only 40% of the points (with randomly added noise) are considered in the Bézier curve fitting. The green line is the optimal curvea n dt h eg r e e n rings are the corresponding control points. In (c) the IRLS and conventional LMS are compared when strong outliers are present in the border detection.

Figure 4 .

 4 Figure 4.13: Bézier curve seems to be more sensitive to noise if all the initial and ending points of the apparent border are strongly misplaced (a).Ah i g h e rd e n s i t y (even though with noisy points) is requested to mitigate thise ff e c ta n dh a v e ag o o dr e p r e s e n t a t i o n(b) .

Figure 4 .

 4 Figure 4.14: In-vivo results of the presented method. The green dot is the reprojection of the center of the grasper. In (a),despitethestrongspecularitiestheinstrument structure is recovered. In (c) and (d),t h a n k st ot h el o c a lb o r d e re x t r a c t i o n and the Beaton Tukey loss function the outliers are eliminated and the real instrument is correctly discriminated from its reflection ont h eo r g a n . I n(e) and (f) the segmentation process partly fails for the lower border because the instrument is hidden.

Contents 5 . 1 Fixed 4 5. 2 8 5. 2 . 2 6 5. 5 9 5. 5 . 1 4 5. 6

 51428226595146 Mechanical Model Parameters .............. 9 Variable mechanical parameters ................. 9 6 5.2.1 Managing the mechanical parameters ............. 9 Expressing the error wrt the Bézier curve ........... 1 0 0 5.3 Simulation Study .......................... 1 0 1 5.4 Motor Sensor Data Fusion .................... 1 0 Experimental Results ....................... 1 0 In-vivo Qualitative Results ................... 1 1 Conclusion ............................. 1 1 5

Figure 5 . 1 :

 51 Figure 5.1: Reprojected instrument borders (green spots) after the optimization process.The reference image features are the markers apparent corners (red spots). On the left the result considering the theoretical CAD parameters. On the right the solution with some tolerances on the mechanical parameters.

Figure 5 . 2 :

 52 Figure 5.2: Scheme showing the reference frame used to express the visuale r r o ro nt h e marker corners. s ∥ and s ⊥ are computed analytically based on the expression of the first derivative of the Bézier curve computed for each instrument border.

Figure 5 . 3 :

 53 Figure 5.3: Scheme adopted for the simulation where the kinematic model used for the pose estimation has an offset on r m wrt to the reference kinematic model.

  k w =15,a=1for both ζ ch k w =5,a=3for both ζ ch k w =100,a= π/180 for ζ ψ k w =10,a= π/60 for ζ ψ As suspected, in case of uncertainty on the model (either on the ψ or channel position), considering a fixed mechanical model does not provide good results neither in terms of 3D position estimation (Fig. 5.4(a) and (b))n o ri nt e r m so fi m a g e convergence (Fig. 5.4(c) and (

Figure 5 . 4 :

 54 Figure 5.4: Results of simulation considering an error in the model on theinstrumen tc hannel position. The graphic shows the outline of the RMS error ont h et i p3 D position estimation (top line) and of the 2D error over all thec o r n e r s( b o t t o m line) for different cases (cf 5.2). In (a) and (c) the error is introduced in the instrument channel position and in (b) and (d) the error is introduced only in the orientation of the channel wrt to the camera.

Figure 5 . 5 :

 55 Figure 5.5: Zoom on the origin of Fig. 5.4(a) and (b).I ft h ed e a db a n di sc o r r e c t l yc e n t e r e d(i.e. small uncertainty on the model), the convergence to theo p t i m a ls o l u t i o n is faster and slightly more precise (given an equal number of iterations) when the dead-band is narrow.

Figure 5 . 6 :

 56 Figure 5.6: RMS errors committed in the estimation of the channel position when the supposed channel position correspond to the correct value (cases 12-22) (a) and those committed in the estimation of the ψ when the supposed ψ is equal to the correct value (cases 1-11) (b).

Figure 5 . 7 :

 57 Figure 5.7: Effect of adding noise at the visual measurements (i.e. the position of the markers' corners) and considering uncertainty on both ψ and channel position (x ch ,y ch ).I n(a) the RMS of the 3D position estimation errors, in (b) the RMS errors between actual and reprojected corners in the image (after convergence of the optimization algorithm). RMS error on channel position and orientation are shown respectively in (c) and (d).

Figure 5 . 8 :Figure 5 . 9 :

 5859 Figure 5.8: On the left: pose estimation result over a trajectory along x (top), y (middle) and z (bottom) coordinates: the blue line is the ground truth position retrieved by the stereo-vision system and the red line is the estimated position. On the right column: distribution of the estimation absolutee r r o ra l o n gt h e different coordinates.

Figure 5 . 10 :

 510 Figure 5.10: Representation of the 3D TCP positions of the validation sequence and the associated position estimation error considering (in (a) )o rn o t( i n(b))t h e kalman filtering. The error norm is color coded: the more reddish the higher is the norm of the estimation 3D error.

Figure 5 .

 5 Figure 5.11: in-vivo results of the presented method: (Left) the segmentation process and (right) the reprojection of the instrument from the estimated pose. The green dot is the reprojection of the center of the grasper. In (a),t h ec o n s i d e r e d mechanical play allows to fit the instrument even when it is pushing on the organ. In (c),t h er e a li n s t r u m e n ti sc o r r e c t l yd i s c r i m i n a t e df r o mi t sr e flection on the organ. Even though in (e) the segmentation process partly fails, an acceptable result is obtained in the instrument fitting and reprojection (f).

Contents 6 . 1 3 6. 4 1 6. 5 8 6. 7

 61341587 Radial Basis Functions Network ................ 1 2 1 6.1.1 Principle ............................. 1 2 1 6.1.2 Learning the 3D position from image .............. 1 2 2 6.2 Improved Clustering Domain .................. 1 2 7 6.3 RBF: Simulation Result ..................... 1 3 0 6.3.1 Cluster Initialization ....................... 1 3 1 6.3.2 Choice of K ............................ 1 3 2 6.3.3 Input Domain ........................... 1 3 Locally Weighted Regression (LWR) Method ........ 1 3 8 6.4.1 Simulation Results ........................ 1 4 Counteracting the noise ..................... 1 4 1 6.6 Experimental Results ....................... 1 4 Comparison with Model-Based Method ............ 1 5 1

Figure 6 . 1 :

 61 Figure 6.1: Different steps of the learning process for a single input single output function.

Table 6 . 1 :

 61 Elements of the training set pertaining to cluster k ŷi Estimation of the i-th coordinate y 1 = X, y 2 = Y and y 3 = Z ŷm,i i-th component of the output sample corresponding to x m Symbols used in the RBF formalization and their meanings.

Figure 6 . 2 :

 62 Figure 6.2: Example illustrating the problem related with the initial clustering of the RBF approach.Let us suppose that the instrument is bent and turn around its main axis (a) and that we want to approximate the relationship between the x coordinate of the tip of the instrument and its distance from the camera z (b).If the clustering algorithm (k-means like) is applied considering only the input data (x coordinate), it would lead to a bad estimation(c).I f ,o n t h eo t h e r hand, we take into account also the output data (the z coordinate describing the depth), it will be more probable to obtain a final clustering result similar to the one shown in (d) which leads to a Gaussian whose shape better describes the tendency of the x-to-z relationship.

  set # # DOFs considered feature (n)i n p u t d i m . ( 2n)# s a m p l

Figure 6 . 3 :

 63 Figure 6.3: Sorted 3D error norms for the cases listed in table6.4.

Figure 6 . 4 :

 64 Figure 6.4: Sorted 3D error norms for the cases listed in table6.5.

Figure 6 . 5 :

 65 Figure 6.5: Sorted 3D error norms for the cases listed in table6.6.

Figure 6 . 6 :

 66 Figure 6.6: Principle of the LWR approach on a one-dimensional example. For a new query point, the weights associated to the all training set samples are computed according to a distance metric (such as the one in (b))a n dw i t hr e s p e c tt o the query point in question. After that, equation (6.15)t oo b t a i nt h el i n e a r approximation valid only in a neighborhood of the query point.

Figure 6 . 7 :

 67 Figure 6.7: Sorted 3D error norms for the cases listed in table6.7.

Figure 6 . 8 :

 68 Figure 6.8: Sorted 3D error norms for the best initialization of the caseslistedintable6.8.

Figure 6 .

 6 Figure 6.12: Effect of the two improvement proposed for the learning based method. On the graphic, we can observe that the training set enrichment improves the quality of the position estimation obtained by the RBF (cf. curves RBF classic vs RBF enrich)a n db yL W R( c f . LWR classic vs LWR enrich). Finally, in the case of RBF, realizing the clustering in the input/output joint domain (curve RBF enrich inout)p r o v i d e sb e t t e rr e s u l t st h a nt h o s eo b t a i n e dw i t ho n l yt h e training set enrichment.

Figure 6 .

 6 Figure 6.13: The norm of the 3D error committed with the different approaches is sorted and shown in (a) (red: Model Based, blue: RBF and green: LWR). In (b) a color code is used to highlight the norm of the error in the workspace. The more red they are the higher is the error committed in its estimation. The highest errors seem to be located at the borders of the training set.

Contents

7. 1

 1 Conclusion ............................. 1 5 1 7.1.1 Chapter 3 ............................. 1 5 1 7.1.2 Chapter 4 ............................. 1 5 2 7.1.3 Chapter 5 and 6 ......................... 1 5 2 7.2 Future Work ............................ 1 5 4 7.2.1 Feature Segmentation ...................... 1 5 4 7.2.2 Model-Based Approach ..................... 1 5 5 7.2.3 Learning-Based Approach .................... 1 5 5 7.2.4 Simulation Experiments ..................... 1 5 6 7.2.5 Instrument Control ........................ 1 5 6 7.2.6 Possible Further Application .................. 1 5 6

  (A θ )i sm o d i fi e d ,b e c a u s e θ produces different effects on different sections of the instrument. For deriving the comp onent A θ for J g_gen ,o n ec a nf o l l o wt h es a m ep r o c e s sa s for J g ,b u tt a k i n gi n t oa c c o u n tan o nn u l lt h i r dc o m po n e n tf o rt b S,C :

φ 6 K

 6 Scheme of the tip of STRAS with the left instrument.

Figure 1 -

 1 Figure 1-En(a)laphotodelapointedel'endoscopeaveclesinstruments et en (b) le schéma correspondant.

Figure 2 -

 2 Figure 2-( a ):c e l l u l ee x p é r i m e n t a l ec o m p o s é ed el ap l a t e f o r m eS T R AS et deux caméras conventionnelles positionnées au-dessus du système robotique et formant un système de vision stéréoscopique ; (b) : pour chaque configuration une image est prise par la camera endoscopique (bas) et simultanément par les caméras du système stéréo (haut).

Figure 3 -

 3 Figure 3-P o u rc h a q u ec o n fi g u r a t i o nd ur o b o t ,l ad i r e c t i o na v e cl ap l us grande variance sur l'estimation de la position du bout de l'instrument est sélectionnée et lesnormesdeserreursobten uesson tordonnées.Le résultat pour différentes primitives est montré, la meilleure solution consistant à considérer les coins apparent des différents marqueurs (5 marqueurs dans ce cas).

  Figure 4-E x e m p l ed es c è n ei n -v i v oc a p t u r é ep a rl ac a m é r ae n d o s c o p i que. On peut observer la forte spécularité au milieu de l'instrument due à l' illumination intense et le reflet de l'instrument sur l'organe.

Figure 5 -

 5 Figure 5-É t a p e sd el am é t h o d ed es e g m e n t a t i o n .( 1 )L er é s u l t a td o n n ép a rl e sm o d è l e sG a u s s i e n sd e s couleurs est binarisé pour déterminer les régions jaune et bleu dans l'image. (2) Création d'un arbre en utilisant la connectivité spécifiée : la branche 2 n'est pas connectée parce qu'elle ne respecte pas la condition 1; la branche 1 ne respecte pas la condition 2 parce que l'orientation de la branche diffère sensiblement de l'orientation des branches qui la précèdent. Finalement, l'arbre est traité pour obtenir un chemin orienté. (3) Le squelette est utilisé pour chercher localement les pointsd eb o r d s ,e nr e c h e r c h a n tl e sm a x i m a sl o c a u xd u gradient le long des perpendiculaires à la section du squelette. Ces points candidats sont ensuite décrits avec des courbes de Bézier . Les coins apparents des marqueurs sontfi n a l e m e n te nr e c h e r c h a n tl e sd i s c o n t i n u i t é s de couleur le long des courbes de Bézier (4).

Figure 6 -

 6 Figure 6-C o n s t r u c t i o nd el ' a r b r ea v e cl ac o n n e c t i v i t és p é c i fi é e .D ans le premier cas, la branche n'est pas créée parce qu'elle ne respecte pas la condition 1. Dans lec a s2e t3 ,c ' e s tl ac o n d i t i o n2q u in ' e s tp a s respectée, parce que l'orientation de la branche considéréed i ff è r es e n s i b l e m e n td el ' o r i e n t a t i o nd o n n é ep a r les branches qui la précèdent. Cela permet de ne pas connecterd e sf a u x -p o s i t i f s( c a s2 )o ud ef o r m e rd e s boucles internes qui correspondent à configurations qui n'ont aucune signification physique (cas 3).

Figure 10 -

 10 Figure 10 -Comparaison entre les deux approches basées modèle. Les deux courbes représentent les normes des erreurs commises sur l'estimation de la position 3D pour différentes configurations dans l'espace de travail et ordonnées de façon croissante. La considération des jeux mécaniques (courbe rouge) permet obtenir une estimation nettement meilleure à celle obtenue avec une méthode plus classique comme celle de Reilink (courbe bleu).

Figure 11 -

 11 Figure 11 -Résultats in-vivo de la méthode présentée : (gauche) le résultat de la segmentation et (droite) la reprojection de l'instrument à partir de la pose estimée. Le point vert est la reprojection du centre de la pince. Dans (a), les jeux mécaniques considérés permettent d'approcher l'instrument même s'il est en train de pousser sur l'organe. Dans (c), l'instrument réel est correctement discriminé de son reflet sur l'organe. Même si la segmentation échoue partiellement (e), la pose estimée peut être considérée acceptable du point de vue qualitatif en termes d'erreur de reprojection (cf. (f)).

  6A p p r o c h e b a s é e a p p r e n t i s s a g eLes erreurs résiduelles de l'approche basée modèle peuvent être expliquées en partie par l'utilisation d'un modèle paramétrique du système. En effet, la tâche de déterminer et décrire tous les aspects qui influent sur le comportement de l'instrument est ardue et le résultat serait un modèle complexe et difficilement exploitable ou transférable à d'autres systèmes. Pour éviter l'utilisation de modèles, nous avons étudié une deuxième approche où l'idée de base est d' apprendre la fonction qui relie les indices visuels dans l'image avec la position 3D du TCP.La forme de ce type de fonctions est difficilement déterminableap r i o r i .P o u rc e t t er a i s o nn o u sa v o n s choisi d'étudier les potentiels de plusieurs méthodes générales parmi lesquelles nous avons choisi les deux qui ont donné les meilleurs résultats : un réseau de fonctions à base radiale (RBF) et une régression localement pondérée (LWR). La première approche modélise la fonction comme une somme pondérée de K noyaux gaussiens ϕ k (modèle local) dont les poids sont calculés d'après un critère global[START_REF] Broomhead | Multivariable functional interpolation and adaptive networks[END_REF][START_REF] Moody | Fast learning in networks of locally-tuned processing units[END_REF]. Plus en détails, étantdonné x =[x 1 , ••• ,x 2n ] T levecteur d'entrée, l'estimation d'une sortie ŷ obtenue par un réseau de fonctions àb a s er a d i a l ep e u tê t r eé c r i t ec o m m e: ŷ = h(x)= K k=1 γ k ϕ k (x)+γ K+1

  où C k est le centre de chaque noyau, Σ k sa variance et Y k son amplitude. Cette méthode nécessite une étape initiale d'apprentissaged i v i s é ee nd e u xp a s .L ep r e m i e rp a sr é p a r t i t les donnée d'entrée en clusters (en utilisant un algorithme type k-means) et, selon les composants de chaque cluster, calcule la forme et la position des gaussiennes ; le deuxième pas détermine la pondération des noyaux qui permet de décrire au mieux la relation entrée/sortie. La seconde approche effectue une approximation linéaire en fonction de l'information locale de l'ensemble d'apprentissage [1]. Cette méthode ne nécessite pas d'étaped ' e n t r a î n e m e n tc a rt o u t e sl e sd o n n é e sd el ' e nsemble d'apprentissage sont utilisées lors d'une nouvelle estimation. Comme visualisé en figure 13(a), la valeur estimée de la sortie pour un nouvel échantillon d'entrée (query point)e s tl er é s u l t a td ' u n ea p p r o x imation linéaire où la contribution de chaque échantillon de l'ensemble d'entraînement (x m )e s td é t e r m i n é e

Figure 13 -Figure 14 -Figure 15 -

 131415 Figure 13 -Principe de l'approche LWR pour un cas monodimensionnel.P o u ru n en o u v e l l ee n t r é e( query point )( r e dl i n ei n( a ) ) ,l e sp o i d sa s s o c i é sàt o u sl e sé c h a n t i l l o n sd el ' e n s e m b l ed ' e n t r a î n e m e n ts o n tc a l c u l é s en fonction de la mesure de distance adoptée (comme celle proposée en (b)) et par rapport à l'entrée considérée (query point). Ensuite, l'équation (11) est utilisée pour obtenir l'approximation linéaire, qui est valide seulement dans le voisinage du query point.

Finalement

  , dans le cas du RBF, le clustering réalisé dans le domaine conjoint entrée/sortie (courbe RBF enrich inout)a m é l i o r el er é s u l t a to b t e n ua v e cl ' e n r i c h i s s e m e n ts e u l .

Figure 16 -

 16 Figure 16 -Comparaison entre les trois approches proposées pour l'estimation de la position 3D du bout de l'instrument. Les trois courbes représentent les normes dese r r e u r sc o m m i s e ss u rl ' e s t i m a t i o nd el ap o s i t i o n 3D pour différentes configurations dans l'espace de travail etordonnées.Laméthodederégressionlocalement pondérée (LWR) semble donner le meilleur résultat par rapport aux RBF et la méthode basée modèle.

  Motivation and Problem Statement .............. 2 1.2 Objectives ..............................

	Chapter 1
	Introduction
	Contents
	1.1

3 1.3 Thesis Overview .......................... 4 1.4 Labex CAMI Context ....................... 5

  State of the Art: Medical Flexible Systems and the Problem of their Control

	Chapter 2

Contents 2.1 Medical Context .......................... 7 2.1.1 Flexible Systems for NO -SCAR Surgery ........... 8 2.1.2 Medical Robotics for MIS .................... 1 5 2.2 Continuum Robots ........................ 2 7 2.2.1 External Actuation ........................ 2 7 2.2.2 Concentric Tube Design ..................... 2 8 2.3 Control of Continuum Robots .................. 2 9 2.3.1 Kinematic and Dynamic Model-Based Methods ........ 3 0 2.3.2 External Sensors ......................... 3 1 2.3.3 Pose estimation of continuum robots using vision ....... 3 2

Table 2 .

 2

1:

The main commercial manual flexible platforms for medicine.

Table 2 . 2 :

 22 Principal commercial flexible robotic platform for medicine.

	Robotic Flexible Instruments: Research Experimental Platform
	MASTER
	Application		DOFs Characteristics
	General	Endoscopic	5
	Surgery		(+1)
			x2
			instr.

Table 2 . 3 :

 23 Principal flexible robotic platforms for medical application (Research).

  a n d (3.22)w h i c h ,i nt u r n ,r e q u i r et h ek n o w l e d g eo ft h ed i s t o r t i o np a r ameters usually available after a the calibration stage. Let us, now, suppose that P I moves outside the projection line achieving the new position P ′ I and let us indicate the perceived corresponding motion on thei m a g e plane Π I with (∆x, ∆y).T h ec o r r e s p o n d i n gn e wp o s i t i o n( p ′ d )o nt h er e a li m a g e plane Π I d can be computed relying on eq. (3.28). Afterwards, the displacement vector (∆x d , ∆y d ) describing the translation between p d and p ′ d can be easily retrieved. The equivalent distortion gain is then defined as:

Table 5 . 2 :

 52 Offsets used for the simulation tests. The supposed kinematicm o d e lu s e dt o estimate the 3D pose is equal to the correct model used to create the synthetic image except for the 3D position of the instrument channel (which is displaced) or the orientation of the channel wrt to the camera axis.

	Test case	Channel on	Test Case	ψ on
	Test case supposed model [mm] Test Case supposed model [deg.]
	1 2 3 4 5 6	(x ch -5,y ch -5) (x ch -4,y ch -4) (x ch -3,y ch -3) (x ch -2,y ch -2) (x ch -1,y ch -1) (x ch ,y ch )	12 13 14 15 16 17	ψ -5 ψ -4 ψ -3 ψ -2 ψ -1 ψ
	7	(x ch +1,y ch +1)	18	ψ +1
	8	(x ch +2,y ch +2)	19	ψ +2
	9	(x ch +3,y ch +3)	20	ψ +3
	10	(x ch +4,y ch +4)	21	ψ +4
	11	(x ch +5,y ch +5)	22	ψ +5

Table 5 . 4 :

 54 Numercial results of the different contributions of the proposed method. The three column (from left to rith) represent: pose estimation using a simple LM optimization, LM optimization expressing the error wrt to a Bézier -based reference frame and using motor encoder data to filter the results.

		LM	LM Bézier Frame Complete
		on x : 3.22	2.98	2.03
	RMS 3D err on y : 1.76	1.89	2.07
	[mm]	on z : 3.76	3.43	2.77
	Max 3D Err [mm]	16.72	11.84	11.93

Table 6 . 2 :

 62 Sets used for the simulations considering only 3 or 6 DOFs. Sets 2, 4 and 6 are used to asses the accuracy of the function learned respectively on sets 1, 3 and 5.

Table 6 . 3 :

 63 Effect of the number of cluster over the 3D position estimationa c c u r a c y .

		train: 280 elements; test: 180
	K	Av g nu mb e r o f e l e m e nt s per cluster	3D RMS error
	5	56	2.3659
	10	28	1.9970
	15	18.67	1.9
	20	15.0556	1.9868
		train: 140 elements; test: 90
	K	Av g nu mb e r o f e l e m e nt s per cluster	3D RMS error
	3	46.6667	2.7574
	5	28	2.4408
	8	17.5000	2.4512
	10	15.1	2.8070

Table 6 . 4 :

 64 Quantitative results of pose estimation accuracy when considering 3DOFs, different clustering and training domains and different visual features. Cases 10 and 11 exploit the 3 yellow markers centroids whereas cases 20, 21 and 22 the all 5 centroids. The worst 3D error value is computed (as for all theo t h e rt a b l e s ) selecting the estimation result deriving by the best initialization in terms of the RMS of the norms of estimation errors.

	1	2	3	3 0.5914 0.7039 1.5546	6.2484
	1	2	6	6 0.6312 0.4797 1.7528	5.2821
	3	4	3	3 0.4384 0.5231 1.3416	6.5967
	3	4	6	6 0.5971 0.4689 0.8496	3.1330
	3	4	10 10 0.5446 0.2705 0.7006	3.1936

Table 6 . 8 :

 68 Simulation test results considering image noise (and consequent error on visual feature extraction) both in training and test set. In this case, higher training and clustering domain dimensionality does not imply a highera c c u r a c y .

	50	5	6	6	6	RBF 1.1411 1.5936 3.5830 LW R 0 . 9 0 3 0 1 . 3 0 1 7 2 . 3 5 1 2	11.0444 7.6843
	51	5	6	10 10	RBF 1.2365 1.7560 4.0120 LW R 0 . 9 9 9 3 1 . 4 2 2 5 2 . 6 4 7 5	12.2903 8.1172
	52	5	6	3	3	RBF 0.8489 1.4579 3.2583 LW R 0 . 8 2 7 6 1 . 2 4 8 7 2 . 2 0 2 1	9.2711 7.1378

Table 6 . 9 :

 69 Effect of the image noise on the accuracy of the 3D position estimation for various clustering domains. The output, which is supposed tob el e s sa ff e c t e d by noise, can be used to build the clustering set to improve theG a u s s i a ns h a p e and, consequently, the position estimation. The same effect is less evident when using LWR whose shape seems to be more adequate for this regression problem.

		11.8784
	2	8.1118

Table 6 .

 6 11: Accuracy results with domain enrichment to "learn" the noisee ff e c to nt h e image centroids. Comparing them with cases from 50 to 52 (Tab. 6.8), such enrichment seems to help the position estimation precision above all for RBF Network and also in terms of maximum 3D error. As for the other cases, the maximum 3D error is computed over the best initialization in terms of RMS error.

	Tab. 6.10)c a nb ea sg o o da s( o re v e nb e t t e r )t h eo n eo b t a i n e dc o n s i d e r i ng higher
	dimensionality (e.g. case 62, Tab. 6.9).

Table 6 .

 6 Figure 6.10: Sorted 3D error norms for the cases listed in table6.11 and comparison with cases from 50 to 52 where no domain enrichment was performed. In(c),a zoom on the last samples of (b) is shown. 12: Effect of domain enrichment used to increase the cardinality of the training test. In case 100, only 140 samples are used to train the regression machine which is tested on a sequence of 180 samples. In case 101, the training domain density is increased as in the case of the domain enrichment and output is used to form the clustering set.

		14
		12		50 rbf
					51 rbf
					52 rbf
		10		90 rbf
	RMS 3D Err [mm]		6 8		91 rbf 92 rbf
			4
			2
			0
			0	20	40	60	80	100	120	140	160	180
							sample
							(a)
			9
			8		50 lwr
					51 lwr
			7		52 lwr
					90 lwr
	RMS 3D Err [mm]	3 4 5 6		91 lwr 92 lwr
			2
			1
			0
			0	20	40	60	80	100	120	140	160	180
							sample
							(b)
			8	50 lwr
				51 lwr
		7.5	52 lwr
				90 lwr
			7	91 lwr
	RMS 3D Err [mm]	5 5.5 6 6.5	92 lwr
		4.5
			4
		3.5
			166	168	170	172	174	176	178	180	182
							sample
							(c)

Plane formed by the access point of the instruments (trocars position in case of Laparoscopy or working channels end in case of flexible systems) and -ideally -the point of the organ that has to be operated.

Institut de Recherche contre les Cancers de l'Appareil Digestif [Institute of Digestive Cancer Research]

The next chapter[START_REF] Broomhead | Multivariable functional interpolation and adaptive networks[END_REF], then, will clarify and formalize all these elements which will be fundamental tools all along this manuscript and not only for this preliminary study (chapter 4).

Alternatively, it could be a local minimum with a wide attraction basin.

The notation here is simplified and details will be given latero n . L is computed from(5.8) where 4 blocks are added corresponding to the new DOFs.

The mechanical parameters µ is considered known in this study. The chosen parameters are sufficient to discuss the point. Variation of µ would lead to totally similar results

Actually, the described problem could be solved by iterativel e a r n i n ga n dr e i n f o r c e m e n tl e a r ning, but this is not in the scope of this thesis.

After taking the three PCs of each sample of set 1 and 2, the clustering and input domain are equals between each other but their dimension has decreased from 6 to

(q cl = qtr =3).

Let us remind that the fifth mechanical DOF is considered fix ande q u a lt oi t sn o m i n a lv a l u e .

Acknowledgments

mar mis miedos e inseguridades, por hacerme recordar mis verdaderas motivaciones profesionales, por enseñarme a disfrutar de la vida desde sus más pequeñas cosas.

Part I

Visual Features Selection and Extraction

 4.6:Scheme for the tree updating process. At each generation, theb ran c ht ow h om the maximum likelihood is associated is selected (highlighted in green). If, as in the represented case, the most likely branch is that of a putative node (i.e. the node representing a fusion of two regions. The nodes of thec o r r e s p o n d i n g two regions are deleted and the putative node turn into a real node. If the generation presents more than two regions, the new nodes and branch values are computed and the process is repeated on the updated tree (third line)

Part II

3D Pose Estimation of the Flexible Instrument

Method

Inspired by [START_REF] Reilink | [END_REF] configuration, which can be wrong.

We have then chosen to employ a reference p oint (the white ball) distant from the ending of the bendable part so as to have a large 3D positionerrorinthosecases where the estimated orientation is wrong. This makes the RMS errors on the 3D position to represent, somehow, also the quality of the entire pose estimation.

Looking at the RMS it can be stated that our method provides a more precise orientation wrt the fix model approach. In fact, the RMS error over this same sequence using fix mechanical parameters increases abruptlyw h e nc o n s i d e r i n gt h e white ball as the validation 3D point (instead of a EM sensor attached at the end of the bendable part) getting to [3.35, 3.59, 10.54] mm respectively along x, y and z coordinates.

Even if the RMS error of the fix mechanical model approach are computed with aG T3 Dr e f e r e n c epo i n tn e a r e rt ot h ei n s t r u m e n tt i p( a sd o n ei ns e c . 5.1 with EM sensors), the result is still worse than the one obtained by the proposed method.

Once demonstrated the effectiveness of the consideration of the mechanical plays in the optimization process, another interesting result is the one concerning the contributions of each one of the proposed implementation aspects: Bézier reference frame for the error and motor encoder data fusion. Table 5.4 and Fig. 5.9 summarize these aspects showing the sorted norms of the errors committed in the estimation of the TCP 3D position. Bézier based reference frame seems to improve the pose estimation only slightly and the major contributioni si nt e r mo fm a x i m u m error. Maybe, further improvement could be achieved with a more exhaustive study over the trade-off between the parallel/perpendicular errorc o m p o n e n t s . W h a ti s evident from data, though, it is the capacity of the Kalman filter with dynamic confidence adjustment of mitigating the aberration in pose-estimation errors taking into account temporal data when visual information is incomplete.

The aforementioned problem deriving by the low reliability of the deflection encoder data is also visible in Fig. 5.10(b) where the 3D TCP positions are represented and a color code is used to represent the norm of the error (the more reddish the 3D estimation error can be computed. This can be repeated as many time as it is desired according to how precise the value of K is wanted to be know, without the need of creating a validation (test) set.

Input Domain

The interest of study the effect of input domain dimensionality arises from the following Consideration 2. Taking into account only three DOFs in building the training and test set, inherently, implies that the shape of the instrument depends only by three factors or three variables. This should be, somehow, reflected also in the image-topose function provided that the chosen feature can ful ly capture the 3D shape. In this case, the domain of the function relating the 2n-dimensional input space with one 3D coordinate (either X, Y or Z)o ft h eT C Ps h o u l dl i eo nam a n i f o l do fd i m e n s i o n 3.

However, this is not always true, since it depends on the chosen feature and information representation. Most of the time, this consideration would lead to the conclusion that the input domain dimensionality should be at least equal to the number of DOFs. Moreover, in certain conditions, spatial localization can become meaningless in high dimensional domains because, typically, with increasing dimensionality all data points tend to have the same distance to each other [Beyer 1999] and this could have side effects on the good convergence of the k-means-like algorithm.

In this case, though, domain dimensionality is not critical but we want to study which is the needed quantity of visual information to have an accurate 3D position estimation. This could also be useful with noisy visual features to isolate only the information useful for 3D position retrieval.

Ad i m e n s i o nr e d u c t i o nc a nb ec a r r i e do u tp e r f o r m i n gaP r i n c i pal Component Analysis (PCA) over the input samples so as to observe if the first 3 principal components (PCs) are enough to estimate the 3D position.

Consideration 2 seems to be confirmed analyzing the results when using set 1 and 2 (respectively for the training and for the test). In fact, there is no substantial difference between the RMS error using 3 PCs or the whole information given by the three centroids (case 10 and 11) 2 .

However, this assertion is contradicted when 5 markers (set 3a n d4 )a r eu s e d instead of 3. In fact, even if case 10 and 20 are comparable, ther e s u l t si m p r o v e considerably when q cl and q tr are equal to 6 (cf case 21 and 11). Nevertheless no further improvement is appreciable when the whole 5 centroids information is used (case 22) except for the estimation of Y coordinates which seems to get some Appendix A

Apparent Border Points

A.1 Computation of the 3D Corresponding Points

In chapter 3 we pointed out the dependency of the extended image Jacobian from the depth of the point for which it is computed. Therefore, we have to be capable of computing such point for different configurations of the instrument.

The instrument can be considered as a torus of internal radius r which changes its 3D shape according to the variations of the 3 natura DOFs (λ, φ, θ). Knowing the instrument kinematic model and r these points can be computed according to perspective projective relationship for the torus. Fig. A.1 shows the profile of a traversal section of the instruments and the projection rayso fi t sb o a r d e r st o w a r d s the camera. Looking at the scheme of this figure, we can define some properties for those point whose projection will form the apparent border oft h ei n s t r u m e n t :

• The projection rays are tangent to the circumference in the points P i,u and P i,l and pass through the principal point of the camera C.

• Unlike C, P i,u and P i,l will lay on the same plane defined by the circumference (which is the same defined by the x and y axis of F sec ).

• The distance of P i,u and P i,l from S will be equal to r.

To make all the computation easier, these geometrical relationships can be expressed taking as reference frame the one associated to the center of the circumference (i.e. F sec ). Therefore, defining

the following equations can be written:

since P 1 and P 2 pertain to the circumference and

Image Plane for the perpendicularity condition.

Executing the multiplication in equation (A.2)a n dc o n s i d e r i n ge q u a t i o n( A.1), X i can be expressed as

Then, equation (A.1)c a nb ew r i t t e na sf o l l o w s :

which is a second order equation with two solutions: one is theupperboarderpoin t P sec u =(X u ,Y u , 0) and the other is the lower P sec l =(X l ,Y l , 0). The position with respect to the camera frame can be easily found from the solutions just obtained as:

and the same thing for P l .

A.2 Computation of the Jacobian for Apparent Points

Corners of the markers are not completely defined physically.T h e i ra c t u a lp o s i t i o n on a section varies depending on the pose of the instrument with respect to the camera, and as a consequence the velocity of the apparent point on the section

velocity between F a and F b computed in c and expressed wrt

is also affected. This effect has to be taken into account in the geometrical 3D Jacobian.

In this section, a change in the notation is performed to precisely express all the relationships needed for the computation of the Jacobian fora p p a r e n tp o i n t s( c f . Tab. A.1).

A.2.1 Velocity of an apparent point onto its section

Let consider an apparent point A (either P i,u or P i,l ), which can slide onto a given section of the instrument of center S.T h e n

where the first term of the right side of the equation is the velocity of the apparent point with respect to physical points of the considered section, and the second term is the velocity of the physical point with respect to the camera.

The second term can be expressed conventionally in function of the joint velocities as : cam V A sec/cam = J A 3D ṙ. To express the velo city of the apparent p oint A onto the section it pertains (the first term), one first expresses the constraint of a point on thes e c t i o nt ob ea n apparent point: orthogonality between SA and CA.

In the frame of the section, one has :

which can be rewritten as:

By noting sec C =( x c ,y c ,z c ) T and stating that the previous relation should be constant with respect to time, one has dQ dt =0and one obtains:

Validation Pro cess

For the validation pro cess to b e meaningful, the estimated q uantity (3D TCP pose) needs to be compare with the value obtained with an alternative measurement method. A possibility for retrieving such measurement is attaching an external sensor, such as electromagnetic sensors, to the tip of the instrument. However, in previous experiments, poor quality results have been provided probably due to the effect of metallic parts of the Anubis and its motorization on the electromagnetic field.

To avoid these problems, an alternative metho d is used here based on stereo vision. A system composed by two cameras has been placed looking towards the left instrument workspace so that the white ball was visible in both images.

To build the ground truth (GT), a sequence of several instrument configurations is created. For each of them the endoscopic image is stored together with 3D position of the white ball of the electric tool (as shown in Fig. 3.8)e s t i m a t e du s i n gs t e r e o vision.

The pose estimation based on endoscopic images provides the 3D pose wrt to the endoscopic camera whereas the ground truth as defined so far is expressed wrt the stereo-vision system. A registration step between the stereo system and the endoscopic camera is needed to be able to compute the root means q u a r e( R M S ) estimation errors over the 3D position of the electric tool tip (white ball).

To carry out this registration, the particular ob ject shown in Fig. B. 1(b) is placed so that one grid is captured by the endoscopic camera and the other is captured by the stereo system as shown in (Fig. B.1.K n o w i n gt h ei n t r i n s i cp a r a m e t e r so f the cameras (thanks to a previous calibration process for both stereoscopic system and endoscopic cameras) and detecting the corners of the corresponding grids, the extrinsic parameters can be computed for both endoscopic ands t e r e o s c o p i cs y s t e m defining, respectively, T left grid 2 and T endo grid 1 . To finalize the registration, the transformation (T grid 1 grid 2 )b e t w e e nt h er e f e r e n c e frame of the two grids (whose origins is the common corner of the green yellow colored squares) must be known. This depends on the geometry of the chosen object which has been retrieved using the stereo-vision system.

In this manner, the global transformation between one camerao ft h es t e r e o system and the endoscopic camera can be computed as follows: En première approximation, nous pouvons considérer que, connaissant le marqueur A,l esév én em en t sd e forme et de topologie sont des variables Gaussiennes qui peuvent être définies comme :

Appendix B. Validation Process

p(S (2) |A) ∼ 1 2πσ

(2) sa exp(-

pour la forme (fig. 7(a)) et

sa ) et (µ

sa ), -d el ad i s t a n c eE u c l i d i e n n ed e p u i sl ec e n t r ed el ar é g i o np a r ent (µ da ,σ da ).

Ces valeurs sont déterminées pour chaque marqueur (de 1 à 5). Pour tenir en compte de la variation de taille et de forme perçues dans l'image pendant le mouvement de l'instrument, ces paramètres sont actualisés à chaque image en considérant les observations sur une fenêtreg l i s s a n t ed e s1 0d e r n i è r e si m a g e s .

Apartirdescaractéristiquesd'unnoeud(réelouartificiel),onpeutalorscalculerlavaleurdelaprobabilité de ce noeud d'être le marqueur du niveau considéré :

Al ' e x c e p t i o nd el ar a c i n e( l em a r q u e u rd el ab a s ed el ap a r t i epliable), cette probabilité conditionnelle peut être considérée comme une récompense associée à chaque branche de l'arbre en provenance du niveau

5A p p r o c h e b a s é e m o d è l e

La première approche que nous avons proposé pour estimer la pose de l'instrument à partir des primitives image consiste à utiliser un modèle géométrique paramétré dus y s t è m e .

En supposant que les modèles géométriques de l'instrument etd el ac a m é r as o n tc o n n u s ,u n ei m a g e synthétique (virtuelle) de la scène peut être calculée pour une configuration donnée de l'instrument. En conséquence, le problème de l'estimation de la pose 3D peut être formalisé comme un problème d'optimisation, dont l'objectif est de trouver la meilleure configuration de l'instrument selon un "critère visuel ", par exemple basé sur la différence entre l'image virtuelle courante (position des points dans l'image calculée) et l'image de référence (position des indices visuels dans l'image vidéo acquise) [START_REF] Marchand | Virtual visual servoing : a framework for real-time augmented reality[END_REF].

Nous avons montré que l'hypothèse considérant que l'état actuel de l'instrument dépend seulement de 3D D L( c o m m ed a n s [START_REF] Reilink | Pose reconstruction of flexible instruments from endoscopic images using markers[END_REF] )n ' e s tp a sa d a p t é e:e ne ff e t ,c e r t a i n e sp arties du modèle peuvent varier en raison, par exemple, des jeux mécaniques entre les instruments et lesc a n a u xq u ' i l st r a v e r s e n t .C e sj e u xn es o n tp a s négligeables ni évitables puisque ils sont indispensables afin que l'instrument puisse se déplacer à l'intérieur du canal.

Nous avons donc décidé de considérer une tolérance sur les paramètres mécaniques et, en conséquence, nous avons développé une nouvelle méthode d'optimisation sef o n d a n ts u rd e sf o n c t i o n sd epo n d é r a t i o n ,q u i aide à contraindre l'estimation de ces paramètres dans un voisinage de leurs valeurs nominales. Le critère d'optimisation, donc, n'est plus strictement visuel, mais tient compte de ces contraintes sur les paramètres mécaniques :

où p i est l'état de référence et p i est l'état estimé. Les astérisques indiquent les valeurs nominales des paramètres mécaniques et . les valeurs estimées. Le choix de la forme de ρ dérive de la nature des tolérances.

Abstract

Thanks to their dexterity and compliance, flexible systems can reach distal body zone in a real non-invasive way through natural orifice allowing no-scar or single scar surgery. Being able to measure the 3D position of such instruments can be useful for various tasks, such as controlling automatically the robotized instruments or providing gesture guidance. In this thesis, we propose two automatic methods to infer the 3D pose of a single bending section instrument using only the images provided byt