The final publication is available at link.springer.comThis chapter explains a precise SLAM technique, PL-SLAM, that allows to simultaneously process points and lines and tackle situations where point-only based methods are prone to fail, like poorly textured scenes or motion blurred images where feature points are vanished out. The method is remarkably robust against image noise, and that it outperforms state-of-the-art methods for point based contour alignment. The method can run in real-time and in a low cost hardware.Peer ReviewedPostprint (author's final draft

Agudo Martínez, Antonio

Moreno-Noguer, Francesc

Pumarola Peris, Albert

Sanfeliu Cortés, Alberto

Vakhitov, A.

English

The final publication is available at link.springer.comThis chapter explains a precise SLAM technique, PL-SLAM, that allows to simultaneously process points and lines and tackle situations where point-only based methods are prone to fail, like poorly textured scenes or motion blurred images where feature points are vanished out. The method is remarkably robust against image noise, and that it outperforms state-of-the-art methods for point based contour alignment. The method can run in real-time and in a low cost hardware.Peer Reviewe

UPCommons

Relative localization for aerial manipulation with PL-SLAM

UPCommons (Universitat Politècnica de Catalunya)

ChapterRelative Localization for Aerial Manipulationwith PL-SLAMA. Pumarola, F. Moreno-Noguer, A. SanfeliuAbstract This chapter explains a precise SLAM technique, PL-SLAM, that allows to simultaneously process points and lines and tackle situations where point-only based methods are prone to fail, like poorly textured scenes or motion blurred im-ages where feature points are vanished out. The method is remarkably robust against image noise, and that it outperforms state-of-the-art methods for point based contour alignment. The method can run in real-time and in a low cost hardware.1 IntroductionThe precise localization of an aerial robot is crucial for manipulation. In this sec-tion, we tackle the task of precise localization relative to a close up workspace for robot inspection and manipulation. The method requires robustness to poorly tex-tured surfaces and, when the tracker is lost, relocalize the robot when passing over an already seen area. SLAM methods have proven effective to accurately estimate trajectories while keeping record of previously seen areas.Since the groundbreaking Parallel Tracking And Mapping (PTAM) [3] algorithm was introduced by Klein and Murray in 2007, many other real-time visual SLAM approaches have been proposed, including the feature point-based ORB-SLAM [5], and the direct-based methods LSD-SLAM [2] and RGBD-SLAM [1] that optimize directly over image pixels. Among them, the ORB-SLAM seems to be the current state-of-the-art, yielding better accuracy than the direct methods counterparts. How-ever, it is prone to fail when dealing with poorly textured frames or when feature points are temporary vanished out due to, e.g., motion blur. This kind of situations are often encountered in man-made workspaces. However, despite the lack of reli-able feature points, these environments may still contain a number of lines that can be used in a similar way.Building upon the ORB-SLAM framework, we present PL-SLAM (Point and Line SLAM) [6], a solution that can simultaneously leverage points and lines in-formation. Lines are parameterized by their endpoints, whose exact location in the12Table 1 Symbols used in the development of the PL-SLAM method.Definition Symbol2D detections of lines endpoints[P Q]pd ,qd ∈ R2Homogeneous 2D coordinates of endpoints detections pd ,qd ∈ R2 phd,qhd ∈ R3Detected line coefficients lLine reprojection error ElineCamera calibration matrix (internal parameters) Ki-th camera parameters, θ i = Ri, ti θ iProjection of P into the image plane of camera (θ ,K) π(P,θ ,K)Detected line reprojection error Eline,dDetected point to line error Epl,dProjection of the j-th point X j ∈ R3 into the i-th keyframe x̃i, jEstimation error for X j in i-th keyframe ei,jCost function to minimize during Bundle Adjustment CHubert robust cost function ρCovariance matrices for the detection scales Ω i, j , Ω ′i, j , Ω′′i, jimage plane is estimated following a two-step optimization process. This represen-tation is robust to occlusions and mis-detections and enables integrating the line rep-resentation within the SLAM machinery. The resulting approach is very accurate inpoorly textured environments, and also, improves the performance of ORB-SLAMin highly textured sequences.2 PL-SLAM methodPL-SLAM pipeline highly resembles that of ORB-SLAM, in which we have inte-grated the information provided by line features (see Fig. 1). We next briefly reviewthe main building blocks in which line operations are performed. For a descriptionof the operations involving point features, the reader is referred to [5]. First, pointand line features are detected using [8] an LSD [11], respectively. Then, after havingobtained an initial set of map-to-image point and line feature pairs, all features ofthe local map are projected onto the image to find further correspondences. If theimage contains sufficient new information about the environment, it is flagged as akeyframe and its corresponding points and lines are triangulated and added to themap. To discard possible outliers, features seen from less than three viewpoints orin less than 25% of the frames from which they were expected to be seen are dis-carded too (culling). Point and line features position in the map are optimized witha local BA. Note in Fig. 1 that we do not use lines for loop closing. Matching linesacross the whole map is too computationally expensive. Hence, only point featuresare used for loop detection.We next describe the line parameterization and error function as well as theirintegration within the main building blocks of the SLAM pipeline, namely bundleadjustment and global re-localization.3In order to extend the ORB-SLAM [5] to lines, we need a proper definition ofthe reprojection error and line parameterization. Following [10], let P,Q ∈ R3 bethe 3D endpoints of a line, pd,qd ∈ R2 their 2D detections in the image plane, andphd,qhd ∈ R3 theirs corresponding homogeneous coordinates. From the latter we canobtain the normalized line coefficients as:l =phd×qhd∣∣phd×qhd∣∣ . (1)The line reprojection error Eline is then defined as the sum of point-to-line dis-tances Epl between the projected line segment endpoints, and the detected line in theimage plane (see Fig. 2-right). That is:Eline(P,Q, l,θ ,K) = E2pl(P, l,θ ,K)+E2pl(Q, l,θ ,K), (2)with:Fig. 1 PL-SLAM pipeline, an extension of the ORB-SLAM [5] pipeline. The system is composedby three main threads: Tracking, Local Mapping and Loop Closing. The Tracking thread estimatesthe camera position and decides when to add new keyframes. Then, Local Mapping adds the newkeyframe information into the map and optimizes it with BA. The Loop Closing thread is constantlychecking for loops and correcting them.4Epl(P, l,θ ,K) = l>π(P,θ ,K), (3)where l are the detected line coefficients, π(P,θ ,K) represents the projection ofthe endpoint P onto the image plane, given the internal camera calibration matrixK, and the camera parameters θ = {R, t} that includes the rotation and translationparameters, respectively.Note that in practice, due to real conditions such as line occlusions or mis-detections, the image detected endpoints pd and qd will not match the projectionsof the endpoints P and Q (see Fig. 2-left). Therefore, we define the detected linereprojection error as:Eline,d(pd,qd, l) = E2pl,d(pd, l)+E2pl,d(qd, l), (4)where l is the projected 3D line coefficients and the detected point-to-line error isEpl,d(pd, l) = l>pd.Based on the methodology proposed in [10], a recursion over the detected repro-jection line error will be applied in order to optimize the pose parameters θ whileapproximating Eline,d to the line error Eline defined on Eq. (2).The camera pose parameters θ = {R, t} are optimized at each frame with a Bun-dle Adjustment with Points and Lines strategy that constrains θ to lie in the SE(3)group. For doing this, we build upon the framework of the ORB-SLAM [5] butbesides feature point observations, we include the lines as defined in the previoussubsection. We next define the specific cost function we propose to be optimized bythe BA that combines the two types of geometric entities.Let X j ∈R3 be the generic j-th point of the map. For the i-th keyframe, this pointcan be projected onto the image plane as:Fig. 2 Left: Notation. Let P,Q ∈ R3 be the 3D endpoints of a 3D line, p̃, q̃ ∈ R2 their projected2D endpoints to the image plane and l̃ the projected line coefficients. pd,qd ∈R2 the 2D endpointsof a detected line, Pd,Qd ∈R3 their real 3D endpoints, and l the detected line coefficients. X ∈R3is a 3D point and x̃ ∈ R2 its corresponding 2D projection. Right: Line-based reprojection error.d1 and d2 represent the line reprojection error, and d′1 and d′2 the detected line reprojection errorbetween a detected 2D line (blue solid) and the corresponding projected 3D line (pink dashed).5x̃i, j = π(X j,θ i,K), (5)where θ i = {Ri, ti} denotes the specific pose of the i-th keyframe. Given an obser-vation xi, j of this point, we define following 3D error:ei, j = xi, j− x̃i, j . (6)Similarly, let us denote by P j and Q j the endpoints of the j-th map line segment.The corresponding image projections (expressed in homogeneous coordinates) ontothe same keyframe can be written as:p̃hi, j = π(P j,θ i,K), (7)q̃hi, j = π(Q j,θ i,K) . (8)Then, given the image observations pi, j and qi, j of the j-th line endpoints, we useEq. (1) to estimate the coefficients of the observed line l̃i, j. We define the followingerror vectors for the line:e′i, j = (̃li, j)>(K−1phi, j), (9)e′′i, j = (̃li, j)>(K−1qhi, j). (10)The errors (9, 10) are in fact instances of the point-to-line error (3). As explainedin [10] they are not constant w.r.t. shift of the endpoints P j, Q j along the corre-sponding 3D line, which serves as implicit regularization allowing us to use such anon-minimal line parametrization in the BA.Observe that representing lines using their endpoints we obtain comparable errorrepresentations for points and lines. We can therefore build a unified cost functionthat integrates each of the error terms as:C = ∑i, jρ(e>i, jΩ−1i, j ei, j + e′i, j>Ω′i, j−1e′i, j + e′′i, j>Ω′′i, j−1e′′i, j)where ρ is the Huber robust cost function and Ω i, j, Ω ′i, j, Ω′′i, j are the covariancematrices associated to the scale at which the keypoints and line endpoints weredetected, respectively.An important component of any SLAM method is the Global Relocalization, anapproach to relocalize the camera when the tracker is lost. This is typically achievedby means of a PnP algorithm, that estimates the pose of the current (lost) frame givencorrespondences with 3D map points appearing in previous keyframes. On top of thePnP method, a RANSAC strategy is used to reject outliers correspondences.In the ORB-SLAM, the specific PnP method that is used is the EPnP [4], whichhowever, only accepts point correspondences as inputs. In order to make the ap-proach appropriate to handle lines for relocalization, we replace the EPnP by therecently published EPnPL [10], which minimizes the detected line reprojection er-ror of Eq. (4).6Absolute KeyFrame Trajectory RMSE [cm]TUM RGB-DSequencePL-SLAMClassic InitPL-SLAMLine Init ORB-SLAM PTAM† LSD-SLAM† RGBD-SLAM†f1 xyz 1.21 1.46 1.38 1.15 9.00 1.34f2 xyz 0.43 1.49 0.54 0.2 2.15 2.61f1 floor 7.59 9.42 8.71 - 38.07 3.51f2 360 kidnap 3.92 60.11 4.99 2.63 - 393.3f3 long office 1.97 5.33 4.05 - 38.53 -f3 nstr tex farambiguitydetected 37.60ambiguitydetected 34.74 18.31 -f3 nstr tex near 2.06 1.58 2.88 2.74 7.54 -f3 str tex far 0.89 1.25 0.98 0.93 7.95 -f3 str tex near 1.25 7.47 1.5451 1.04 - -f2 desk person 1.99 6.34 5.95 - 31.73 6.97f3 sit xyz 0.066 9.03 0.08 0.83 7.73 -f3 sit halfsph 1.31 9.05 1.48 - 5.87 -f3 walk xyz 1.54 ambiguitydetected 1.64 - 12.44 -f3 walk halfsph 1.60 ambiguitydetected 2.09 - - -Table 2 Localization accuracy in the TUM RGB-D Benchmark [9]Furthermore, EPnPL is robust to partial line occlusion and mis-detections. This isachieved by means of a two-step procedure in which first minimizes the reprojectionerror of the detected lines and estimates the line endpoints pd,qd. These points, arethen shifted along the line in order to match the projections pd,qd of the 3D modelendpoints P,Q (see Fig. 2). Once these matches are established, the camera posecan be reliably estimated.3 Experiments for validation of the methodTo validate the method, Table 2 presents the the localization accuracy of PL-SLAM [6] against other state-of-the-art Visual SLAM methods, including ORB-SLAM [5], PTAM [3], LSD-SLAM [2] and RGBD-SLAM [1] using the TUMRGB-D benchmark [9]. The metric used for the comparison is the Absolute Tra-jectory Error (ATE), provided by the evaluation script of the benchmark. Beforecomputing the error, all trajectories are aligned using a similarity warp except forthe RGBD-SLAM [1] which is aligned by a rigid body transformation. All experi-ments were carried out with an Intel Core i7-4790 (4 cores @3.6 GHz), 8Gb RAMand ROS Hydro [7]. Due to the randomness of the some stages of the pipeline, e.g.,initialization, position optimization or global relocalization, all experiments wererun five times and we report the median of all executions.7− 2 − 1 0 1 2 3 4x [m ]− 3− 2− 1012y [m]Ground TruthPoint  based ORB-SLAMPoint  and Line based PL-SLAM0204060810Error−2−10123x [m]−3−2−1 0y  [m ]− 2 − 1 0 1 2x [m ]− 3− 2− 10y [m]− 2 − 1 0 1 2x [m ]− 3− 2− 1− 2 − 1 0 1 2 3 4x [m ]− 3− 2− 1012y [m]Ground Truth0.020.040.060.080.100.120.140.16Errory [m]Fig. 3 ORB-SLAM [5] vs PL-SLAM [6]. Comparison of the trajectories obtained using the state-of-the-art point-based method ORB-SLAM and the proposed PL-SLAM, in a TUM RGB-D se-quence. The black dotted line shows the ground truth, the blue dashed line is the trajectory obtainedwith ORB-SLAM, and the green solid line is the trajectory obtained with PL-SLAM. Note how theuse of lines consistently improves the accuracy of the estimated trajectory.Note that PL-SLAM consistently improves the trajectory accuracy of ORB-SLAM in all sequences (see Fig. 3 for a comparison example). Indeed, it yields thebest result in all but two sequences, for which PTAM performs slightly better. Nev-ertheless, PTAM turned not to be so reliable, as in 5 out of all 12 sequences it losttrack. LSD-SLAM and RGBD-SLAM also lost track in 3 and 7 sequences, respec-tively. PL-SLAM builds upon the architecture of the state-of-the-art ORB-SLAMand modifies its original pipeline to operate with line features without significantlycompromising its efficiency.Real-life experiments where done in the Karting experimental site where mostof the methods where tested in real-life conditions in the AEROARMS project. Themethod was implemented using a monochromatic camera located at the bottom ofthe aerial robot. Fig. 4 shows the points and lines detected using the PL-SLAM,and the trajectory followed by the aerial robot before (BSC) and after (ASC) scaleconvergence. The scale is computed in the beginning of the fly to obtain the realscale and once the estimated map scale has converged, we compare the estimateddistance of the robot from the ground against the real one. To obtain the real height,a laser pointer was installed in the bottom of the robot pointing to the ground andcorrected with the relative angles of the robot.The Karting presents a challenging scenario as it contains: (1) visual features ata long distance and (2) apparent lines. Long distant features are prone to error assmall movements of the robot correspond to large displacements of the observed8Fig. 4 Real-life experiments in the Karting experimental site. The figure shows the pipe in theKarting experimental site and the trajectory obtained after (blue) and before (red) scale conver-gence. The figure also shows the points and line detected with the method.point. Small errors in the estimation of this points would cause large penalties inoptimization of the 3D map. To overcome these penalties, long distant points wereremoved by the points culling filter. Similarly, the apparent lines (not real lines)of the scene, e.g. the pipe contours, were also filtered with the same mechanism toprevent the Bundle Adjustment from trying to fit non-real scene landmarks in the 3Dmap. The experiment concluded that the method can robustly operate on challengingscenarios with noisy landmarks.4 ConclusionsIn this chapter we have presented PL-SLAM [6], an approach to visual SLAM thatallows to simultaneously process points and lines and tackle situations where point-only based methods are prone to fail, like poorly textured scenes or motion blurredimages where feature points are vanished out. We have also developed a novel line-based map initialization approach, which estimates camera pose and 3D map from5 line correspondences in three consecutive images. This approach holds on the as-sumption of constant and small inter-frame rotation in these three images. We haveshown that this indeed is a good approximation for many situations and showedconsistent improvement w.r.t. current competing methods results when evaluatingthe full pipeline on the TUM RGB-D benchmark. To the best of our knowledge,the continuous contours based relative localization approach has not been studiedbefore, even though it provides a very natural measure of alignment error without9the need of correspondences. The experiments concluded that the method is remark-ably robust against image noise, and that it outperforms state-of-the-art methods forpoint-based contour alignment. The method was also tested in the Karting experi-mental site were most of the AEROARMS methods were tested. The method canrun in real-time and in a low cost hardware.References1. F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard. 3-D mapping with an RGB-Dcamera. TRO, 30(1):177–187, 2014.2. J. Engel, T. Schöps, and D. Cremers. LSD-SLAM: Large-scale direct monocular SLAM. InECCV, pages 834–849. Springer, 2014.3. G. Klein and D. Murray. Parallel tracking and mapping for small AR workspaces. In ISMAR,pages 225–234. IEEE, 2007.4. F. Moreno-Noguer, V. Lepetit, and P. Fua. Accurate non-iterative O(n) solution to the pnpproblem. In ICCV, pages 1–8. IEEE, 2007.5. R. Mur-Artal, J. M. M. Montiel, and J. D. Tardós. ORB-SLAM: a versatile and accuratemonocular slam system. TRO, 31(5):1147–1163, 2015.6. A. Pumarola, A. Vakhitov, A. Agudo, A. Sanfeliu, and F. Moreno-Noguer. PL-SLAM: Real-Time Monocular Visual SLAM with Points and Lines. In International Conference in Roboticsand Automation, 2017.7. M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng. ROS:an open-source robot operating system. In ICRAW, volume 3, page 5. Kobe, Japan, 2009.8. Ethan Rublee, Vincent Rabaud, Kurt Konolige, and Gary Bradski. Orb: An efficient alternativeto sift or surf. In Computer Vision (ICCV), 2011 IEEE international conference on, pages2564–2571. IEEE, 2011.9. J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers. A benchmark for the evalua-tion of RGB-D SLAM systems. In IROS, Oct. 2012.10. A. Vakhitov, J. Funke, and F. Moreno-Noguer. Accurate and linear time pose estimation frompoints and lines. In ECCV, 2016.11. R. G. von Gioi, J. Jakubowicz, J. M. Morel, and G. Randall. LSD: a line segment detector.IPOL, 2:35–55, 2012.

UPCommons. Portal del coneixement obert de la UPC

ChapterRelative Localization for Aerial Manipulationwith PL-SLAMA. Pumarola, F. Moreno-Noguer, A. SanfeliuAbstract This chapter explains a precise SLAM technique, PL-SLAM, that allows to simultaneously process points and lines and tackle situations where point-only based methods are prone to fail, like poorly textured scenes or motion blurred im-ages where feature points are vanished out. The method is remarkably robust against image noise, and that it outperforms state-of-the-art methods for point based contour alignment. The method can run in real-time and in a low cost hardware.1 IntroductionThe precise localization of an aerial robot is crucial for manipulation. In this sec-tion, we tackle the task of precise localization relative to a close up workspace for robot inspection and manipulation. The method requires robustness to poorly tex-tured surfaces and, when the tracker is lost, relocalize the robot when passing over an already seen area. SLAM methods have proven effective to accurately estimate trajectories while keeping record of previously seen areas.Since the groundbreaking Parallel Tracking And Mapping (PTAM) [3] algorithm was introduced by Klein and Murray in 2007, many other real-time visual SLAM approaches have been proposed, including the feature point-based ORB-SLAM [5], and the direct-based methods LSD-SLAM [2] and RGBD-SLAM [1] that optimize directly over image pixels. Among them, the ORB-SLAM seems to be the current state-of-the-art, yielding better accuracy than the direct methods counterparts. How-ever, it is prone to fail when dealing with poorly textured frames or when feature points are temporary vanished out due to, e.g., motion blur. This kind of situations are often encountered in man-made workspaces. However, despite the lack of reli-able feature points, these environments may still contain a number of lines that can be used in a similar way.Building upon the ORB-SLAM framework, we present PL-SLAM (Point and Line SLAM) [6], a solution that can simultaneously leverage points and lines in-formation. Lines are parameterized by their endpoints, whose exact location in the12Table 1 Symbols used in the development of the PL-SLAM method.Definition Symbol2D detections of lines endpoints[P Q]pd ,qd ∈ R2Homogeneous 2D coordinates of endpoints detections pd ,qd ∈ R2 phd,qhd ∈ R3Detected line coefficients lLine reprojection error ElineCamera calibration matrix (internal parameters) Ki-th camera parameters, θ i = Ri, ti θ iProjection of P into the image plane of camera (θ ,K) pi(P,θ ,K)Detected line reprojection error Eline,dDetected point to line error Epl,dProjection of the j-th point X j ∈ R3 into the i-th keyframe x˜i, jEstimation error for X j in i-th keyframe ei,jCost function to minimize during Bundle Adjustment CHubert robust cost function ρCovariance matrices for the detection scales Ω i, j , Ω ′i, j , Ω ′′i, jimage plane is estimated following a two-step optimization process. This represen-tation is robust to occlusions and mis-detections and enables integrating the line rep-resentation within the SLAM machinery. The resulting approach is very accurate inpoorly textured environments, and also, improves the performance of ORB-SLAMin highly textured sequences.2 PL-SLAM methodPL-SLAM pipeline highly resembles that of ORB-SLAM, in which we have inte-grated the information provided by line features (see Fig. 1). We next briefly reviewthe main building blocks in which line operations are performed. For a descriptionof the operations involving point features, the reader is referred to [5]. First, pointand line features are detected using [8] an LSD [11], respectively. Then, after havingobtained an initial set of map-to-image point and line feature pairs, all features ofthe local map are projected onto the image to find further correspondences. If theimage contains sufficient new information about the environment, it is flagged as akeyframe and its corresponding points and lines are triangulated and added to themap. To discard possible outliers, features seen from less than three viewpoints orin less than 25% of the frames from which they were expected to be seen are dis-carded too (culling). Point and line features position in the map are optimized witha local BA. Note in Fig. 1 that we do not use lines for loop closing. Matching linesacross the whole map is too computationally expensive. Hence, only point featuresare used for loop detection.We next describe the line parameterization and error function as well as theirintegration within the main building blocks of the SLAM pipeline, namely bundleadjustment and global re-localization.3In order to extend the ORB-SLAM [5] to lines, we need a proper definition ofthe reprojection error and line parameterization. Following [10], let P,Q ∈ R3 bethe 3D endpoints of a line, pd,qd ∈ R2 their 2D detections in the image plane, andphd,qhd ∈ R3 theirs corresponding homogeneous coordinates. From the latter we canobtain the normalized line coefficients as:l =phd×qhd∣∣phd×qhd∣∣ . (1)The line reprojection error Eline is then defined as the sum of point-to-line dis-tances Epl between the projected line segment endpoints, and the detected line in theimage plane (see Fig. 2-right). That is:Eline(P,Q, l,θ ,K) = E2pl(P, l,θ ,K)+E2pl(Q, l,θ ,K), (2)with:Fig. 1 PL-SLAM pipeline, an extension of the ORB-SLAM [5] pipeline. The system is composedby three main threads: Tracking, Local Mapping and Loop Closing. The Tracking thread estimatesthe camera position and decides when to add new keyframes. Then, Local Mapping adds the newkeyframe information into the map and optimizes it with BA. The Loop Closing thread is constantlychecking for loops and correcting them.4Epl(P, l,θ ,K) = l>pi(P,θ ,K), (3)where l are the detected line coefficients, pi(P,θ ,K) represents the projection ofthe endpoint P onto the image plane, given the internal camera calibration matrixK, and the camera parameters θ = {R, t} that includes the rotation and translationparameters, respectively.Note that in practice, due to real conditions such as line occlusions or mis-detections, the image detected endpoints pd and qd will not match the projectionsof the endpoints P and Q (see Fig. 2-left). Therefore, we define the detected linereprojection error as:Eline,d(pd,qd, l) = E2pl,d(pd, l)+E2pl,d(qd, l), (4)where l is the projected 3D line coefficients and the detected point-to-line error isEpl,d(pd, l) = l>pd.Based on the methodology proposed in [10], a recursion over the detected repro-jection line error will be applied in order to optimize the pose parameters θ whileapproximating Eline,d to the line error Eline defined on Eq. (2).The camera pose parameters θ = {R, t} are optimized at each frame with a Bun-dle Adjustment with Points and Lines strategy that constrains θ to lie in the SE(3)group. For doing this, we build upon the framework of the ORB-SLAM [5] butbesides feature point observations, we include the lines as defined in the previoussubsection. We next define the specific cost function we propose to be optimized bythe BA that combines the two types of geometric entities.Let X j ∈R3 be the generic j-th point of the map. For the i-th keyframe, this pointcan be projected onto the image plane as:Fig. 2 Left: Notation. Let P,Q ∈ R3 be the 3D endpoints of a 3D line, p˜, q˜ ∈ R2 their projected2D endpoints to the image plane and l˜ the projected line coefficients. pd,qd ∈R2 the 2D endpointsof a detected line, Pd,Qd ∈R3 their real 3D endpoints, and l the detected line coefficients. X ∈R3is a 3D point and x˜ ∈ R2 its corresponding 2D projection. Right: Line-based reprojection error.d1 and d2 represent the line reprojection error, and d′1 and d′2 the detected line reprojection errorbetween a detected 2D line (blue solid) and the corresponding projected 3D line (pink dashed).5x˜i, j = pi(X j,θ i,K), (5)where θ i = {Ri, ti} denotes the specific pose of the i-th keyframe. Given an obser-vation xi, j of this point, we define following 3D error:ei, j = xi, j− x˜i, j . (6)Similarly, let us denote by P j and Q j the endpoints of the j-th map line segment.The corresponding image projections (expressed in homogeneous coordinates) ontothe same keyframe can be written as:p˜hi, j = pi(P j,θ i,K), (7)q˜hi, j = pi(Q j,θ i,K) . (8)Then, given the image observations pi, j and qi, j of the j-th line endpoints, we useEq. (1) to estimate the coefficients of the observed line l˜i, j. We define the followingerror vectors for the line:e′i, j = (˜li, j)>(K−1phi, j), (9)e′′i, j = (˜li, j)>(K−1qhi, j). (10)The errors (9, 10) are in fact instances of the point-to-line error (3). As explainedin [10] they are not constant w.r.t. shift of the endpoints P j, Q j along the corre-sponding 3D line, which serves as implicit regularization allowing us to use such anon-minimal line parametrization in the BA.Observe that representing lines using their endpoints we obtain comparable errorrepresentations for points and lines. We can therefore build a unified cost functionthat integrates each of the error terms as:C =∑i, jρ(e>i, jΩ−1i, j ei, j+ e′i, j>Ω ′i, j−1e′i, j+ e′′i, j>Ω ′′i, j−1e′′i, j)where ρ is the Huber robust cost function and Ω i, j, Ω ′i, j, Ω ′′i, j are the covariancematrices associated to the scale at which the keypoints and line endpoints weredetected, respectively.An important component of any SLAM method is the Global Relocalization, anapproach to relocalize the camera when the tracker is lost. This is typically achievedby means of a PnP algorithm, that estimates the pose of the current (lost) frame givencorrespondences with 3D map points appearing in previous keyframes. On top of thePnP method, a RANSAC strategy is used to reject outliers correspondences.In the ORB-SLAM, the specific PnP method that is used is the EPnP [4], whichhowever, only accepts point correspondences as inputs. In order to make the ap-proach appropriate to handle lines for relocalization, we replace the EPnP by therecently published EPnPL [10], which minimizes the detected line reprojection er-ror of Eq. (4).6Absolute KeyFrame Trajectory RMSE [cm]TUM RGB-DSequencePL-SLAMClassic InitPL-SLAMLine Init ORB-SLAM PTAM† LSD-SLAM† RGBD-SLAM†f1 xyz 1.21 1.46 1.38 1.15 9.00 1.34f2 xyz 0.43 1.49 0.54 0.2 2.15 2.61f1 floor 7.59 9.42 8.71 - 38.07 3.51f2 360 kidnap 3.92 60.11 4.99 2.63 - 393.3f3 long office 1.97 5.33 4.05 - 38.53 -f3 nstr tex farambiguitydetected 37.60ambiguitydetected 34.74 18.31 -f3 nstr tex near 2.06 1.58 2.88 2.74 7.54 -f3 str tex far 0.89 1.25 0.98 0.93 7.95 -f3 str tex near 1.25 7.47 1.5451 1.04 - -f2 desk person 1.99 6.34 5.95 - 31.73 6.97f3 sit xyz 0.066 9.03 0.08 0.83 7.73 -f3 sit halfsph 1.31 9.05 1.48 - 5.87 -f3 walk xyz 1.54 ambiguitydetected 1.64 - 12.44 -f3 walk halfsph 1.60 ambiguitydetected 2.09 - - -Table 2 Localization accuracy in the TUM RGB-D Benchmark [9]Furthermore, EPnPL is robust to partial line occlusion and mis-detections. This isachieved by means of a two-step procedure in which first minimizes the reprojectionerror of the detected lines and estimates the line endpoints pd,qd. These points, arethen shifted along the line in order to match the projections pd,qd of the 3D modelendpoints P,Q (see Fig. 2). Once these matches are established, the camera posecan be reliably estimated.3 Experiments for validation of the methodTo validate the method, Table 2 presents the the localization accuracy of PL-SLAM [6] against other state-of-the-art Visual SLAM methods, including ORB-SLAM [5], PTAM [3], LSD-SLAM [2] and RGBD-SLAM [1] using the TUMRGB-D benchmark [9]. The metric used for the comparison is the Absolute Tra-jectory Error (ATE), provided by the evaluation script of the benchmark. Beforecomputing the error, all trajectories are aligned using a similarity warp except forthe RGBD-SLAM [1] which is aligned by a rigid body transformation. All experi-ments were carried out with an Intel Core i7-4790 (4 cores @3.6 GHz), 8Gb RAMand ROS Hydro [7]. Due to the randomness of the some stages of the pipeline, e.g.,initialization, position optimization or global relocalization, all experiments wererun five times and we report the median of all executions.7− 2 − 1 0 1 2 3 4x [m ]− 3− 2− 1012y  [ m]Ground TruthPoint  based ORB-SLAMPoint  and Line based PL-SLAM0204060810Error−2−10123x [m]−3−2−1 0y  [m ]− 2 − 1 0 1 2x [m ]− 3− 2− 10y  [ m]− 2 − 1 0 1 2x [m ]− 3− 2− 1− 2 − 1 0 1 2 3 4x [m ]− 3− 2− 1012y  [ m]Ground Truth0.020.040.060.080.100.120.140.16Er rory  [ m]Fig. 3 ORB-SLAM [5] vs PL-SLAM [6]. Comparison of the trajectories obtained using the state-of-the-art point-based method ORB-SLAM and the proposed PL-SLAM, in a TUM RGB-D se-quence. The black dotted line shows the ground truth, the blue dashed line is the trajectory obtainedwith ORB-SLAM, and the green solid line is the trajectory obtained with PL-SLAM. Note how theuse of lines consistently improves the accuracy of the estimated trajectory.Note that PL-SLAM consistently improves the trajectory accuracy of ORB-SLAM in all sequences (see Fig. 3 for a comparison example). Indeed, it yields thebest result in all but two sequences, for which PTAM performs slightly better. Nev-ertheless, PTAM turned not to be so reliable, as in 5 out of all 12 sequences it losttrack. LSD-SLAM and RGBD-SLAM also lost track in 3 and 7 sequences, respec-tively. PL-SLAM builds upon the architecture of the state-of-the-art ORB-SLAMand modifies its original pipeline to operate with line features without significantlycompromising its efficiency.Real-life experiments where done in the Karting experimental site where mostof the methods where tested in real-life conditions in the AEROARMS project. Themethod was implemented using a monochromatic camera located at the bottom ofthe aerial robot. Fig. 4 shows the points and lines detected using the PL-SLAM,and the trajectory followed by the aerial robot before (BSC) and after (ASC) scaleconvergence. The scale is computed in the beginning of the fly to obtain the realscale and once the estimated map scale has converged, we compare the estimateddistance of the robot from the ground against the real one. To obtain the real height,a laser pointer was installed in the bottom of the robot pointing to the ground andcorrected with the relative angles of the robot.The Karting presents a challenging scenario as it contains: (1) visual features ata long distance and (2) apparent lines. Long distant features are prone to error assmall movements of the robot correspond to large displacements of the observed8Fig. 4 Real-life experiments in the Karting experimental site. The figure shows the pipe in theKarting experimental site and the trajectory obtained after (blue) and before (red) scale conver-gence. The figure also shows the points and line detected with the method.point. Small errors in the estimation of this points would cause large penalties inoptimization of the 3D map. To overcome these penalties, long distant points wereremoved by the points culling filter. Similarly, the apparent lines (not real lines)of the scene, e.g. the pipe contours, were also filtered with the same mechanism toprevent the Bundle Adjustment from trying to fit non-real scene landmarks in the 3Dmap. The experiment concluded that the method can robustly operate on challengingscenarios with noisy landmarks.4 ConclusionsIn this chapter we have presented PL-SLAM [6], an approach to visual SLAM thatallows to simultaneously process points and lines and tackle situations where point-only based methods are prone to fail, like poorly textured scenes or motion blurredimages where feature points are vanished out. We have also developed a novel line-based map initialization approach, which estimates camera pose and 3D map from5 line correspondences in three consecutive images. This approach holds on the as-sumption of constant and small inter-frame rotation in these three images. We haveshown that this indeed is a good approximation for many situations and showedconsistent improvement w.r.t. current competing methods results when evaluatingthe full pipeline on the TUM RGB-D benchmark. To the best of our knowledge,the continuous contours based relative localization approach has not been studiedbefore, even though it provides a very natural measure of alignment error without9the need of correspondences. The experiments concluded that the method is remark-ably robust against image noise, and that it outperforms state-of-the-art methods forpoint-based contour alignment. The method was also tested in the Karting experi-mental site were most of the AEROARMS methods were tested. The method canrun in real-time and in a low cost hardware.References1. F. Endres, J. Hess, J. Sturm, D. Cremers, and W. Burgard. 3-D mapping with an RGB-Dcamera. TRO, 30(1):177–187, 2014.2. J. Engel, T. Scho¨ps, and D. Cremers. LSD-SLAM: Large-scale direct monocular SLAM. InECCV, pages 834–849. Springer, 2014.3. G. Klein and D. Murray. Parallel tracking and mapping for small AR workspaces. In ISMAR,pages 225–234. IEEE, 2007.4. F. Moreno-Noguer, V. Lepetit, and P. Fua. Accurate non-iterative O(n) solution to the pnpproblem. In ICCV, pages 1–8. IEEE, 2007.5. R. Mur-Artal, J. M. M. Montiel, and J. D. Tardo´s. ORB-SLAM: a versatile and accuratemonocular slam system. TRO, 31(5):1147–1163, 2015.6. A. Pumarola, A. Vakhitov, A. Agudo, A. Sanfeliu, and F. Moreno-Noguer. PL-SLAM: Real-Time Monocular Visual SLAM with Points and Lines. In International Conference in Roboticsand Automation, 2017.7. M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs, R. Wheeler, and A. Y. Ng. ROS:an open-source robot operating system. In ICRAW, volume 3, page 5. Kobe, Japan, 2009.8. Ethan Rublee, Vincent Rabaud, Kurt Konolige, and Gary Bradski. Orb: An efficient alternativeto sift or surf. In Computer Vision (ICCV), 2011 IEEE international conference on, pages2564–2571. IEEE, 2011.9. J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers. A benchmark for the evalua-tion of RGB-D SLAM systems. In IROS, Oct. 2012.10. A. Vakhitov, J. Funke, and F. Moreno-Noguer. Accurate and linear time pose estimation frompoints and lines. In ECCV, 2016.11. R. G. von Gioi, J. Jakubowicz, J. M. Morel, and G. Randall. LSD: a line segment detector.IPOL, 2:35–55, 2012.

Relative localization for aerial manipulation with PL-SLAM

Abstract

Similar works

Full text

Available Versions

UPCommons

UPCommons (Universitat Politècnica de Catalunya)

UPCommons. Portal del coneixement obert de la UPC