SLAM visual con cámaras RGB-D basado en optimización de grafo de poses

ESCUELA TÉCNICA SUPERIOR DE INGENIERÍA INFORMÁTICA GRADO EN INGENIERÍA INFORMÁTICA SLAM VISUAL CON CÁMARAS RGB-D BASADO EN OPTIMIZACIÓN DE GRAFO DE POSES VISUAL SLAM WITH RGB-D CAMERAS BASED ON POSE GRAPH OPTIMIZATION Realizado por David Zúñiga Noël Tutorizado por Javier González Jiménez y José Raúl Ruiz Sarmiento Departamento Ingeniería de Sistemas y Automática UNIVERSIDAD DE MÁLAGA MÁLAGA, SEPTIEMBRE 2016 Fecha defensa: El Secretario del Tribunal Resumen: En este trabajo abordamos el problema de localización y mapeo simultáneo (SLAM) utilizando únicamente información obtenida mediante una cámara RGB-D. El objetivo principal es desarrollar un sistema SLAM capaz de estimar la trayectoria completa del sensor y generar una representación 3D consistente del entorno en tiempo real. Para lograr este objetivo, el sistema se basa en un método de estimación del movimiento del sensor a partir de información de profundidad densa y en técnicas de reconocimiento de lugares a partir de características visuales. A partir de estos algoritmos, se extraen restricciones espaciales entre fotogramas cuidadosamente seleccionados. Con estas restricciones espaciales se construye un grafo de poses, empleado para inferir la trayectoria más verosímil. El sistema se ha diseñado para ejecutarse en dos hilos paralelos: uno para el seguimiento y el otro para la construcción de la representación consistente. El sistema se evalúa en conjuntos de datos públicamente accesible, alcanzando una precisión comparable a sistemas de SLAM del estado del arte. Además, el hilo de seguimiento se ejecuta a una frecuencia de 60 Hz en un ordenador portátil de prestaciones modestas. También se realizan pruebas en situaciones más realistas, procesando observaciones adquiridas mientras se movía el sensor por dos entornos de interiores distintos. Palabras claves: SLAM, RGB-D, tiempo real, grafo de poses, Robótica, Visión por Computador. Abstract: In this work, we address the Simultaneous Localization And Mapping (SLAM) problem using only an RGB-D camera. The main purpose is to develop a SLAM system capable of estimate the full sensor's trajectory and generate a globally consistent 3D reconstruction of the environment in real time. To achieve this goal, we rely on a dense motion estimation algorithm and on a feature based place recognition technique to derive spatial constraints between selected frames from a sequence. All computed spatial constraints are merged into a graph of poses, used to infer the most likely trajectory. The system is designed to run in two parallel threads: one for tracking and the other for mapping. The resulting system is evaluated on a publicly available benchmark for SLAM systems, reaching an accuracy comparable to state-of-the-art SLAM systems in the estimation of the sensor's trajectory. Moreover, the tracking thread runs at 60 Hz on a modest laptop. Also, we test the system in real settings, processing observations acquired while moving the sensor in two different indoor environments. Keywords: SLAM, RGB-D, real-time, pose graph, Robotics, Computer Vision. Contents 1 Introducction 1 1.1 Motivation.................................... 2 1.2 Goals....................................... 3 1.3 RelatedWork .................................. 4 1.4 Resources .................................... 5 1.5 Documentoutline ................................ 6 2 System Overview 7 2.1 Front-end..................................... 8 2.2 Back-end..................................... 18 3 Experimental Evaluation 21 3.1 VisualOdometry................................. 22 3.2 Loop Closure Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 3.3 TheWholeSystem................................ 26 4 Conclusions and Future Work 31 Bibliography 35 Chapter 1 Introducction Robots have been successfully applied to industrial manufacturing, working in structured environments and releasing us from repetitive tasks. By now, robots are also taking place in our everyday life, working in uncontrolled environments and helping us in tasks that require highlevel abilities to be completed. The so-called service robots aim to work in a priori unknown environments, with the purpose of improving our quality of life. For instance, TPR-Robina is a robot that guide tours in a museum, and it is also possible to acquire domestic robots to help us keep our houses clean, like Roomba does, which was designed to vacuum an entire level of a facility (see Figure 1.1). In order to provide services and perform autonomously, robots usually need a representation of their working environment: for example guiding robots need to know where they are, and home cleaning robots need to know where they have not cleaned yet. These representations (a) TPR-Robina (b) Roomba 960 Figure 1.1: TRP-Robina museum guide robot (1.2 m height), by Toyota, acquires a new map whenever the environment changes. Roomba home cleaning robot (0.35 m diameter), by iRobot, builds a map of the working environment so it does not lose track of visited places. 1 2.1 Front-end A front-end abstracts sensor measurements for maximum-a-posteriori estimation. In this work, we use a single RGB-D camera as sensor, and the input measurements are represented as two functions: I: Ω →N(2.1) Z: Ω →R(2.2) where Iis the intensity value function and Zis the depth value function for each image pixel in the image domain Ω⊂N2(typically Ω = 640 ×480). The intensity function is computed from the R,Gand, Bcomponents following the Rec. 601 [21]: I(p)=0.299R(p)+0.587G(p)+0.114B(p)(2.3) The front-end component outputs selected frames represented as graph nodes, i.e. state variables subject to future optimization, and constraints between nodes represented as graph edges. In this work, we will consider only spatial constraints from: 1. Odometry, and 2. Loop Closure The first ones constrain two consecutive keyframes, while the second ones constrain two keyframes, not necessarily consecutive, observing the same place. These spatial constraints, as well as state variables, are represented as elements in the three-dimensional Special Euclidean Group SE(3). Keyframe Selection To reduce the number of loop closure candidates and the number of state variables, only a subset of selected frames are considered, namely keyframes. Several techniques for keyframe selection exist, and probably the ones based on visual overlap are the most widely extended. For instance, in [17, 32] the same features are employed for motion estimation are reused for keyframe selection, and whenever the number of tracked features from the last keyframe fall below a given threshold, a new keyframe is added. The key idea behind this approach is that the number of tracked features are progressively fewer as the camera moves, and therefore the density of keyframes is adapted to the camera motion. In [23], they propose a keyframe selection criterion based directly on the uncertainty of the camera motion estimate, using an entropy measure. This measure encapsulates the uncertainty from the covariance matrix into a scalar value. They add a new keyframe whenever the entropy ratio is below a predefined threshold. The entropy ratio is computed between the uncertainty of the motion from last keyframe and of the first captured motion from the last keyframe (between the last keyframe and the immediately following frame from it). 8 Figure 2.2: A 9 px segment test corner in a circle of 16 px length, by Edward Rosten. In this work, we follow an appearance-based keyframe selection criterion over a Bag of Words model, trying to negatively affect as less as possible the loop closure detection algorithm. Within this approach, the whole image is described using a global descriptor build from local image feature descriptors. The similarity between two images can be then computed from a simple vector norm. Bag of Words techniques are taken from text retrieval in document processing, and used in computer vision as object or scene retrieval [46], often called Bag of Visual Words. The general idea consists in building a properly weighted histogram (or a BoW vector) over a finite number of keywords w: the vocabulary. In computer vision, instead of keywords, image feature descriptors are used. Each extracted local descriptor is matched to a visual word from the vocabulary to computed the histogram, which acts as global scene descriptor represented as a w-dimensional vector. Here, we rely on FAST [40] features and BRIEF [7] binary descriptor, as they have shown good performance in scene retrieval [11] and feature matching for small displacements [16], and are orders of magnitude faster than SIFT [26] or SURF [2] methods. Features from Accelerated Segment Test (FAST) [40] is a high-speed corner detector from the segment test criterion. A pixel candidate p∈Ωis considered to be a corner if the Bresenham circle around it contains exactly nconsecutive pixels, all brighter or all darker than the candidate pixel (see Figure 2.2). Brighter and darker pixels are defined as: Sbright ={x∈Ωp|I(x)> I(p) + t}(2.4) Sdark ={x∈Ωp|I(x)< I(p)−t}(2.5) for a given threshold t, where Ωpdenotes the circle pixels around p. Within this definition, most non-corner pixels can be excluded before completing the full segment test, speeding up the computation time required for the corner detection. To avoid the detection of multiple adjacent keypoints for the same corner, a non-maximal suppression step is applied to discard redundant detections. For this purpose, an efficient score 9 Figure 2.3: Spatial arrangement of the close binary tests used with BRIEF descriptor. function is computed for each detected feature: V(p) = max X x∈Sbright |I(x)−I(p)| − t, X x∈Sdark |I(p)−I(x)| − t(2.6) keeping only local maxima keypoints. Binary Robust Independent Elementary Features (BRIEF) [7] is a simple binary descriptor of a local patch around a detected feature. Given a keypoint p∈Ω, the local descriptor is computed using a intensity difference test over a predefined sequence of sampling points, generating a binary string b(p)of length L. The test function is defined as τ(x,y) = (1if I(x)< I(y) 0otherwise (2.7) where xand yare a pair of sampling points. The sequence of sampling points is generated randomly from the normal distributions [10]: xi∼ N(0,1 25S2)(2.8) yi∼ N(xi,4 625S2)(2.9) where Sis the side length of the local square patch centered at p. To reduce the effect of image noise, patches are smoothed using a Gaussian kernel before computing the tests. Finally, the binary string is generated as follows: b(p) = L X i=1 2i−1τ(p+xi,p+yi)(2.10) From these features, the vocabulary for the Bag of Words model is generated by a hierarchical quantization of the descriptor space into wwords [35]. Training features are partitioned into kclusters by k-means [1] algorithm, and this procedure is repeated for each new cluster up to dlevels, yielding a vocabulary tree with w=kdleaves. 10 Each word is weighted according to its inverse document frequency idf(i) = log N Ni (2.11) where Nis the number of training images, and Nithe number of occurrences of the word i in these images. This metric downweights very frequent words in the training images, being less discriminative. To convert an image I(represented by its BRIEF description from FAST features) into a BoW vector v∈Rw, its binary descriptors traverse the tree (from root to leaves) by selecting at each level the immediate descendant node that minimizes the Hamming distance: d(a,b) = X i ai⊕bi(2.12) where ⊕is the XOR binary operator. In addition, the term frequency for each word in the image Iis computed as tf(i, I) = ni nI (2.13) where nistands for the number of occurrences of word iin image Iand nIfor the total number of words in I. This weighting gives more importance to frequent words in a particular image, as they describe it well. The resulting vector for an image Iis v=   w1 . . . ww    where wi=tf(i, I)idf(i)is the term frequency–inverse document frequency (tf-idf ) as proposed in [46]. The similarity between two BoW vectors v1and v2is computed as a L1-score [11]: s(v1,v2)=1−1 2 v1 |v1|−v2 |v2|(2.14) whose value lies in [0,1]. The range in which these scores varies depends on the query image and the visual words it contains. In order to directly compare similarity scores an additional normalization step is performed. The normalized similarity score ηbetween two BoW vectors viand vjis defined as [11]: η(vi,vj) = s(vi,vj) s(vi,vi−1)(2.15) where s(vi,vi−1)is an approximation of the maximum expected similarity for vi: the similarity with the immediate previous frame vi−1. Within this approach, a frame iis considered to be a keyframe if the normalized similarity score η(vi,vj)with the last keyframe jis less than a given threshold β. 11 Visual Odometry The main purpose of a visual odometry algorithm is to estimate the relative sensor motion between two consecutive frames relying only on visual input data [34]. The whole trajectory is then calculated incrementally, composing all relative motions and starting from a reference frame (usually, but not necessarly, the first one). A typical feature-based visual odometry method processes a sequence of images as follows [8]: 1. Feature Detection 2. Feature Matching 3. Motion Estimation The feature detection step selects distinctive and repeatable interesting points (2D image points or 3D shape points) from the two consecutive observations. The extracted keypoints are then described locally and matched from one image to the other in a high-dimensional space (the descriptor space). Finally, from the set of corresponding points, the sensor motion is computed. In contrast to that, dense visual odometry methods don’t perform any feature extraction or matching step, instead, the motion is estimated by minimizing some general error function. For instance, ICP [3] algorithm (or its variants [42]) aligns two 3D point clouds by corresponding each point from one cloud to the closest in the other cloud and minimizing the Euclidean distance between correspondences. The accuracy of ICP heavily depends on the initial alignment, while, in general, feature-based methods are more robust to large motions. Therefore, ICP-like algorithms are often used to refine a feature-based alignment [17]. An interesting variant of ICP for indoor environments could be Generalized-ICP [44], as it can be formulated with a plane-to-plane error metric, and then it can cope with larger motions. Instead, in this work we use an efficient alternative to GICP: Differential Odometry (DIFODO), as it has shown better execution time as well as accuracy [22]. DIFODO algorithm takes a depth map Zas input and computes a motion estimation between consecutive depth observations from the average linear and angular camera velocities during the time interval elapsed (usually 30 Hz). The sensor velocities are derived by applying the range flow constraint equation [47] ˙ Z=∂Z ∂t +∂Z ∂u ˙u+∂Z ∂v ˙v+O( ˙u, ˙v, ∆t)(2.16) to each pixel p= (u, v)in Ω, where ˙ Zrepresents the depth map derivative with respect to time t, and ˙p = ( ˙u, ˙v)the optical flow [19]. The three partial derivatives of Zin Eq. (2.16) can be directly computed from the consecutive depth images. And ˙ Z,˙uand ˙vcan be expressed in terms of the camera velocities 12 Figure 2.4: Pin-hole camera model, by Mariano Jaimez ξ= (vx, vy, vz, ωx, ωy, ωz)>under a static world assumption: ˙ P=   ˙x ˙y ˙z   =   −vx−zωy+yωz −vy+zωx−xωz −vz−yωx+xωy   (2.17) meaning that every 3D point Pmoves with the same velocities as the sensor, but with opposite directions. Ignoring the higher order terms O( ˙u, ˙v, ∆t), Eq. (2.16) becomes ˙ Z≃Zt+Zu˙u+Zv˙v(2.18) where Zt,Zuand Zvare, for simplicity of notation, the partial derivatives of Z. Rearranging terms and replacing the depth map ˙ Zby the depth coordinate ˙zsince Z(u, v) = z, yields: −Zt=−˙z+Zu˙u+Zv˙v(2.19) From the pin-hole camera model, and assuming that the pixel coordinates of a 3D point P are time-varying (see Figure 2.4), we get: u=fx x z+uc⇒˙u=fx˙xz −˙zx z2(2.20) v=fy y z+vc⇒˙v=fy˙yz −˙zy z2(2.21) 13 where (uc, vc)is the principal point (image center) and fxand fythe focal length values, in pixels. Expressing the optical flow in terms of ˙ Pin Eq. (2.19): −Zt=−˙z+Zufx˙xz −˙zx z2+Zvfy˙yz −˙zy z2(2.22) and applying the static world assumption yields: −Zt=1 + xfx z2Zu+yfy z2Zv(vz+yωx−xωy) +fx zZu(−vx+yωz−zωy) + fy zZv(−vy−xωz+zωx) (2.23) The linearization in Eq. (2.18) holds for small motions or if observed points belong to local planar patches, since the higher order terms are negligible. To derive the velocity constraints, a system of linear equations is built from Eq. (2.23). At least, six linearly independent restrictions are required to solve the algebraic system. However, in practice a higher number of points are considered, leading to an over-determined linear system solved by weighted least squares in closed form. In order to cope with motions larger than a single pixel, a coarse-to-fine scheme is used [5] to compute the optical flow. Within this strategy, a Gaussian pyramid is built by iteratively downsampling and filtering the depth image, allowing to capture larger displacements. The optical flow is solved from coarser to finer levels and at each level, the previous solution is used to warp one image against the other at the same level, leading to image pairs presenting less displacement than the original pair for which the assumption of small motions holds. Loop Closure Detection The errors in relative motion estimation that arise from incremental frame-to-frame alignment accumulate over time and eventually yield to significant drift errors. These drifts causes inconsistencies in the map estimation, as a single region may have multiple representations. Aloop closure occurs when a previously seen place is revisited, providing additional constraints that can be used to correct the accumulated drift. Therefore, it is necessary to: 1. Detect loop closures between two observations, and 2. Compute the spatial constraint that relates the sensor poses Most of the approaches to visual loop closure detection are based on the Bag of Visual Words model, but there are other alternatives. For instance, the similarity between two images can be computed from the Locality Sensitive Hashing of the image descriptors [45], avoiding the use of a predefined vocabulary. Instead of local descriptors, a global descriptor can be learned using deep Convolutional Neural Networks to describe the whole image [20]. In this work we use a slightly modified version of the Bag of Visual Words approach, using local binary descriptors [11], that has shown to be more efficient than the other alternatives. 14 The same Bag of Words model for keyframe selection is reused1for loop closure detection, adding an image database of previous observations in order to detect revisited places. The general idea is to find the best match between the current keyframe and all previous keyframes stored in the database. To speed-up this search, an inverse index is maintained along with the database to retrieve the images that contain a given word. The inverse index allows to compare the current keyframe only against images that share some words in common. The last Kkeyframes are excluded from the loop closure candidates as, even having high similarity scores, they don’t constitute a true loop closure. A loop closure candidate jis considered for a given keyframe iif the normalized similarity score η(vi,vj)between their BoW vectors vjand viis greater than a given threshold α. Only the best scoring candidate is taken into account for future checks. However, if the expected score s(vi,vi−1)is low enough (e.g from fast sensor movements), the normalization step can result in erroneously high similarity values. Therefore, keyframes with low expected scores are discarded from the loop closure detection. An additional geometric verification of the feature’s distribution in the two images is necessary to discard incorrect visually similar loop closure candidates, as introducing false loop closure constraints could lead to even worse errors. In this work, we consider the 3D spatial distribution of the features, as opposed to the 2D distribution proposed in the original approach [11]. A geometric check usually involves the following steps: 1. Feature Matching 2. Outlier Rejection 3. Validation The feature matching step is only a first guess and it usually contains incorrect matches, which are detected and discarded in the outlier rejection step. Finally, the resulting matches are accepted only if the error of the spatial distribution is acceptable. The feature matching is performed using the nearest neighbor distance ratio [26] policy. A match between a feature from an image and its nearest neighbor in the descriptor space (namely the closest in Hamming distance) on the other image is accepted if the ratio d(a,c) d(a,b)(2.24) is lower than a threshold, where band ccorrespond to the closest and second-closest matches in one image, respectively, for a feature ain the other image, and dis the distance function in the descriptor space (the Hamming distance for binary descriptors). This gives us a set of matches {ai↔bi}between local descriptors on both images. The exact nearest neighbor search has computational complexity of Θ(n2)in the number of features, but faster approximations can be used. In this case, the words (or intermediate 1Actually, the BoW approach described was originally developed for loop closure detection in [11] and reused here for keyframe selection afterwards, so the BoW model was reused for keyframe selection. 15 nodes) of the vocabulary reduces the search space for nearest neighbors [11], speeding-up the feature matching process in one hand but sacrificing the exact result in the other hand. For this reason, a direct index is maintained with the database, allowing to retrieve features associated to each word (or tree node) for a given image. The nearest neighbor distance ratio ensures that the matches are distinctive enough, so that they are less likely to contain incorrect matches. Unfortunately, outliers can be still present, and they negatively affect the quality of the loop closure detection and thus the quality of the overall estimation, so an outlier rejection step is still necessary. The RANdom SAmple Consensus (RANSAC) is a general model fitting paradigm able to cope with a large proportion of outliers. The algorithm takes a minimum size sample from the observed data to compute a model that fits that sample. The support for the computed model is the consensus set: the observed data within an error threshold from the model. These steps are repeated until a reasonably good model (one that has enough support) is computed, or the maximum number of iterations is achieved. Algorithm 1 Generic RANSAC 1: Select randomly the minimum number of observations required to determine the model parameters. 2: Solve for the parameters of the model. 3: Determine the set of data observations which fit the model within a distance threshold tr. This is the consensus set of the sample and defines the inliers for the initial data. 4: If the number of inliers is greater than some threshold T, re-estimate the model using all inliers and terminate. 5: Otherwise, repeat steps 1–4 a maximum of Ntimes. In this case, the observed data is the match set {ai↔bi}. For each image, we generate a sparse feature point cloud using the per pixel depth information. This point cloud includes only 3D information for the detected features in the intensity image, giving to each feature descriptor aiand bia 3D position xi,yi∈R3. For a pixel p= (u, v)>, its 3D coordinates P= (x, y, z)>can be derived from the pin-hole camera model: z=Z(u, v)(2.25) x= (u−uc)z fx (2.26) y= (v−vc)z fy (2.27) where (uc, vc)is the principal point (image center) and fxand fythe focal length values. From the corresponding 3D points {xi↔yi}, the model is given by a rigid body transformation T∈SE(3) that aligns the two point clouds {xi}and {yi}. A rigid body transformation has a rotation component Rand a translation component t: T="R t 01×31#(2.28) 16 with R∈SO(3) and t∈R3. Here, RANSAC performs both outlier rejection and validation for the geometrical verification, but it also computes the spatial constraint required to complete the loop closure detection procedure. The components Rand tare computed as the solution of a least squares formulation [50], minimizing the error function e(T) = 1 n n X i=1 kyi−Txik2(2.29) in closed form, for a set of ncorresponding points. The rotation component is computed from the Singular Value Decomposition (SVD) of the covariance matrix of X={x1,x2,· · · ,xn}and Y={y1,y2,· · · ,yn}: Σxy =1 n n X i=1 (yi−µy)(xi−µx)>(2.30) where µx=1 n n X i=1 xi(2.31) µy=1 n n X i=1 yi(2.32) are the mean vectors (or centroids) of Xand Y, respectively. Then, Σxy =UDV >is the SVD of Σxy (with D=diag(d1, d2, d3)a diagonal matrix satisfying d1≥d2≥d3≥0). The optimum rotation Rcan be computed as R=USV >(2.33) when rank(Σxy)≥2, where S=(Iif det(Σxy)≥0 diag(1,1,−1) if det(Σxy)<0(2.34) If rank(Σxy)=2, then S=(Iif det(U)det(V) = 1 diag(1,1,−1) if det(U)det(V) = −1(2.35) must be chosen instead of Eq. (2.34). Finally, the translation tthat minimizes the error function is t=µy−Rµx(2.36) for the rotation R. 17 Table 3.2: Performance of DIFODO, in terms of translational and rotational RMSE, for the final parameters and skipping fs consecutive frames. Sequence Translational (m/s) Rotational (deg/s) fs = 0 fs = 1 Fs = 2 Fs = 0 fs = 1 fs = 2 fr1/desk 0.044 0.043 0.078 2.724 3.130 6.034 fr1/desk2 0.051 0.047 0.162 3.454 3.359 3.761 fr1/room 0.049 0.047 0.052 2.787 2.679 2.680 fr2/desk 0.032 0.025 0.023 1.240 0.987 0.906 fr3/office 0.072 0.019 0.018 2.439 1.065 0.969 3.2 Loop Closure Detection For the loop closure detection module, we are interested in the number of correct detections and how this number is affected by the keyframe selection. We also analyze the effect of reducing the sequence length (by applying a keyframe selection crietrion) on the execution time of the loop closure detection algorithm. We run the implementation of FAST from the OpenCV library [36], while for BRIEF, we run the implementation in DLib [15]. For the binary Bag of Words, we rely on the implementation from DBoW2 []. We use the vocabulary generated in [11], available for download2, with w= 106words from the ‘Bovisa 2008-09-01’ session of the Rawseeds project3. We implemented our own RANSAC procedure, based on the SVD transformation estimation implemented in the PCL [38]. We set the maximum distance threshold to 1 cm, the minimum size consensus set to 12 and a maximum of 500 iterations. Loop closure detections can be represented as a binary relation between sequence indices. Here, R(i, j)means that a loop between frames (or keyframes, depending on the case) was detected. The precision and recall metrics, to compare the estimated loop closure relation R with a ground truth loop closure relation S, are defined as4: precision(R, S) = |ΓR∩S| |ΓR|(3.5) recall(R, S) = |ΓR∩S| |ΓS|(3.6) where ΓA={i|i>j+k, (i, j)∈A}(3.7) for a given set Aand the discarding parameter K, used to reject recent frames from the loop closure candidates. 2http://doriangalvez.com/resources/DLoopDetector/resources.tar.gz 3http://www.rawseeds.org/rs/capture_sessions/view/10 4Here we give a formal definition of the precision and recall metrics, as described in [11]. 24 The precision metric is defined as the ratio between the number of correct detections and all the detections fired by the algorithm, and it measures the quality of the detections. For a robust SLAM system, a 100% precision is required, as usually loop closures are unmovable decisions. The recall is defined as the ratio between the correct detections and all loops in the ground truth, and measures the ability of the algorithm to capture the loop closures occurring in a sequence. A higher recall is preferable, as more correction constraints will be available, resulting in a better trajectory estimation. Since there is no loop closure ground-truth information on the dataset used, we derive this information from ground-truth pose information to evaluate the precision of the loop closure detection method. Following a similar approach as in [43], a loop closure detection is verified (or correct) if: 1. ktrans(Tij )k<2 m, and 2. |ang(Tij)|<45 deg where Tij =Q−1 iQjrepresents the relative ground-truth transformation between frames number i, j ∈ {1, . . . , n}in the sequence. Detected loop closures but not automatically verified from the ground-truth information (meaning at least one of the above conditions does not hold) are manually verified, as it is hard to define a procedure sound and complete for this purpose. With these simple and easy implementable conditions most loop closures are automatically verified, so only a few need human supervision. Instead of the true recall metric, we use a relative lower bound recall metric, dividing by the sequence length n, or the number of keyframes considered, if it is the case: recall(R, S) = |ΓR∩S| n(3.8) This definition of recall allows us to measure the trade off between the number of correct loop closures detected and the reduction in the length of the sequence. The number Kof frames discarded from loop closure candidates affects the recall, and depends on the speed of the camera motion. When no keyframes are selected (i.e. all frames are processed), we set K= 100 (a bit more than 3 s for sequences recorded at 30 Hz). For keyframe selection criteria, we set K= 40, as the expected reduction factor of the frames to process is around 2.5. In order to evaluate the accuracy of the loop closure detection with these values for Kfor all sequences, we skip two consecutive frames in the slower sequences (namely fr2/desk and fr3/office) to simulate faster motions. Table 3.3 shows the results form these experiments. In most cases, using a keyframe selection criterion improves the relative lower bound recall metric, meaning that loop closures are preserved to some degree while reducing the total number of frames to process. Figure 3.2 shows the effect of reducing the sequence length in the maximum execution time spent by the loop closure detection algorithm. The maximum execution time is reduced roughly by 25 Table 3.3: Relative lower bound recall performance of the Loop Closure Detection algorithm for different values of the similarity threshold β. Similarity Sequence fr1/desk fr1/desk2 fr1/room fr2/desk fr3/office No Keyframe 1.79% 2.13% 0.68% 1.28% 4.46% β= 0.82.70% 1.59% 1.34% 1.33% 3.88% β= 0.71.91% 2.67% 1.32% 2.17% 3.44% β= 0.61.09% 3.05% 0.76% 1.05% 3.07% fr1/desk fr1/desk2 fr1/room fr2/desk fr3/office 5 10 15 20 25 30 35 No KF 0.8 0.7 0.6 Figure 3.2: Maximum execution time spend on each sequence, for various keyframe selection criteria, in ms. ‘No KF’ means all frames were processed. the same factor as number of frames to process, since the computational complexity for loop closure detection grows linearly with the number of frames in the worst case. For the subsequent experiments, we set β= 0.8, has it has shown the best performance for the relative lower bound recall metric and runs at 60 Hz for a sequence with more than 500 keyframes. 3.3 The Whole System In this section, we compare our system with state-of-the-art SLAM systems, in terms of the Absolute Trajectory Error (ATE) [48] metric. We also evaluate the improvement in the trajectory estimation achieved by optimizing the graph of poses, and how this procedure is affected by the keyframe selection criterion, in execution time and in terms of the ATE metric. This metric measures the accuracy of the resulting estimated trajectory against the groundtruth trajectory, taking into account the global consistency. Each keyframe pose Pi∈SE(3) 26 is assigned to a corresponding ground-truth pose Qi∈SE(3) based on the timestamp values. The 3D corresponding points, obtained from the translation component of each pose, are aligned by the rigid body transformation S∈SE(3), from a least squares solution. The error between corresponding poses is then Fi=Q−1 iSPi(3.9) From these errors, the RMSE is computed as RMSE(F1:n) = v u u t1 n n X i=1 ktrans(Fi)k2(3.10) for the translational component of Fi. Within this metric, we compare our approach with different SLAM systems. More precisely, we consider the DVO-SLAM [23] and RGB-D SLAM [9] systems as they minimize both photometric and geometric errors form RGB-D observations. The former is based only on dense methods, while the latter uses feature-based ones. We also compare it with systems that minimizes only the geometric error following a dense formulation. In this class fall the Kintinuous [51] system: an extension to large scale environments of KinectFusion [39] algorithm, and the CPA-SLAM [30]: a very recent system based on plane segmentation of depth information. The accuracy of the visual odometry algorithm directly affects the accuracy of the whole system. For this reason, we run our system for several frame skip values, as it has shown to be a reliable strategy to improve the trjectory estimation. The results from these experiments are presented in Table 3.4.5Despite the low resolution used and the simplicity of the feature extraction algorithms, our system reaches an accuracy comparable to state-of-the-art SLAM systems (see Figure 3.3). The execution time of the whole system for all sequences is shown in Table 3.5. The tracking thread (visual odometry and keyframe selection) runs at 60 Hz on a single CPU. This performance, for the best of our knowledge, has never been reported in the literature. For the analysis of the back-end module, we summarize in Table 3.6 the improvements achieved in terms of absolute trajectory RMSE in contrast to a visual odometry only trajectory (i.e. without any optimization), along with general information about the number of keyframes and loop closures considered for each sequence, setting fs = 1. In all cases, as one may expect, there is a great improvement in the estimated trajectory. In Figure 3.4 we show the effect of the keyframe selection strategy on the maximum execution time of the graph optimization and on the trajectory estimation, for each sequence. Has the intuition says, the execution time is improved in all cases, while, in general, the error in the estimated trajectory increases when only keyframes are considered. This is mostly due 5The errors for each system were taken form their original publications, as well as form [14]. If there is any contradiction between these values, we take always the smallest one. 27 (a) fr2 desk (b) fr3 office Figure 3.3: The ground-truth and estimated trajectories aligned, for the longest sequences. The absolute trajectory error is computed from these differences. Table 3.4: Absolute Trajectory Error RMSE (m) comparison with state of the art approaches. Frame skip Sequence fr1/desk fr1/desk2 fr1/room fr2/desk fr3/office fs = 0 0.047 0.048 0.068 0.118 0.116 fs = 1 0.039 0.049 0.062 0.064 0.063 fs = 2 0.214 0.205 0.082 0.056 0.055 CPA-SLAM [30] 0.018 0.029 0.055 0.046 0.025 Kintinuous [51] 0.037 0.071 0.075 0.034 0.030 RGB-D SLAM [9] 0.023 0.043 0.084 0.057 0.032 DVO-SLAM [23] 0.021 0.046 0.053 0.017 0.035 to a lower number of loop closures. The execution time was plotted using a logarithmic scale, and it is improved in a greater factor than the additional error induced by a keyframe selection strategy. Finally, we tested the developed system in real-life scenarios, by and RGB-D camera on indoor environments. We run it the MAPIR 6and in a typical living room. The resulting 3D maps are shown in Figure 3.5. In both cases, the system was able to detect loop closures and to correct the accumulated drift, generating consistent representations. For larger environments with few loop closures, the accuracy of the representation can be highly affected, yielding to unacceptable errors. 6http://mapir.isa.uma.es/mapirwebsite/ 28 Table 3.5: Execution time for all components of the SLAM system. Component Execution time (ms) Mean Std Min Max DIFODO 5.80 0.55 0.98 13.82 FAST 1.95 0.51 0.34 5.61 BRIEF 3.12 0.33 2.07 7.34 Keyframe Selection 4.32 0.39 1.76 9.18 Loop Closure Detection 5.23 0.96 2.21 20.09 Graph Optimization 106.11 57.30 17.54 196.31 Table 3.6: Graph optimization vs no graph... . Sequence Opt. w/o Opt. Frames Keyframes Loop Closures fr1/desk 0.039 0.113 559 120 4 fr1/desk2 0.049 0.078 609 131 1 fr1/room 0.062 0.159 1310 284 1 fr2/desk 0.064 0.310 2182 536 11 fr3/office 0.063 0.322 2488 425 17 fr1/desk fr1/desk2 fr1/room fr2/desk fr3/office 100 101 102 103 No KF 0.8 (a) Execution time fr1/desk fr1/desk2 fr1/room fr2/desk fr3/office 0.035 0.04 0.045 0.05 0.055 0.06 0.065 No KF 0.8 (b) Trajectory Error Figure 3.4: Maximum execution time and absolute trajectory RMSE, in ms and m, respectively. Note that the execution time is plotted on a logarithmic scale. 29 (a) Lab (b) Living room Figure 3.5: Globally consistent 3D dense representations of two different indoor environments. 30 Chapter 4 Conclusions and Future Work In this work, we developed a SLAM system that relies solely on information taken from an RGBD camera. The system is able to estimate the full trajectory of the sensor and incrementally build a consistent 3D representation for small indoor environments. Real-time execution is achieved by dividing the execution into two threads: tracking and mapping. The tracking thread, composed of the visual odometry and keyframe selection components, runs at 60 Hz on a modest laptop. The mapping thread runs the remaining components, with higher computational demands. The accuracy of the estimated trajectory by our SLAM system was evaluated on a publicly available benchmark for SLAM systems, and performed comparable to state-of-the-art RGB-D SLAM systems. The specific goals considered for this work were addressed as follows: •Measurement representation: RGB-D observations are represented by sparse feature point clouds, using the FAST detector. •Keyframe selection: Interesting frames are selected based on visual similarity, computed with a binary Bag of Words (DBoW2) over BRIEF descriptions. •Pose graph modeling: Visual odometry constraints are computed from DIFODO poses estimation, while loop closure are detected based on the same Bag of Words as for keyframe selection and the spatial constraints are computed from 3D point correspondences. •Global consistency: Trajectory poses are estimated from all computed constraints, under the g2o framework. Even having accomplished the goals we set for this work, there is still room for improvements. Regarding the keyframe selection algorithm, an interesting alternative can be thresholding the entropy measure [23] of the covariance matrix of the motion estimation computed by DIFODO. 31 As we see in the experimental evaluation, the accuracy in the trajectory estimated by DIFODO can be improved by skipping consecutive frames. The number of frames depends on the speed at which the sensor is moving, and an optimal value could be derived online, from the sensor velocities computed during the estimation. For the loop closure detection module, we only consider the best match for the geometric validation procedure. A higher number of candidates can be considered, depending on the computational load, to increase the detection recall. These candidates can be sorted by the uncertainty of their pose, favouring candidates with lower pose uncertainty. Also, more robust features and descriptors can be used, such as ORB [41]. Finally, the information matrix in the graph optimization formulation should be used to allow higher variations in poses with higher uncertainty, in order to preserve poses with lower uncertainty as much as possible. 32 Conclusiones y Trabajos Futuros En este trabajo se presenta un sistema SLAM que depende ´unicamente de informaci´on obtenida mediante una c´amara RGB-D. El sistema es capaz de estimar la trayectoria completa del sensor y construir de manera incremental una representaci´on 3D consistente de entornos de interiores peque˜nos. Se alcanza la ejecuci´on en tiempo real dividi´endola en dos hilos: uno para el seguimiento y el otro para la construcci´on de la representaci´on consistente. El hilo de seguimiento, compuesto por los algoritmos de odometr´ıa visual y de selecci´on de fotogramas a procesar, alcanza los 60 Hz en un ordenador port´atil de prestaciones modestas. El otro se encarga de la ejecuci´on el resto de componentes, que tienen un crecimiento lineal en cuanto a complejidad computacional. La precisi´on de la trayectoria estimada por este sistema ha sido evaluada en conjuntos de datos p´ublicamente accesibles, obteniendo resultados comparables a sistemas de SLAM del estado del arte. Los objetivos espec´ıficos propuestos para este trabajo han sido abordados del siguiente modo: •Representaci´on: Las observaciones RGB-D se representan con nubes de puntos dispersas, a partir de puntos de inter´es obtenidos mediante el detector FAST. •Selecci´on de fotogramas: Los fotogramas relevantes se seleccionan en base a una m´etrica de similitud visual, calculada mediante una Bolsa de Palabras binarias (DBoW2) sobre descripciones BRIEF. •Grafo de poses: Las restricciones espaciales entre los fotogramas consecutivos se calculan utilizando el algoritmo DIFODO, mientras que las detecciones de cierre de bucle se realizan utilizando la misma Bolsa de Palabras empleada para la selecci´on de fotogramas y las restricciones espaciales entre las detecciones se calculan a partir de correspondencias entre puntos 3D. •Consistencia global: La trayectoria resultante de integrar todas las restricciones calculas se realiza en el marco de la librer´ıa g2o. A´un habiendo cumplido los requisitos establecidos para este trabajo, siguen habiendo mejoras por realizar. Respecto a la selecci´on fotogramas, una alternativa interesante puede ser 33