Repositorio Institucional de Documentos

     !  ""#$% &'()* +,-+ Visual SLAM and Scale Estimation from Omnidirectional Wearable Vision RESUMEN La resoluci´ on del problema de Localizaci´ on y Mapeado Simult´ aneos (SLAM) con sistemas de visi´ on permite reconstruir un mapa del entorno a partir de medidas extra´ ıdas de im´ agenes y, al mismo tiempo, estimar la trayectoria u odometr´ ıa visual de la c´ amara. En los ´ ultimo a˜ nos el SLAM visual ha sido uno de los problemas m´ as tratados en el campo de la visi´ on por computador y ha sido abordado tanto con sistemas est´ ereo como monoculares. Los sistemas est´ ereo tienen la caracter´ ıstica de que conocida la distancia entre las c´ amaras se pueden triangular los puntos observados y por lo tanto, es posible obtener una estimaci´ on tridimensional completa de la posici´ on de los mismos. Por el contrario, los sistemas monoculares, al no poderse medir la profundidad a partir de una sola imagen, permiten solamente una reconstrucci´ on tridimensional con una ambig¨ uedad en la escala. Adem´ as, como es frecuente en la resoluci´ on del problema de SLAM, el uso de filtros probabil´ ısticos que procesan las im´ agenes de forma secuencial, da lugar a otro problema m´ as alla de una ambig¨ uedad de escala. Se trata de la existencia de una deriva en la escala que hace que esta no sea constate durante en toda la reconstrucci´ on, y que da lugar a una deformaci´ on gradual en la reconstrucci´ on final a medida que el mapa crece. Dado el inter´ es en el uso de dichos sensores por su bajo coste, su universalidad y su facilidad de calibraci´ on existen varios trabajos que proponen resolver dicho problema; bien utilizando otros sensores de bajo coste como IMUs, [17, 22] o sensores de odometr´ ıa disponibles en los veh´ ıculos con ruedas [5, 7, 26]; bien sin necesidad de sensores adicionales a partir de alg´ un tipo de medida conocida a priori como la distancia de la c´ amara al suelo [16] o al eje de rotaci´ on del veh´ ıculo [25]. De entre los trabajos mencionados, la mayor´ ıa se centran en c´ amaras acopladas a veh´ ıculos con ruedas. Las t´ ecnicas descritas en los mismos son dificilmente aplicables a una c´ amara llevada por una persona, debido en primer lugar a la imposibilidad de obtener medidas de odometr´ ıa, y en segundo lugar, por el modelo m´ as complejo de movimiento. En este TFM se recoge y se amplia el trabajo presentado en el art´ ıculo “Full Scaled 3D Visual Odometry From a Single Wearable Omnidirectional Camera” enviado y aceptado para su publicaci´ on en el pr´ oximo “IEEE International Conference on Intelligent Robots and Sytems (IROS)”. En ´ el se presenta un algoritmo para estimar la escala real de la odometr´ ıa visual de una persona a partir de la estimaci´ on SLAM obtenida con una c´ amara omnidireccional catadi´ optrica portable y sin necesidad de usar sensores adicionales. La informaci´ on a priori para la estimaci´ on en la escala viene dada por una ley emp´ ırica que relaciona directamente la velocidad al caminar con la frecuencia de paso o, dicho de otra forma equivalente, define la longitud de zancada como una funci´ on de la frecuencia de paso [11]. Dicha ley est´ a justificada en una tendencia de la persona a elegir una frecuencia de paso que minimiza el coste metab´ olico para una velocidad dada [29], [15]. La trayectoria obtenida por SLAM se divide en secciones, calcul´ andose un factor de escala en cada secci´ on. Para estimar dicho factor de escala, en primer lugar se estima la frecuencia de paso mediante an´ alisis espectral de la se˜ nal correspondiente a la componente zde los estados de la c´ amara de la secci´ on actual. En segundo lugar se calcula la velocidad de paso mediante la relaci´ on emp´ ırica descrita anteriormente. Esta medida de velocidad real, as´ ı como el promedio de la velocidad absoluta de los estados contenidos en la secci´ on, se incluyen dentro de un filtro de part´ ıculas para el c´ alculo final del factor de escala. Dicho factor de escala se aplica a la correspondiente secci´ on mediante una f´ ormula recursiva que asegura la continuidad en posici´ on y velocidad. Sobre este algoritmo b´ asico se han introducido mejoras para disminuir el retraso entre la actualizaci´ on de secciones de la trayectoria, as´ ı como para ser capaces de descartar medidas err´ oneas de la frecuencia de paso y detectar zonas o situaciones, como la presencia de escaleras, donde el modelo emp´ ırico utilizado para estimar la velocidad de paso no ser´ ıa aplicable. Adem´ as, dado que inicialmente se implement´ o el algoritmo en MATLAB, aplic´ andose offline a la estimaci´ on de trayectoria completa desde la aplicaci´ on SLAM, se ha realizado tambi´ en su implementaci´ on en C++ como un m´ odulo dentro de esta aplicaci´ on para trabajar en tiempo real conjuntamente con el algoritmo de SLAM principal. Los experimentos se han llevado a cabo con secuencias tomadas tanto en exteriores como en interiores dentro del Campus R´ ıo Ebro de la Universida dde Zaragoza. En ellos se compara la estimaci´ on de la trayectoria a escala real obtenida mediante nuestro m´ etodo con el Ground Truth obtenido de las im´ agenes por sat´ elite de Google Maps. Los resultados de los experimentos muestran que se llega a alcanzar un error medio de hasta menos de 2metros a lo largo de recorridos de 232 metros. Adem´ as se aprecia como es capaz de corregir una deriva de escala considerable en la estimaci´ on inicial de la trayectoria sin escalar. El trabajo realizado en el presente TFM utiliza el realizado durante mi Proyecto de Fin de Carrera [13] 1 con una beca de Iniciaci´ on a la Investigaci´ on del I3A y defendido en septiembre de 2011. En dicho proyecto se adapt´ o una completa aplicaci´ on C++ de SLAM en tiempo real con c´ amaras convencionales, para ser usada con c´ amaras omnidireccionales de tipo catadi´ optrico. Para ello se realizaron modificaciones sobre dos aspectos b´ asicos: el modelo de proyecci´ on y las transformaciones aplicadas a los descriptores de los puntos caracter´ ısticos. Fruto de ese trabajo se realiz´ o una publicaci´ on [12] en el “11th OMNIVIS” celebrado dentro del ICCV 2011. 2 Contents 1 Introduction 5 2 Related Work 9 3 Visual SLAM with catadioptric systems 11 4 Scaling of the visual odometry 15 4.1 Description of the basic scaling algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . 15 4.1.1 Spectral analysis on SLAM visual odometry . . . . . . . . . . . . . . . . . . . . . 15 4.1.2 Walking speed estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 4.1.3 Particle Filter for scale factor tracking . . . . . . . . . . . . . . . . . . . . . . . . 17 4.1.4 Scaling of the trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 4.2 Implementation within a real time monoSLAM framework . . . . . . . . . . . . . . . . . 20 4.3 Check of the spectral power consistency . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 5 Experiments 23 5.1 Spectral analysis for step frequency estimation . . . . . . . . . . . . . . . . . . . . . . . . 23 5.2 Scaling of the Visual Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 5.2.1 The Ground Truth . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 5.2.2 Setup of the parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 5.2.3 Scaling of the trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 5.3 Analysis of the computational cost . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 5.4 Analysis of the power consistency condition . . . . . . . . . . . . . . . . . . . . . . . . . 27 6 Conclusions and Future Work 35 A The Extended Kalman Filter 37 B The Sphere Camera Model 39 B.1 The Spherical Camera Model for the EKF . . . . . . . . . . . . . . . . . . . . . . . . . . 41 3 CONTENTS 4 Chapter 1 Introduction The resolution of the problem of Simultaneous Localisation and Mapping (SLAM) with one camera allows to reconstruct a map of the environment from the measurements taken from the images and, at the same time, estimate the visual odometry of the sensor. This problem can be addressed using either global optimization techniques or probabilistic filters like the extended Kalman filter or the particle fitler. Focusing on visual SLAM, with global optimization, the map and the position of the cameras are estimated by minimizing the reprojection error of the points in the given images. When using probabilistic filters, odometry and map estimations are updated by processing the images sequentially. As a result, probabilistic filters are frequently used in real time SLAM applications where images are processed as they are delivered. However, they have the drawback of a decrease in the accuracy in the long term since the images are forgotten as they are processed and used to update the estimation. In the last years, visual SLAM has become one of the most trending research fields in computer vision and has been addressed both by using stereo and monocular systems. The main feature of stereo systems is that, knowing the baseline of the cameras, detected landmarks of the scene can be triangulated and the visual odometry and landmark positions can be completely estimated. SLAM approaches using stereo systems have been presented in [19,21,23]. On the other hand, due to the impossibility to extract the depth of a landmark just from one single image, monocular systems only allow the camera motion and scene to be estimated up to an unknown scale. With this in mind, stereo systems may seem more appropriate than monocular ones to perform visual SLAM. However the use of single cameras for visual SLAM is still appealing since they are cheaper, more compact and easier to calibrate than stereo systems. One of the most important and succesful works on monocular SLAM is the one developed by Davison et al. [6], which is based on the extended Kalman filter. As landmark depths cannot be estimated only from the first image, this approach uses a pattern of known size to initialise some feature locations allowing the SLAM to start. Thus the scale of the map is fixed by the size of this initial pattern. In a later work by Civera et al. [2], the inverse depth parametrization for the map points allowed the SLAM to start automatically without the need of using an initialisation pattern. In this case the scale is arbitrarily fixed by a depth prior of the map landmarks and an acceleration noise setup parameter. Altough the scale can be initialised by a pattern of known size or some kind of prior, it is likely that scale drift arises between different portions of the scene as the size of the map gets larger. The reason why this drift occurs is the continuous lost and initialization of tracked landmarks, which act as anchor for the scale, due to the sequential processing of the images. This drift acts as a source of incremental error in the SLAM estimation, which leads to a deformation of the final map even after applying conventional loop closing techniques by identifying revisited parts of the map. In [27], Strasdat et al. propose a loop closing method which corrects the map deformation due to scale drift. Visual SLAM using omnidirectional cameras has been proposed in [4, 18, 28]. Due to the 360ofield of view (FoV) of omnidirectional cameras, features last longer on the image than in the case of conventional cameras, specially during big camera rotations. The increased lifespan of the features on the image translates in a better estimation of the position of the features on the map, a lower need to initialise new features and an increased robustness [24]. In this work we extend the SLAM approach for catadioptric cameras developed in our previous work 5 [12] which was presented as final degree project and submitted for the 11th OMNIVIS Workshop in 2011. This approach derived from state of the art real time EKF monocular SLAM for conventional cameras [3] and is used in this work to compute the visual SLAM estimation from sequences of images acquired with a catadioptric camera mounted on a helmet which is carried by an operator (Fig. 1.1). (a) (b) Figure 1.1: (a) Hemlet-camera device used in our experiments. (b) Omnidirectional image captured with our device. An induced effect of human walking is a head vertical oscillation whose frequency matches up with the step frequency [14]. Under the premise that the 6 d.o.f. visual SLAM is accurate enough, this vertical oscillatory motion of the head should be visible. Fig. 1.2a depicts an example of this behaviour, where the camera trajectory was obtained by performing a visual SLAM algorithm. Hence the step frequency of the camera carrier can be measured by estimating the power spectra of the vertical component of the camera trajectory (Fig. 1.2b). (a) 0 1 2 3 4 5 6 7 8 0 0.02 0.04 0.06 Frequency(Hz) Power Spectra (b) Figure 1.2: (a) Trajectory estimation of Visual SLAM from a head-mounted catadioptric camera. (b) Power spectra of the vertical component Walking speed is strictly calculated as the product of step frequency and stride length. However, there exist biomedical studies like the one lead by Grieve [11], which show an empirical relation between step frequency and the walking speed with no dependence on the stride length (or equivalently, a dependence 6 CHAPTER 1. INTRODUCTION of the stride length on the step frequency). Further studies explain this relation as the result of a human tendence to choose a step frequency that minimizes metabolic cost of locomotion at a given walking speed [15,29]. Based on this, we propose an approach to calculate the scale of the visual odometry from a single omnidirectional camera carried on the head of a person. This is done by first performing spectral analysis on short sections of the trajectory to extract the step frequency. Then we compute the estimated walking speed using the relation between step frequency and walking speed, and finally this estimation is integrated into a particle filter which recursively computes the scale factor. This work gave rise to a research article which has been recently accepted for the International Conference on Intelligent Robots and Systems, to be held between 7-12 October in Vilamoura(Portugal). In addition to the presentation of the work developed in this publication, in this Final Master Project we also improve the initial algorithm. Firstly, since this algorithm has been initially programmed in MATLAB and performed offline on the final SLAM reconstruction, we have implemented it in a module inside the C++ monoSLAM real time application and thus being able to obtain a real time scaled estimation. In the line of real time performance we also introduce changes in the algorithm to reduce the delay in the update of the scaled trajectory. Besides this, we have also included a condition to check the truth of the estimated step frequency and thus, being able to detect situations or zones, like stairs, where the used walking empirical model is not correct. This memory is structured as follows. In Section 2we discuss the Related Work on the determination of the scale of the visual odometry with monocular vision. In Section 3we detail the visual SLAM algorithm for catadioptric cameras. In Section 4we introduce our scaling algorithm. The experimental evaluation of our algorithm is presented in Section 5. Finally, in Section 6we extract the conclusions and discuss the future work. 7 8 Chapter 4 Scaling of the visual odometry Up to here we have introduced and explained the basic visual SLAM algorithm for omnidirectional cameras. However, one problem of using a monocular system is that it is only able to provide an estimation of the scene reconstruction up to a scale factor. Moreover, this drawback is linked to a more critical one. Since in every Visual SLAM approach all the tracked points are lost sooner or later, the scale of the scene is not anchored and shifts along time as old points are lost and new points are initialised. This phenomena, known as scale drift, makes the scale problem go beyond a simple scale ambiguity which can be solved by applying a uniform scale factor. Indeed, variation of the scale involves a great deformation of the final reconstruction in larger scenes. For this reason it is neccesary to provide a method to compute the scale factor periodically along the motion estimation. To solve the scale problem, in this work we propose a method which is performed iteratively on sections of the trajectory estimated by the EKF visual SLAM approach. The final output of our method is a full scaled estimation of the visual odometry. The main assumption and the core of our method is that the SLAM estimation of the visual odometry must register the oscillatory motion of the head during human walking. Thus, although our experiments are focused in SLAM with omnidirectional cameras, its use can be extended to any kind of camera or sensor as long as the unscaled visual odometry estimation registers any oscillatory motion of a part of the human body linked to the step frequency. Despite this is only applicable on humans, we claim its wide utility, since internal odometry measurements, which are very reliable to provide scale information in SLAM with vehicles, are not available in the case of human walking. 4.1 Description of the basic scaling algorithm The basic algorithm of our method to determine the scale can be divided in four steps, which will be treated in detail in next subsections: •Spectral analysis on the SLAM visual odometry for the estimation of the step frequency. •Empirical estimation of the walking speed from the step frequency. •Integration of the walking speed in a particle filter for a recursive estimation of the scale factor. •Scaling of the final visual odometry. 4.1.1 Spectral analysis on SLAM visual odometry In the case of our omnidirectonal camera, the camera frame is oriented with its z-axis pointing approximately to the direction of the normal to the ground plane, so the head vertical oscillation is given by the z-component of the camera position vector. If we split the visual odometry in sections of Ncamera poses, spectral analysis is carried on the data sequence (zk,1, zk,2, ..., zk,N ), where zk,n is the z-component of the n-th camera pose in the section k. 15 4.1. DESCRIPTION OF THE BASIC SCALING ALGORITHM The power spectral density Γdis calculated by applying the Discrete Fourier Transform (DFT) to the data sequence as follows: Γd,k(fm) = 1 FsNk N X n=1 zk,nexp −j2πfm(n−1) Fsk2(4.1) fm=mFs Nm=−N 2, ..., −1,0,1, ..., N 2(4.2) where Fsis the sampling frequency, which in our case is the number of frames per second (fps) of the camera, and fmare the frequencies for which the spectrogram is sampled. The computation of the DFT of discrete signals involves a series of issues which have to be adressed. The most inmediate one is related to the sampling frequency Fsof the camera. As we are interested in extracting an step frequency, Fshas to be large enough to avoid aliasing in the case when the highest admisible step frequency occurs. In the acquired sequences, the sampling frequency of the camera was set to 15 frames per second (i.e.,Fs= 15 Hz), which is greater enough than the f+ st = 3 Hz taken as the upper limit for a feasible human step frequency. The choice of the number of samples Nis also important for the computation of the spectrogram. From de definition of the DFT in 4.1 and 4.2, one can see that the spectrogram is discretized in Nfrequency bins ranging from −Fs 2to Fs 2, so a higher Ninvolves an increased resolution. Also, in any case, Nhas a lower limit given by the minimum number of samples needed to observe at least one oscillation in the less favourable case of the minimum admissible step frequency (taken as f− st = 1 Hz): Nmin =Fs f− st = 15 (4.3) Note that although for DFT computation purposes Nhas to be as highest as possible, from the global point of view of trajectory scaling, a high Ninvolve a less frequent update of the scale factor and a reduced ability to detect changes in the step frequency. This can result in a decreasing accuracy in the computation of the scale factor. Moreover, if interested in real time operation, the time delay to update the scaled trajectory grows linearly with N, since before scaling one section we need to get the new Nunscaled camera poses from the SLAM algorithm. Thus, in summary, for the choice of Nwe must reach a compromise between the resolution of the DFT and the frequency with which the scale factor is updated. Another problem of computing the DFT which may not seem very evident is the creation of new low frequency components which did not exist in the original signal. This phenomena is known as spectral leakage and arises when we work with finite signals. When applying the DFT, the input signal is considered to be one period of an infinite signal. Thus, discontinuties are likely to occur and these discontinuities end up by producing non-sinusoidal components with a low frequency and a high amplitude (Fig. 4.1a). The harmonics in which these components are decomposed are spreaded along all the spectrogram and might end up by masking the searched step frequency (Fig. 4.1c). To solve this problem we preproccess the data sequence (zk,1, zk,2, ..., zk,N )by substracting the first element zk,1from all the elements and filtering with a second order digital filter with a cutoff frequency of fc= 0.3Hz (Fig. 4.1b). This way, the power peak at the real step frequency becomes clearly visible in the spectrogram (Fig. 4.1d). Once the spectrogram of the signal is computed, we extract the maximum peak in the interval of feasible human step frequencies, which are assumed to fall in the range between f− st = 1 Hz and f+ st = 3 Hz. Given the spectrogram Γd,k(fm), the estimated step frequency fst,k is computed as: fst,k = arg max fm∈[f− st,f+ st] Γd,k(fm)(4.4) 4.1.2 Walking speed estimation To estimate the walking speed, we consider the biomedical work by Grieve et al. [11] where a relation between the step frequency (fst,k) and the walking speed (Vwalk,k ) normalized with height (H) is presented: Vwalk,k =αfβ st,kH(4.5) 16 CHAPTER 4. SCALING OF THE VISUAL ODOMETRY 0 200 400 600 −1.25 −1.2 −1.15 −1.1 −1.05 Sample Z component (unscaled) 0 200 400 600 −0.1 −0.05 0 0.05 0.1 Sample Z component (unscaled) (a) (b) 02468 −30 −20 −10 0 10 Frequency (Hz) 10log10(psd) 0 2 4 6 8 −50 −45 −40 −35 −30 −25 Frequency (Hz) 10log10(psd) (c) (d) Figure 4.1: Z-component signal segment (top) and corresponding power spectra in logarithmic scale (bottom) of two instances from the same visual odometry section: (a,c) without preprocessing the input signal and (b,d) with offset elimination and filtering of the input signal. Note how in (b) the power peak at the step frequency (2Hz) is observable and the highest in the interval of feasible step frequencies. Signal segments have been copied three times to make visible the difference in the discontinuty between the two instances. where Vwalk,k is in m/s, fst,k in Hz, Hin m, and αand βare characteristic parameters which differ from one individual to another. Further studies have proved that this direct relationship between walking speed and step frequency responds to a tendence to minimize the metabolic cost of walking [15,29]. In the work by Grieve et al., values of αand βparameters are presented as dependent on the characteristics of each individual and they provide a group equation with the means of the values obtained for the subjects participating in their experiments. For higher accuracy, in this work we have computed our own αand βparameters for the camera operator. We measured the time tiit took the operator to walk a distance s= 100 m at the times per step ∆Tigiven by a metronome ranging from 0.45 to 0.80 seconds in intervals of 0.05 seconds (see Table 4.1). The height of the operator is H= 1.88 m. Normalized walking speeds Vi′and step frequencies fiwere computed from the raw experimental data. Then a power fitting was applied to obtain the values of α= 0.329 and β= 1.534 (Fig. 4.2). 4.1.3 Particle Filter for scale factor tracking Having the walking speed estimate, the scale factor for section kcould be straighforwardly computed by dk=Vwalk,k µV,k , where µV,k is the average adimensional speed of the camera poses in section k. However, given the empirical method for the walking speed estimation and the possible high variability of the SLAM velocity along Nframes, we decide to use a probabilistic filter for the computation of the scale factor. This allows us to introduce an uncertainty to the scale factor and at the same time decrease the 17 4.1. DESCRIPTION OF THE BASIC SCALING ALGORITHM Table 4.1: Experimental data used to compute the empirical Step frequency-Walking speed relationship for the camera operator. ∆Ti[s]ti[s]fi=1 Ti[Hz]Vi′=s tiH1 s 0.45 48.18 2.22 2.08 0.50 55.60 2 1.80 0.55 61.63 1.82 1.62 0.60 74.54 1.67 1.34 0.65 84.42 1.54 1.19 0.70 94.63 1.43 1.06 0.75 104.42 1.33 0.96 0.80 116.06 1.25 0.86 1 1.5 2 2.5 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 Step frequency (Hz) Normalized walking speed (1/s) Fitting Experimental data V H= 0.3291f1.534 step Figure 4.2: Power fitting of the experimental data to compute the relation between walking speed and step frequency (µerr = 0.018,maxerr = 0.04). effect of spurious estimations of the walking speed in the computation of the scale factor. For the design of the probabilistic filter we consider a dynamic system whose state xkis composed by the magnitude of the SLAM velocity VSLAM,k and the decimal logarithm of the scale factor λk= log10(dk). x(L) k="V(L) SLAM,k λ(L) k#(4.6) As it will be detailed in further reasoning in this section, there exist a variety of good reasons to take the logarithm instead of the scale factor directly: •It allows to restrict the scale factor to positive values by using simply a gaussian distribution to model the uncertainty. •Uncertainty is encoded in orders of magnitude, which is more realistic than taking an interval in d with the same upper and lower limits. For example, with no prior knowledge of the scale factor, the chances of it falling between 0.1and 1should be equal to the chances of falling between 1and 10. •Every uncertainties in the model can be modeled by additive gaussian noise. •All the non-linearities of the model are encapsulated in the measurement function. To track the scale factor, a particle filter with Sampling Importance Resampling is designed [10]. We use a particle filter rather than an extended Kalman filter (EKF) so that it can deal with high uncertainty priors of the scale factor which would involve a large linearization error in an EKF approach. Hence the state of the system in each section kis approximated by a set of particles: Sk=n(x(L) k, w(L) k)|L= 1,2, ..., Po(4.7) 18 CHAPTER 4. SCALING OF THE VISUAL ODOMETRY where Pis the number of particles and x(L) kand w(L) kare respectively the state vector and the resampling weight of particle L. The particles are initialised such that the initial values of λ(L) 0are drawn from a Gaussian distribution λ0∼ N(0, σ0), where σ0is a parameter related to the orders of magnitude being scoped out. In the first step of the particle filter, particles are sampled down by a proposal distribution p(xk|xk−1): x(L) k∼p(xk|x(L) k−1)(4.8) In our system the sampling of the proposal distribution includes both the update of the SLAM velocity, which is taken as a control input coming from the visual odometry, and the possible drift in the scale. This is encoded in the following equations: V(L) SLAM,k =µV,k +ν(L)(4.9) λ(L) k=λ(L) k−1+α(L)(4.10) with ν(L)∼ N(0, σV,k)and α(L)∼ N(0, σdrift), and where µV,k and σV,k are the averaged speed and the corresponding standard deviation of the last set of NSLAM camera poses used for spectral analysis, and σdrift is the standard deviation prior of the scale drift between two consecutive sections, which is modelled as Gaussian noise. This initial sampling by the proposal distribution responds to an initial prediction of the state in the step k. After this prediction, the uncertainty of the estimation is reduced by integrating the measurement of the real walking speed Vwalk,k from the spectral analysis routine. To do this, firstly the particles are weighted as follows: w(L) k=p(Vwalk,k|x(L) k)(4.11) where p(Vwalk,k|x(L) k)is the probability density function defined by the measurement model h(xk)and the statistics of the sensor noise. Intuitivelly, this expression means that particles for which the measurement function yields a walking speed consistent with the walking speed measurement will get higher weights. Assuming that the speed estimation is affected by Gaussian noise of zero mean and standard deviation σV walk to be set up empirically, weights are computed as: ω(L) k=p(Vwalk,k|x(L) k) = φ Vwalk,k −h(x(L) k) σV walk !(4.12) where φ(z)is the probability density function of the standard normal distribution and the measurement function h(x(L) k)is given by: h(x(L) k) = V(L) SLAM,k10λ(L) k(4.13) Then weights have to be normalized as follows: ˆω(L) k=ω(L) k P P M=1 ω(M) k (4.14) Finally, the set of particles Skis resampled by drawing Pparticles from a multinomial distribution Mult(P, ˆω(1), ..., ˆω(P))where the probability of drawing a particle (L)is given by its corresponding weight ˆω(L). 4.1.4 Scaling of the trajectory The scale factor to be applied to the camera poses of each section kis obtained by averaging the logarithmic scale values of the particle set Skand undoing the logarithmic change as follows: ¯ λk= P P i=1 λ(i) k P(4.15) 19 4.2. IMPLEMENTATION WITHIN A REAL TIME MONOSLAM FRAMEWORK dk= 10¯ λk(4.16) This scale factor must be applied to the position and velocity of the Ncamera states of section k. To simplify the notation we define a vector Ck(n)which encapsulates all the variables to be scaled: Ck(n) = (rn x, rn y, rn z, vn x, vn y, vn z)n= 1,2, ..., N (4.17) To ensure the continuity in position and velocity, the offset in the initial unscaled pose of the section is eliminated by substracting the last unscaled pose of the previous section from each vector Ck(n). Then the scale factor is applied and the offset is recovered by adding the last scaled point of the previous section. This is encoded by the following recursive equation: ˆ Ck(n) = ˆ Ck−1(N) + dk[Ck(n)−Ck−1(N)] k= 2,3, ... (4.18) ˆ C1(n) = d1C1(n)(4.19) where ˆ Ck(n)is the vector which includes the scaled position and velocity of the camera poses contained in section k. 4.2 Implementation within a real time monoSLAM framework The original state of the art monoSLAM C++ application used in this work uses two threads. The main thread executes the monoSLAM algorithm iteratively from the incoming frames. The second thread is devoted to update the two graphical outputs: the real display, where the original image with the tracked landmarks is shown, and the virtual display, where the estimated map and the camera trajectory are displayed. Drawing functions executed by the second thread are triggered from the main monoSLAM thread. To avoid simultaneous use of shared variables (SLAM state variables are needed also by the drawer to update the displays) a mutual exclusion variable is used. The implementation of our scaling algorithm has been done in a new thread. This way monoSLAM can go on working while the last section of camera poses is being scaled. After each iteration, the main thread stores the last state variables of the camera in a shared buffer. When this buffer is filled (i.e., it contains the states corresponding to the Ncamera poses needed for the espectral analysis), the main thread sends a signal which triggers the scaling thread, which loads the buffer into a variable exclusive for this thread. After executing the scaling algorithm described in the previous sections of this chapter, the scaled trajectory is updated by adding the recently scaled camera poses. To avoid conflicts between the new thread and the original ones, two new mutual exclusion variables have been added: one for the buffer containing the camera state variables, shared by the monoSLAM and the scaling threads, and another one for the scaled trajectory which is shared by the scaling and the drawing threads. As it was breafly introuced in Sec. 4.1.1, one drawback of the described approach is that, although it is able to operate in real time, there will always exist a delay in the update of the scaled estimation. This delay is linked to the time it takes to fill the buffer with the Nstates needed to perform the DFT. For example, given the camera frame rate of 15 fps and assuming N= 200 as the minimum number of camera poses needed to get an accurate estimation of the step frequency, the minimum delay in the update of the scaled visual odometry would be: tmin delay =N Fs =100 f 15 f/s = 13.33s(4.20) We propose to decrease this delay by updating only one fraction of the buffer instead of renewing it completely for each iteration of the scaling algorithm. Thus, the number of poses of each scaled section (except for the first section which has to be Ncompulsorily) will be: Nf=ceil (αN)(4.21) with 0< α ≤1. The only restriction in the choice of αis the time to scale the Nfcamera states to be lower than the time taken to acquire Nfframes. This way the number of camera poses used for the spectral analysis will remain N(by reusing poses from previous sections), while the amount of scaled camera states per section is reduced to Nf. 20 CHAPTER 4. SCALING OF THE VISUAL ODOMETRY 4.3 Check of the spectral power consistency One issue of the estimation of the step frequency by spectral analysis is the possibility of getting false estimations due to the presence of other dominant frequencies in the spectrogram. To try to reject these false estimations we propose improving the basic algorithm by checking that the spectral power of the frequency taken as step frequency is consistent with the typical range of amplitudes of the head oscillation during walking. To do this, first we develop the following general formulation: Let us take a continuous sinusoidal signal: z(t) = Azsin (2πfpt)(4.22) The power of this signal is computed as: ¯ P=1 TpZTp 0 z(t)2dt =1 TpZTp 0 A2 zsin22πt Tpdt =1 2πZ2π 0 A2 zsin2x dx =A2 z 2(4.23) Now let us sample the continuos signal z(t)into a finite time series zn=zn Fswith 1≤n < N = Fs fp. Then, by applying 4.1 and 4.2, we obtain the power spectra Γ (fm)of the signal, which will be zero for every fmexcept for fm=±fp. The power spectra is related to the power of the original signal through the Parseval’s theorem, which states that the energy of a signal is preserved in the frequency domain: Theorem (Parseval): Let Γ (f)be the power spectral density function of one signal z(t). Then we have: Z∞ −∞ Γ (f)df =1 TZT 0 z(t)2dt (4.24) For discrete time signals the Parseval’s theorem becomes: Fs N N 2 X m=−N 2 Γd(fm) = 1 N N X n=1 z2 n(4.25) where Fsis the sampling frequency and N, the number of samples. Applying this theorem to our signal and substituting the right term by the result of 4.23 we obtain: 2Fs NΓd(fp) = ¯ P=A2 z 2(4.26) which encodes a relation between the power of one sinusoidal signal and its spectral power density. Now lets move back to our real problem of the estimation of the step frequency. After estimating the step frequency fst,k with 4.4, the power of the component associated with the head oscillation can be approximated as: ¯ P(fst,k) = 2Fs NΓd(fst,k)(4.27) However, since the signal is not perfect and due to discretization error the power of the head oscillation may be spreaded along the near frequencies. Thus we propose to reformulate the previous equation as: ¯ P(fst,k) = 2 Zfst+∆f fst−∆f Γ (f)df = 2Fs N m+ X m=m− Γd(fm,k)(4.28) with m−= round Nfst−∆f Fsand m+= round Nfst+∆f Fs To validate fst,k as a feasible step frequency we have to check that ¯ P(fst,k)is consistent with the typical range of amplitudes of the head oscillation movement during walking. Thus, first we need some knowledge about the maximum A+ zand a minimum A− zreachable values for Az. Basing on [14], one conservative estimation of such values would be A+ z= 40 mm and A− z= 7.5mm. 21 4.3. CHECK OF THE SPECTRAL POWER CONSISTENCY Also note that, since the power spectral density is computed for the unscaled z-component of the visual odometry, the computed power must be scaled by multiplying it by the square of the current scale factor dk. Thus the condition for the spectral power consistency of the step frequency remains: 1 2A− z 2≤d2 k¯ P(fst,k)≤1 2A+ z 2(4.29) If this condition is not filled the strategy would be to keep the current scale factor dkand skip the weighting and resampling steps. Finally, the complete scaling algorithm presented in this section is sumarized in Algorithm 1 at the end of the chapter. Algorithm 1 Complete Visual Odometry Scaling algorithm Require: Ck,1..N ,Sk−1 Ensure: ˆ Ck,1..Nf,Sk //Notation Ck,n =nth unscaled camera state ˆ Ck,n =nth scaled camera state N= # input camera states Nf= # output/new camera states Sk=Set of particles for the particle filter //////////////////////////////////////////////////// //Algorithm k= 0 [S0] = Initialize particles () while Not end of sequence do k=k+ 1 Wait for new Ck,1..N from monoSLAM [zk,1..N , µV,k, σV,k] = Extract z-component and mean speed (Ck,1..N ) [zk,1..N ] = High Pass Filter (zk,1..N ) [fm,Γd,k] = Spectrogram (zk,1..N ) [fst,k,Γd,k (fst,k)] = Estimate Step Frequency (fm,Γd,k) [Sk] = Sample Proposal Distribution (Sk−1, µV,k, σV,k) if Step frequency power is consistent (Sk,Γd(fst)) then [Vwalk,k] = Walking speed model (fst,k) [Sk] = Weighting and Resampling (Sk, Vwalk,k) [dk] = Compute mean scale factor (Sk) else dk=dk−1 end if if k=1 then hˆ C1,1..N i=Scale Trajectory Section (d1,C1,1..N ) else hˆ Ck,1..Nfi=Scale Trajectory Section dk,Ck,(N−Nf+1)..N  end if end while 22 Chapter 5 Experiments We use a catadioptric omnidirectional camera with a resolution of 1024x768 and a frame rate of 15 fps. This camera is mounted on a helmet carried by a human operator. The dataset used for the experiments contains,firstly, 3outdoor image sequences along the same path of 232 m and taken at three different step frequencies. The Ground Truth step frequency was fixed by a metronome with with 0.01 seconds of resolution. It was set up to 0.70,0.60 and 0.50 seconds per beat for each sequence, which translates in step frequencies of 1.43 Hz, 1.67 Hz and 2Hz, respectively. Secondly, we acquired an indoor sequence whithout metronome to evaluate the scaling of the trajectory under a normal gait condition. The experiments are divided in two parts. In the first part we focus only in the analysis of the accuracy in the estimation of the step frequency and select an optimal length of the data sequence with which the DFT is feeded. In the second part we evaluate the global scaling algorithm and compare the performance with different setups of the tunning variables. 5.1 Spectral analysis for step frequency estimation First, we evaluate the feasability of using spectral analysis to measure the step frequency. As stated in Sec. 4.1.1, visual odometry is divided in sections of Ncamera poses and the DFT is carried out on each section. To compute the DFT we use the FFTW (Fast Fourier Transform West) C library [8]. We compare different section dimensions of N= 100 and N= 200. As the routines of this library perform faster when the length of the data sequence is a power of 2, data sequences are padded with zeros to a length of Npto fill this condition. A greater padding involves an increased resolution of the spectrogram, but it should not provide any improvement in the accuracy of the estimation since no new information is added. Thus, to check this fact, we also compare two zero-padding instances ZP1and ZP2. ZP1corresponds to a padding being Npthe power of 2closest to N. ZP2corresponds to a padding with Np= 1024. In Fig. 5.1 we show the results of the measured step frequency of the three trajectories with four different DFT setups resulting from the combination of the possible choices of Nand Np. It can be observed that taking N= 200 provides more accurate estimations. This is done at the expense of increasing the interval between two consecutive estimations. As expected, it is also shown that a greater zero-padding does not provide any improvement in accuracy. Thus we select a setup of N= 200 data points and the ZP1padding instance to compute the DFT for spectral analysis. 5.2 Scaling of the Visual Odometry 5.2.1 The Ground Truth First of all, to evaluate the performance of our algorithm, we need a Ground Truth with which we can compare the results. We have obtained it from the Google Maps satellite view in the following steps: •Build the walked path in Google Maps with the distance Measurement Tool, saving an image capture of the built path and taking note of the total distance dGMaps in m. 23 5.2. SCALING OF THE VISUAL ODOMETRY •Load the captured image in MATLAB and build a Npt ×2matrix t[px] = [tx,ty]of 2D points by consecutively clicking on key points of the trajectory. •Points of this trajectory are expressed in pixel coordinates. To convert them to meters and obtain the final Ground Truth we apply: tGT [m] = t[px]dGMaps PNpt i=2 p(tx,i −tx,i−1)2+ (ty,i −ty,i−1)2(5.1) To be able to numerically compare the Ground Truth with the visual odometry estimations provided by our algorithm, we need to establish a pointwise mapping between each point in the estimated trajectory and the Ground Truth. However, in our case this is a difficult task since our Ground Truth points lack from synchronized timestamps to relate them with point of the trajectory. To solve this issue we propose the following method: •Since Ground Truth has been defined by segments, first we split these segments in points to obtain a fine discretization. 0 500 1000 1500 2000 2500 3000 3500 1.25 1.3 1.35 1.4 1.45 1.5 1.55 Frame Step frequency (Hz) N=100, ZP1 N=100, ZP2 N=200, ZP1 N=200, ZP2 Ground Truth 0 500 1000 1500 2000 2500 3000 1.6 1.65 1.7 1.75 1.8 1.85 Frame Step frequency (Hz) N=100, ZP1 N=100, ZP2 N=200, ZP1 N=200, ZP2 Ground Truth 0 200 400 600 800 1000 1200 1400 1600 1800 1.8 1.85 1.9 1.95 2 2.05 2.1 2.15 Frame Step frequency (Hz) N=100, ZP1 N=100, ZP2 N=200, ZP1 N=200, ZP2 Ground Truth Figure 5.1: Spectral analysis along the same path at the three step frequencies of 1.43 (top), 1.67 (center) and 2Hz (bottom) with different setups for the computation of the DFT. 24 CHAPTER 5. EXPERIMENTS (a) (b) (c) Figure 5.5: Visual odometry estimations using different approaches on the three trajectories walked at different step frequencies of about 1.43 Hz (a), 1.67 Hz (b) and 2Hz (c). 31 5.4. ANALYSIS OF THE POWER CONSISTENCY CONDITION −70 −60 −50 −40 −30 −20 −10 0 10 20 30 40 −10 0 10 20 30 40 50 60 70 x(m) y(m) Ground Truth Raw Visual Odometry Scaling algorithm Figure 5.6: Scaled visual odometry estimation in an indoor environment with normal gait compared to the raw SLAM estimation. Note that the squared section in the middle has partly recovered its shape, practically inobservable in the raw estimation. Also, the great drift which can be appreciated on the top left branch of the trajectory has been totally corrected. Figure 5.7: Computation time used to perform our approach for the different sections of the indoor sequence. 32 CHAPTER 5. EXPERIMENTS Figure 5.8: Estimated step frequency (top) and power of the head oscillation (bottom) along time for the indoor sequence of images. Frames between 3500 and 4000 correspond to a zone with stairs. Note how the estimated step frequency suddenly drops while the associated power increases. This anormal change in the power can be used to detect a situation where the walking model cannot be applied, and follow to another strategy. 33 5.4. ANALYSIS OF THE POWER CONSISTENCY CONDITION 0 1000 2000 3000 4000 5000 6000 0.5 1 1.5 2 2.5 3 3.5 4 4.5 Frame Scale factor Power consistency check off Power consistency check on (a) −70 −60 −50 −40 −30 −20 −10 0 10 20 30 40 −10 0 10 20 30 40 50 60 70 x(m) y(m) Ground Truth Raw Visual Odometry Power consistency check off Power consistency check on (b) Figure 5.9: (a) Scale factor and (b) scaled visual odometry with(red) and without(blue) checking the consistency of the step frequency. 34 Chapter 6 Conclusions and Future Work In this work we have presented a novel approach to estimate the true scaled visual odometry of a headmounted omnidirectional camera without need of additional sensors. The general idea behind our method is to take advantage of the head vertical movement registered in the unscaled visual odometry from the SLAM algorithm to obtain the step frequency. Given the step frequency and assuming a human walking model we can compute a real estimation of the walking speed, from which finally we get an scale factor. To improve the accuracy and correct the scale drift of the raw visual odometry, the scale factor is computed for sections of camera states using a particle filter to reliably update it. The algorithm has been validated experimentally obtaining very satisfactory results. The improvement respect to the raw estimation is clearly noticeable and it has proved to be able to correct a large amount of scale drift present in the visual odometry estimation from an indoor environment. Also its low computational cost and its capability to be executed concurrently without interfering with the main SLAM algorithm made it possible its implementation in the framework of a real-time monoSLAM application. From our point of view the main contribution of this approach is that, while there exist methods which can accurately determine the scale for cameras mounted on wheeled vehicles, to the best of our knowledge there does not exist any method which does so with wearable cameras. As future work we will explore the open possibility of making more use of the information provided by the power spectrum of the head oscillation to detect special situations such as stopping, sudden speed variation, stairs and develop strategies to cope with them. For that purpose we will acquire new sequences of images where these situations arise. 35 36 Appendix A The Extended Kalman Filter The Extended Kalman Filter is a recursive estimator based on dynamic systems discretized in the time domain. The EKF aims to estimate the internal state of a system or process given only a sequence of noisy observations and, optionally, control inputs. To perform this estimation we have to define a state transition model and a measurement model. The state transition model describes the evolution of the system from time k−1to time k, and it is defined by the following equation: xk=f(xk−1,uk) + wk(A.1) where f(·)is the state transition function , xk−1is the past state of the system, ukis the control input and wkis the process noise modeled as zero mean uncorrelated gaussian noise with covariance Qk. The measurement model is defined as follows: zk=h(xk) + vk(A.2) where h(·)is the measurement function, xkis the state of the system and vkis the additive observation error modeled as zero mean uncorrelated gaussian noise with covariance Rk In an Extended Kalman Filter the state of the system is represented by a mean vector ˆ xkand a covariance matrix Pk. At each iteration the next state estimate is performed in two steps: Prediction and Update. In the prediction, a state estimate in the current timestep is produced from the state estimate in the previous time step and a control input by using the state transition model. This is encoded in the following equations: ˆ xk|k−1=f(ˆ xk−1|k−1,uk)(A.3) Pk|k−1=Fk−1Pk−1|k−1Fk−1 T+Qk(A.4) where fis the state transition function, ˆ xk|k−1and Pk|k−1are the state mean and covariance estimates at timestep kfrom measurements until timestep k−1and Fk−1is the jacobian of the state transition function: Fk−1=∂f(x,u) ∂x|xk−1|k−1 In the update step the initial prediction is refined by including the measurements zktaken in the current timestep. To do that, first it is computed the innovation of the measurement as the difference between the real measurements zkprovided by the sensors and the prediction of the measurements h(xk|k−1)given by the measurement model. This innovation has an associated covariance Skwhich encodes both the propagation of the state uncertainty through the measurement model and the possible measurement errors. νk=zk−h(ˆ xk|k−1)(A.5) Sk=HkPk|k−1Hk T+Rk(A.6) 37 where Hkis the jacobian of the measurement function: Hk=∂h(x) ∂x|xk|k−1 Next it is computed the Kalman gain Wk, which intutivelly speaking points how much we can trust in the new measurements to update the initial state prediction. This gain is used to weight the innovation when computing the final state mean and covariance in timestep k. Wk=Pk|k−1Hk TSk−1(A.7) xk|k=ˆ xk|k−1+Wkνk(A.8) Pk|k=Pk|k−1−WkSkWk T(A.9) 38 Appendix B The Sphere Camera Model The Sphere Camera Model is a unified projection model valid for every central catadioptric system, i.e. a system with a unique projection center. This model was developed by Geyer et al. [9] and extended by Barreto et al. [1]. The model takes as the origin of the reference system O, the origin of the central system which is modeled (one focus of the hyperbola/parabola in the case of hiper/para-catadioptric systems or the optical center of the camera in the case of a perspective conventional camera). Then they define a unit sphere S centered on the origin of the reference system and a point CP= (0,0,−ξ)Tknown as virtual projection center. The information about the mirror is encapsulated in the characteristic parameters ξand ψ. The parameter ξis defined as the distance between Oand CPand it encodes the kind of system being modeled and its geometry. So, ξ= 0 for perspective cameras, ξ= 1 for para-catadioptric systems and 0< ξ < 1for hiper-catadioptric systems. Table B.1 shows the values of ξand ψfor every kind of system as a function of the the distance between the focus dand the latus rectum 4p. Taking a 3D point expressed in homogeneus coordinates Xw= [x, y, z, 1], its projection on the image is divided in the following steps (Fig. B.1 and Fig. B.2): 1) Point Xwis mapped into a projective ray xin the camera reference frame. This is done by P, a conventional projection matrix x=PXw. 2) The ray xis projected onto the unit sphere centered in the origin O. The intersection point is projected to a virtual projection plane πthrough the virtual projection center CPyielding the point x′. This step is coded by the non-linear function ~: x′=~(x) =   x y z+ξpx2+y2+z2 (B.1) 3) The virtual plane πis transformed in the image plane πIM through a homographic transformation Hc x′′ =Hcx′(B.2) Hc=KcRMc(B.3) KC=  fx0u0 0fyv0 0 0 1  (B.4) MC=  ψ−ξ0 0 0ξ−ψ0 0 0 1 = −η0 0 0η0 0 0 1 (B.5) 39 ξ ψ Espejo parab´ olico 1 1 + 2p Espejo hiperb´ olico d √d2+4p2 d+2p √d2+4p2 Espejo el´ ıptico d √d2+4p2 d−2p √d2+4p2 C´ amara perspectiva 0 1 Table B.1: Characteristic parameters of the spherical camera Model [1] Figure B.1: Projection of a 3DXwpoint onto the image plane with the spherical camera model where KCincludes the camera intrinsic parameters, MCincludes the mirror parameters [9] and Ris the rotation matrix between camera and mirror. By assuming a pin-hole camera model and R=I, the transformation HCyields: HC=  ηf 0u0 0ηf v0 0 0 1  =  γ0u0 0γ v0 0 0 1  (B.6) where γ=ηf is the generalized focal lenght of the camera-mirror system with ηa mirror parameter and f the focal length of the camera. 4) Finally image coordinates are calculated by dividing x′′ by its z′′ coordinate: p=  u v 1 =fu(x′′) =    x′′ z′′ y′′ z′′ z′′ z′′   (B.7) With this model it is also possible to estimate the 3D ray from where the image point comes. That projection is named the inverse projection model. It starts with the point in image coordinates p= (u, v)T, being x′′ = (u, v, 1)T. The equations of the inverse projection model are: x′=Hc−1x′′ (B.8) x=~−1(x′) =   x′ y′ z′−ξ(x′2+y′2+z′2) ξz′2+χ  (B.9) where χ=p(1 −ξ2)(x′2+y′2+z′2) 40 Bibliography [1] J. Barreto and H. Araujo. Issues on the geometry of central catadioptric image formation. In: Computer Vision and Pattern Recognition (CVPR), pp. 422–427, 2001. [2] J. Civera, A. J. Davison, and J. M. M. Montiel. Inverse depth parametrization for monocular slam. IEEE Transactions on Robotics, 24(5):932–945, 2008. [3] J. Civera, O. G. Grasa, A. J. Davison, and J. M. M. Montiel. 1-Point RANSAC for EKF Filtering: application to real-time structure from motion and visual odometry. Journal of Field Robotics, 27(5):609–631, 2010. [4] P. Corke, D. Strelow, and S. Singh. Omnidirectional visual odometry for a planetary rover. In: Intelligent Robots and Systems (IROS), 4:pp. 4007-4012, 2004. [5] S. Cumani, A. Denasi, A. Guiducci, and G. Quaglia. Integrating monocular vision and odometry for slam. WSEAS Transactions on Computers, 3:625–630, 2004. [6] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse. Monoslam: Real-time single camera slam. IEEE Trans. Pattern Anal. Mach. Intell., 29:1052–1067, 2007. [7] A. Eudes, M. Lhuillier, S. Naudet-Collette, and M. Dhome. Fast odometry integration in local bundle adjustment-based visual slam. In: International Conference on Pattern Recognition (ICPR), volume 0, pages 290–293, 2010. [8] M. Frigo and S. G. Johnson. The design and implementation of FFTW3. Proceedings of the IEEE, 93(2):216–231, 2005. Special issue on “Program Generation, Optimization, and Platform Adaptation”. [9] C. Geyer and K. Daniilidis. A unifying theory for central panoramic systems and practical applications. In European Conference on Conputer Vision (ECCV) (2), pp. 445–461, 2000. [10] N. J. Gordon, D. J. Salmond, and A. F. M. Smith. Novel approach to nonlinear/non-Gaussian Bayesian state estimation. Radar and Signal Processing, IEE Proceedings F , 140(2):107–113, Apr. 1993. [11] D. Grieve and R. J. Gear. The relationships between length of stride, step frequency, time of swing and speed of walking for children and adults. Ergonomics, 5(9):379–399, 1966. [12] D. Gutierrez, A. Rituerto, J. M. M. Montiel, and J. J. Guerrero. Adapting a real-time monocular visual slam from conventional to omnidirectional cameras. In 11th OMNIVIS, held with International Conference on Computer Vision (ICCV), 2011. [13] D. Guti´ errez-G´ omez. Localizaci´ on por visi´ on omnidireccional para asistencia personal. Proyecto Fin de Carrera, Universidad de Zaragoza, 2011. [14] E. Hirasaki, S. T. Moore, T. Raphan, and B. Cohen. Effects of walking velocity on vertical head and body movements during locomotion. Experimental Brain Research, 127(2):117–130, 1999. [15] A. D. Kuo. A simple model of bipedal walking predicts the preferred speed-step length relationship. Journal of Biomechanical Engineering, 123:264–269, 2001. 47 BIBLIOGRAPHY [16] P. Lothe, S. Bourgeois, E. Royer, M. Dhome, and S. Naudet-Collette. Real-time vehicle global localisation with a single camera in dense urban areas: Exploitation of coarse 3d city models. In: Computer Vision and Pattern Recognition (CVPR), pp. 863–870, 2010. [17] T. Lupton and S. Sukkarieh. Removing scale biases and ambiguity from 6dof monocular slam using inertial. In: International Conference on Robotics and Automation (ICRA), pp. 3698–3703. IEEE, 2008. [18] C. Mei. Laser-Augmented Omnidirectional Vision for 3D Localisation and Mapping. PhD thesis, INRIA Sophia Antipolis, Project-team ARobAS, 2007. [19] C. Mei, G. Sibley, M. Cummins, P. Newman, and I. Reid. Rslam: A system for large-scale mapping in constant-time using stereo. International Journal of Computer Vision, 94(2):198–214, 2010. [20] A. C. Murillo, D. Guti´ errez-G´ omez, A. Rituerto, L. Puig, and J. J. Guerrero. Wearable omnidirectional vision system for personal localization and guidance. In: 2nd IEEE Workshop on Egocentric (First- Person) Vision, held with CVPR, 2012. [21] D. Nist´ er, O. Naroditsky, and J. Bergen. Visual odometry for ground vehicle applications. Journal of Field Robotics, 23:3–20, 2006. [22] G. N¨ utzi, S. Weiss, D. Scaramuzza, and R. Siegwart. Fusion of imu and vision for absolute scale estimation in monocular slam. Journal of Intelligent Robotic Systems, 61(1-4):287–299, 2010. [23] L. M. Paz, P. Pini´ es, J. D. Tard´ os, and J. Neira. Large scale 6dof slam with stereo-in-hand. IEEE Transactions on Robotics, 24(5):946–957, 2008. [24] A. Rituerto, L. Puig, and J. J. Guerrero. Visual slam with an omnidirectional camera. In: International Conference on Pattern Recognition (ICPR), pp. 348–351, 2010. [25] D. Scaramuzza, F. Fraundorfer, M. Pollefeys, and R. Siegwart. Absolute scale in structure from motion from a single vehicle mounted camera by exploiting nonholonomic constraints. International Conference on Computer Vision (ICCV), pp. 1413–1419, 2009. [26] D. Scaramuzza, F. Fraundorfer, and R. Siegwart. Real-time monocular visual odometry for onroad vehicles with 1-point ransac. In: International Conference on Robotics and Automation (ICRA), pp. 4293–4299, 2009. [27] H. Strasdat, J. M. M. Montiel, and A. Davison. Scale drift-aware large scale monocular slam. In: Robotics: Science and Systems (RSS), 2010. [28] J.-P. Tardif, Y. Pavlidis, and K. Daniilidis. Monocular visual odometry in urban environments using an omnidirectional camera. In: Intelligent Robots and Systems (IROS), pp. 2531-2538, 2008. [29] M. Zarrugh, F. Todd, and H. Ralston. Optimization of energy expenditure during level walking. European Journal of Applied Physiology and Occupational Physiology, 33:293–306, 1974. 48