Lightweight 3-D Localization and Mapping for Solid-State LiDAR
IIEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2021 1
Lightweight 3-D Localization and Mapping forSolid-State LiDAR
Han Wang , Chen Wang , and Lihua Xie Abstract —The LIght Detection And Ranging (LiDAR) sensorhas become one of the most important perceptual devices dueto its important role in simultaneous localization and mapping(SLAM). Existing SLAM methods are mainly developed formechanical LiDAR sensors, which are often adopted by largescale robots. Recently, the solid-state LiDAR is introduced andbecomes popular since it provides a cost-effective and lightweightsolution for small scale robots. Compared to mechanical LiDAR,solid-state LiDAR sensors have higher update frequency andangular resolution, but also have smaller field of view (FoV),which is very challenging for existing LiDAR SLAM algorithms.Therefore, it is necessary to have a more robust and compu-tationally efficient SLAM method for this new sensing device.To this end, we propose a new SLAM framework for solid-state LiDAR sensors, which involves feature extraction, odometryestimation, and probability map building. The proposed methodis evaluated on a warehouse robot and a hand-held device. In theexperiments, we demonstrate both the accuracy and efficiency ofour method using an Intel L515 solid-state LiDAR. The resultsshow that our method is able to provide precise localizationand high quality mapping. We made the source codes publicat https://github.com/wh200720041/SSL SLAM.
Index Terms —SLAM, mapping, factory automation
I. INTRODUCTION T HE LIght Detection And Ranging (LiDAR) is a kind ofTime of Flight (ToF) sensor which measures the objectdistance by emitting laser to the object and capturing the trav-elling time. It is one of the most popular perceptual systems inrobotic applications due to its high precision, long cover range,and high reliability. Traditional localization approaches oftenleverage on external setup and hence lack of robustness indifferent scenarios. For example, UWB localization [1] relieson the pre-installation and calibration of multiple anchors,and WiFi localization [2] requires multiple routers in order toachieve high accuracy. In comparison, LiDAR SLAM providesan external device/landmark-free localization for mobile robotsin both indoor and outdoor environments [3]–[5]. Moreover,LiDAR SLAM is less affected by environment uncertainties, e.g. , weather change or illumination change, compared to
Manuscript received: October, 15, 2020; Revised December, 9, 2020;Accepted January, 30, 2021.This paper was recommended for publication by Editor Sven Behnke uponevaluation of the Associate Editor and Reviewers’ comments. This work wassupported by Delta-NTU Corporate Laboratory for Cyber-Physical Systemsunder the National Research Foundation Corporate Lab @ University Scheme. Han Wang and Lihua Xie are with the School of Electrical and ElectronicEngineering, Nanyang Technological University, 50 Nanyang Avenue, Singa-pore 639798. e-mail: { wang.han,elhxie } @ntu.edu.sg Chen Wang is with the Robotics Institute, Carnegie Mellon University,Pittsburgh, PA 15213, USA. e-mail: [email protected]
Digital Object Identifier (DOI): see top of this page.
Fig. 1: Example of indoor localization and mapping in thewarehouse environment. The proposed method is integratedto an AGV used for warehouse manipulation. Our method isable to provide real-time localization and dense mapping onan embedded mini computer.other SLAM system such as visual SLAM [6], [7] and visualinertial SLAM [8], [9]. Those properties have made LiDARSLAM widely applied to various robotic applications suchas autonomous driving [10], building inspection [11], andintelligent manufacturing [12].Existing approaches were mainly developed for mechanicalLiDAR sensors which collect the surrounding informationby spinning a high frequency laser array. Although theyhave achieved impressive experimental results on large scalemapping [13], [14], it is not popularly used due to thehigh cost. Moreover, the mechanical LiDAR is difficult tobe implemented on small scale system due to its size andweight. For example, the durability of flight of UnmannedAerial Vehicles (UAVs) is significantly reduced by carrying amechanical LiDAR for building inspection [15]. In addition, itis also not possible to integrate mechanical LiDAR into hand-held device due to its large size.Recently, the introduction of solid-state LiDAR providesa cost-effective and lightweight solution for LiDAR SLAMsystem. The solid-state LiDAR is a system that built entirelyon a silicon chip with no moving parts involved [16]. Henceboth the size and the weight can be made much smallerthan mechanical LiDAR. Moreover, the solid-state LiDARis resistant to vibrations by removing rotating mechanicalstructure. The latest solid-state LiDAR only costs 10% priceof a mechanical LiDAR and is as small as a smart phone, a r X i v : . [ c s . R O ] F e b IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2021
Sensor Type Frequency FoV Horizontal Vertical Detection Accuracy Dimension WeightResolution Resolution RangeVelodyne VLP-16 Mechanical 5-20Hz ◦ × ◦ ◦ . ◦ ×
72 mm 860 gRealsense L515 Solid-state 30Hz ◦ × ◦ . ◦ . ◦ ×
26 mm 95 g
TABLE I: Difference of Mechanical and Solid-state LiDAR.which has a great potential to become a dominant perceptionsystem for small scale application such as Virtual Reality (VR)[17], drone inspection, and indoor navigation. In terms ofperformance, solid-state LiDAR is able to achieve an accuracyup to 1-2 cm and a detectable range up to a few hundreds ofmeters.Although the performance of mechanical LiDAR and solid-state LiDAR is similar, the implementation or challenge ofLiDAR SLAM is different. To illustrate the difference betweentwo LiDARs, we use Velodyne VLP-16 and Realsense L515for example. The specifications can be found at Table I. Solid-state LiDAR has higher angular resolution, implying that thepoints are more dense within the same scanning area. There-fore, traditional LiDAR odometry methods such as IterativeClosest Point (ICP) [18] can be computationally inefficientsince there are more points involved for calculation. Secondly,the update frequency of solid-state LiDAR is higher, whiletraditional LiDAR SLAM such as LOAM [19] is not com-putationally efficient enough to reach real-time performance.Another challenge is the pyramid-like coverage view that cancause severe tracking loss during large rotation. Therefore, amore computationally efficient and robust algorithm has to bedesigned in order to achieve real-time performance for solid-state LiDAR-based SLAM.In this work, we propose a novel and lightweight SLAMframework for solid-state LiDAR, consisting of feature extrac-tion, odometry estimation and probability map construction.Inspired by existing LiDAR SLAM methods such as LOAM[19] and LeGO-LOAM [20], we propose a new rotation-invariant feature extraction method that exploits both horizon-tal and vertical curvature. The proposed method provides real-time localization on mobile platforms. Thorough experimentshave been conducted to evaluate its performance. The maincontributions of this paper are listed as follows: • We propose a full SLAM framework for solid-stateLiDAR, which targets to tackle perception system withsmall FoV and high updating frequency. We make theproposed method open sourced. • We introduce an improved feature extraction strategywhich is able to search for consistent features undersignificant rotations. Moreover, left Lie derivative is usedfor iterative pose estimation so that the pose is stored ina singularity-free format. • A thorough evaluation of the proposed method is pre-sented. More specifically, we integrate an Intel L515 solidstate LiDAR to AGVs and test the proposed method in acomplex warehouse environment. The proposed methodcan provide real time localization and is robust underrotations.This paper is organized as follows: Section II reviews the related works on existing LiDAR SLAM approaches. SectionIII describes the details of the proposed approach, includingfeature point selection, laser odometry estimation, and proba-bility map construction. Section IV shows experimental resultsand comparison with existing works, followed by conclusionin Section V. II. RELATED WORKExisting works on LiDAR SLAM mainly estimate the loca-tion by either scan-to-scan matching or feature-based match-ing. To match between two scan inputs, Iterative Closest Point(ICP) [21] is one of the most classic methods used. Startingwith an initial pose guess, the ICP algorithm determines thepoint correspondence by finding the closest point in targetframes. A transformation matrix between the current frameand target frame can be estimated by minimizing the Euclideandistance between point pairs. Subsequently, the derived trans-formation matrix leads to new correspondence relationshipand new transformation matrix. And the transformation matrixconverges through an iterative manner. A LiDAR scan inputconsists of tens of thousands of points and direct point cloudmatching is computationally inefficient. Hence it is mainlyimplemented to LiDAR SLAM with high computational re-sources and low update frequency, e.g. , HDL-Graph-SLAM[22], and LiDAR-Only Odometry and Localization (LOL)[23]. In the meantime, raw point cloud matching is alsosensitive to noise in practice. For example, in autonomousdriving, the measurements from trees alongside the road areoften noisy. Such noise greatly reduces the matching accuracyand causes localization drift.A more computationally efficient way is to match in fea-ture space where less points are involved to find the posetransformation. Zhang et al. [19] propose a new frameworkvia matching on the edge features and planar features tosolve the LiDAR SLAM problem for mobile robots. Theedge features and planar features are collected based on localsmoothness and are maintained in separate maps. And thefeature correspondence is picked by finding the nearest globaledges and planes respectively. The localization is estimatedby minimizing the point-to-edge and point-to-plane distance.The experimental results also achieve impressive performanceon public dataset evaluation such as KITTI dataset [24]. Animproved version is LeGO-LOAM [20], which targets to solvethe LiDAR SLAM for Unmanned Ground Vehicles (UGVs).The surrounding points from the current frame are segmentedinto ground points and non-ground points. For UGVs, theground points are often planar points and those points areused to identify roll, pitch angle and z translation. The resultsare subsequently used as prior knowledge for edge featurematching, which targets to calculate x, y translation and yaw
ANG et al. : LIGHTWEIGHT 3-D LOCALIZATION AND MAPPING FOR SOLID-STATE LIDAR 3
Scan From solid-state L idar Feature Extr action Update L ocal M apOdometr y Estimation K ey Fr ame?Probabiliy update Global M ap
Fig. 2: System overview of the proposed method. It consists of three main modules: feature extraction, odometry estimationand probability map construction, which are highlighted in yellow, blue and red, respectively.angle. By taking segmentation and two-stage matching, thefeature points selected are reduced by 40%. The experimentalresults show that LeGO-LOAM is able to run on smallcomputing platform at real-time performance.Recently some works aim to use some specially designedLiDAR with small FoV. In [16] the author proposes a newLiDAR SLAM framework for small Fov LiDAR, i.e. , theDJI Livox. DJI Livox spins a laser beam in cone shapedirection and provides smaller FoV but denser points. Totackle the localization problem for small FoV sensor, theauthor introduces a new feature extraction method with theanalysis of local LiDAR intensity. Compared to traditionalfeature extraction, the proposed feature extraction is moreconsistent over nearby frames which can subsequently reducethe tracking loss. However, it is mainly designed for large scaleoutdoor environment, while the performance in the indoorcomplex environment is not demonstrated.III. METHODOLOGYIn this section, the proposed method is described in details.As shown Fig. 2, the system consists of three main modules,namely feature extraction, odometry estimation, and probabil-ity map construction. We firstly present the rotation invariantfeature extraction method and then introduce odometry es-timation via local feature matching. Finally we present theprobability map building and scene reconstruction.
A. Feature Extraction
A solid-state LiDAR integrates all sensors on a single siliconchip without moving parts. It often has higher resolution andhigher update frequency compared to a mechanical LiDAR.Hence it might be too computational heavy to match the rawpoint clouds. Inspired by LOAM [19], we leverage the edgeand planar matching which is much more computationallyefficient. Before processing the data, we remove the noisypoints based on the measured distance. It is observed thatreadings near the maximum detection range are often lessaccurate due to the low reflected energy thus we pre-filterthose noisy points.The point cloud from a LiDAR scan is unordered. To com-pute the edge and planar features, We firstly project the pointcloud into a 2-D point matrix. For the k th LiDAR scan input P k , it is segmented based on the vertical angle and horizontal angle of each point. Given a point p i = { x i , y i , z i } ∈ P k , thevertical angle α i and horizontal angle θ i are calculated by: α i = arctan (cid:18) y i x i (cid:19) ,θ i = arctan (cid:18) z i x i (cid:19) . (1)The point cloud is then segmented by equally diving verticaldetection range [ α min , α max ] and horizontal detection range [ θ min , θ max ] into M sectors and N sectors, where [ α min , α max , θ min and θ max are the minimum vertical angle, max-imum vertical angle, minimum horizontal angle, maximumhorizontal angle according to the sensor specifications. Hence,the point cloud is segmented into M × N cells. For a solid-stateLiDAR with vertical angular resolution of α r and horizontalresolution of θ r , M and N is selected as half of total points ina single direction, i.e., M = α max − α min × α r and N = θ max − θ min × θ r .In general, larger M and N will take more computationalresources. In each cell ( m, n ) , where m ∈ [1 , M ] , n ∈ [1 , N ] and the symbol [1 , N ] represents { , , · · · , N } , we calculatethe mean measurement p ( m,n ) k by finding the geometry centerof all points in the cell.To extract the edge and planar features, we search for itsnearby points and define the local smoothness by: σ ( m,n ) k = 1 λ · (cid:88) p ( i,j ) k ∈S ( m,n ) k ( || p ( i,j ) k || − || p ( m,n ) k || ) , (2)with S ( m,n ) k = { p ( i,j ) k | i ∈ [ m − λ, m + λ ] , j ∈ [ n − λ, n + λ ] } , (3)where λ is a pre-defined searching radius. A larger λ meanssearching for more surrounding points and requires morecomputational resource. The local smoothness indicates thesharpness of surrounding information. A higher σ ( m,n ) k indi-cates that the local surface is sharp, thus the correspondingpoint p ( m,n ) k is taken as edge feature. A smaller σ ( m,n ) k indicates that the local surface is flat, thus the correspondingpoint p ( m,n ) k is taken as plane feature. B. Odometry Estimation
The odometry estimation is a task to estimate the robotcurrent pose T k ∈ SE (3) in global coordinate based onthe historical laser scan P , P , · · · , P k − . Traditionally the IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2021 trajectory is estimated by either scan-to-scan match or scan-to-map match. The scan-to-scan match aligns the current framewith last frame that is faster. However, a single laser scancontains less surrounding information compared to local mapand causes drift in the long run. Therefore, we use the scan-to-map match in order to improve the performance. To reducethe computational cost, sliding window approach is used. Theedge features and planar features from the nearby frames areused to build the local feature maps. For the current input P k , we define the local map M k = { P k − , P k − , · · · , P k − q } ,where q is the number of frames used to build local map.As mentioned above, matching on raw point clouds is lessefficient and sensitive to noise. Thus, we leverage in matchingedge points and planar points in feature space. For each edgepoint p k ∈ P k and its transform in the local map coordinate ˆ p k = T k · p k , we search for the nearest edge from the localmap. Note that the local map is divided into edge map andplanar map, and each map is built by K-D tree in order toincrease searching efficiency. Thus we can select two nearestedge feature points p E and p E from the local map for eachedge point. The point-to-edge residual f E (ˆ p k ) is defined asthe distance between ˆ p k and the edge crossing p E and p E : f E (ˆ p k ) = | (ˆ p k − p E ) × (ˆ p k − p E ) || p E − p E | , (4)where symbol × is the cross product of two vectors. Notethat if the number of nearby points is less than 2, the point-to-edge residual is not taken into accounted by the final cost.Similarly, for each planar point p k ∈ P k , we search for thenearest planar features from the local map. To estimate a planein 3D space, it is necessary to have 3 points. Hence for agiven planar feature point p k and its transform in local mapcoordinate ˆ p k = T k · p k , we can find 3 nearest points p S , p S , and p S from the local map. The point-to-plane residual f S (ˆ p k ) is defined as the distance between ˆ p k and the planecrossing p S , p S , and p S : f S (ˆ p k ) = | (ˆ p k − p S ) T · ( p S − p S ) × ( p S − p S ) | ( p S − p S ) × ( p S − p S ) | | . (5)Similar to the edge residual, the point-to-plane residual is nottaken into account when the number of nearby points is lessthan 3. The final odometry is estimated by minimizing thepoint-to-plane residual and point-to-edge residual:arg min T k (cid:88) f E (ˆ p k ) + (cid:88) f S (ˆ p k ) . (6)This non-linear optimization problem can be solved by theGauss–Newton optimization method. We use left perturbationscheme and apply increment on Lie Group. Compared tothe differentiation model in LOAM [19], there are severaladvantages: (i) the rotation or pose is stored in a singularity-free format; (ii) at each iteration unconstrained optimization isperformed; (iii) the manipulations occur at the matrix level sothat there is no need to worry about taking the derivatives ofa bunch of scalar trigonometric functions, which can easilylead to errors [25]. Define ξ k = [ ρ, φ ] ∈ se (3) and thetransformation matrix: T k = exp( ξ ∧ k ) , (7) where the notation ξ ∧ converts a 6D vector into a × matrixby: ξ ∧ = (cid:20) [ ρ ] × φ × (cid:21) , (8)where [ · ] × is the skew matrix of a 3D vector. The leftperturbation model can be calculated by: J p = ∂ T k p k ∂δξ = lim δξ → (exp( δξ ∧ ) · T k p k − T k p k ) δξ = (cid:20) I × − [ T k p k ] × × × (cid:21) . (9)Note that here [ T k p k ] × transforms 4D point expression { x, y, z, } into 3D point expression { x, y, z } before calcu-lating the skew matrix. The Jacobian matrix of point-to-edgeresidual is defined by J E = ∂f E (ˆ p k ) ∂ ˆ p k · J p = p Tn · J p × (ˆ p k − p E ) + (ˆ p k − p E ) × J p | p E − p E | = p Tn · ( p E − p E ) | p E − p E | × J p , (10)where p n = (ˆ p k − p E ) × (ˆ p k − p E ) | (ˆ p k − p E ) × (ˆ p k − p E ) | . (11)Similarly, we can derive the Jacobian matrix of point-to-planeresidual: J S = ∂f S (ˆ p k ) ∂ ˆ p k · J p = [( p S − p S ) × ( p S − p S )] T | ( p S − p S ]) × ( p S − p S ) | · J p . (12)The alignment of current scan and local map may not beideal at the beginning. Therefore, it is necessary to search forbetter feature correspondences. The optimal feature correspon-dence can be found through an iterative manner, i.e. , with aninitial pose T k and initial correspondence based on T k , we canderive the odometry estimation T k then T k and T k , etc . Thisfinally converges to the optimal estimation of current pose.Although iterative calculation is computationally inefficient, agood estimation of initial pose alignment is able to speed upthe convergence. To find an better initial alignment, we assumeconstant angular velocity and linear translation: T k = T k − · T k − k − = T k − · T − k − · T k − . (13)The process of iterative odometry estimation is listed inAlgorithm 1. C. Probability Map Construction
The global map is often of large size and it is not com-putationally efficient to update it with each frame. Therefore,we only use key frames to update and reconstruct map. Thekey frames are selected based on the following criterion:(1) If the displacement of robot is significant enough ( i.e. ,greater than a pre-defined threshold); (2) If change of rotation
ANG et al. : LIGHTWEIGHT 3-D LOCALIZATION AND MAPPING FOR SOLID-STATE LIDAR 5
Algorithm 1:
Pose estimation for Solid-state LiDAR
Input :
Current scan P k , the robot trajectory T k − Output:
Current pose T k begin if Not Initialized then T ← ; Calculate the local smoothness and extract edgefeatures and planar features ; Compute initial alignment T k ← T k − T − k − T k − ; for Pose optimization is not converged do for each point p k ∈ P k do Transform to map coordinate ˆ p k ← T i − k p k ; if p k is an edge feature then Compute edge residual f E and Jacobianmatrix J E ; end if p k is a planar feature then compute planar residual f S andJacobian matrix J S ; end end if nonlinear optimization converges then Compute T ik ; end end Update current scan into local map and removeoldest scan from local map; Return current pose T k ; end angle (including roll, pitch, yaw angle change) is large; (3)If the time elapsed is more than a certain period. In practice,the rotation and translation thresholds are defined based onthe FoV of the sensor, while the minimum update rate isdefined based on the computational power of the processor. Toincrease the searching efficiency, an octree is used to constructthe global map. It only takes computational complexity of O (log n ) to search for a specific node from an octree of depth n , which can significantly reduce mapping cost [26]. For eachcell in octree, we use P ( n | z t ) to present the probability ofthe existence of an object [27]: P ( n | z t ) =[1 + 1 − P ( n | z t ) P ( n | z t ) · − P ( n | z t − ) P ( n | z t − ) · P ( n )1 − P ( n ) ] − , (14)where z t is the current measurement, z t − is the historicalmeasurements from key frames, P ( n ) is the prior probability,which is set to 0.5 for unknown area.IV. EXPERIMENT EVALUATION A. Experiment Setup
In this section we present experimental results of theproposed method. Our method is firstly evaluated in a roomequipped with a VICON system. Then, it is implementedon an Automated Guided Vehicle (AGV) that is used for -4 -2 0 2 4-3-2-10123
Ground TruthproposedLOAM
Fig. 3: Comparison among the proposed method, LOAM andground truth. LOAM loses tracking when the rotation speedis high while the proposed method is able to track accurately.The units are in meters.warehouse manipulation. We analyze the performance of theproposed method and compare it with existing LiDAR SLAM.To further illustrate the robustness, the proposed method is alsointegrated into a hand-held device used for 3D scanning. In ourexperiment, an Intel Realsense L515 is used for demonstration.It is a small FoV solid-state LiDAR with ◦ × ◦ viewingangle and 30 Hz update frequency. It is smaller and lighterthan a smart phone so that it can be used in many mobilerobotic platforms. The algorithm is coded in C++ and isimplemented on Ubuntu 18.04 and ROS Melodic [28]. In thefirst experiment, the proposed method is tested on a desktopPC with an Intel 6-core i7-8700 processor CPU. For theexperiment on the AGV and hand-held device, an Intel NUCmini computing platform is used which has an i5-10210Uprocessor. B. Performance Evaluation and Comparison
To evaluate the localization results, our method is comparedwith the ground truth, which is provided by a VICON system.A robot is manually controlled to move around the VICONroom of size 4m × cm . We also compared our method with LOAM [19] thatis widely used for LiDAR SLAM. The vertical angle andhorizontal angle inputs in LOAM are changed according to IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2021 (a) (b) (d)(e)(b)(c)
Fig. 4: Example of indoor localization and mapping in warehouse environment. (a) The AGV platform used for warehousemanipulation with a solid-state LiDAR mounted in the front. And the reconstructed map is shown in the middle. We randomlypicked two places for illustration. (b) and (d) are the original camera view. (c) and (e) are reconstructed scene based on theproposed approach. The trajectory is plotted in greenthe sensor properties of L515, while we keep the number ofedge and planar features unchanged. The result of LOAM isplotted in orange. It is obvious that tracking loss happens toLOAM when there is large rotation, while our method is stillable to track accurately.
C. Performance on Warehouse Robot
The algorithm is then evaluated on an AGV running ina warehouse environment. Smart manufacturing is one ofthe main applications for SLAM. In an advanced factory,the robot is supposed to transport, process, and assembleproducts automatically. This requires the robot to efficientlylocalize itself in complex and highly dynamic environmentswith moving operators and other robots. In this experiment,the proposed method is integrated into an industrial AGV thatis shown in Fig. 4 (a). The robot is used for griping andtransporting the materials with a robot arm on the top. A solid-state LiDAR is mounted in the front to provide localization.In this task, the AGV platform automatically explores thewarehouse and reconstructs the environment at the maximumspeed of 0.8 m/s. The localization and mapping results areshown in Fig. 4 (b) with the robot trajectory plotted in green.Our method achieves an average computing time of 42 ms on an Intel NUC mini computing platform. It can be seenthat our method is able to provide reliable localization incomplex environments. We compare the 3D mapping resultswith the images. The image view of the original warehouseis shown in Fig. 4 (b), (d) and the built 3D map is shownin Fig. 4 (c), (e). To evaluate the quality of mapping, wecompare the built objects with actual objects. Specifically, wemeasure the size of operating machines which are shown inFig.4 (b) and Fig.4 (d). The edge points from built object arepicked and the Euclidean distances between edge points aremeasured. The results are calculated by taking the average ofthe measured distances. The measured dimension and actualsize of the operation machine in Fig. 4 (b) are . m × . m and . m × . m , while the measured dimension and actualsize of operation machine in Fig. 4 (d) are . m × . m and . m × . m . It can be seen that our approach is also Solid-state LidarComputing PlatformRotating platform
Fig. 5: Integration of the proposed method on a hand-heldscanner. (a) Light weight hand-held scanner using solid-stateLiDAR as perception system. (b) Localization and mappingresult, the trajectory is plotted in green.able to build a high resolution 3D map that is close to realenvironment.
D. Performance on Hand-held Device1) 3D Mapping:
To further demonstrate the robustness, theproposed method is also evaluated on a hand-held device.With the growing of Virtual Reality (VR), Augmented Reality(AR), and gaming industry, SLAM has been implemented onvarious mobile devices such as smart phones and VR glasses.However, most mobile platforms have limited computationalresources. Compared to implementation on warehouse robot,
ANG et al. : LIGHTWEIGHT 3-D LOCALIZATION AND MAPPING FOR SOLID-STATE LIDAR 7
Method Success Tracking lossThe proposed method 6 0A-LOAM 1 5
TABLE II: Results of rotation test.the hand-held device suffers from vibration and large viewingangle change which can cause tracking loss and localizationfailure. Our system is built as a mobile scanner which is shownin Fig. 5(a). The hand-held scanner consists of three modules:perception module, rotation platform, and computing module.It is less than 500 grams and can be implemented on manyapplications such as drone navigation. In the experiment, wehold the 3D scanner and scan the indoor environment at normalwalking speed. The localization and mapping result is shownin Fig. 5(b), with the trajectory plotted in green. Our methodcan accurately localize itself and perform mapping in real time.
2) Rotation Test:
The hand-held device often has higherrotation changes which can result in tracking loss. In order todemonstrate the performance of the proposed method underlarge rotation, we put the solid-state LiDAR in horizontalorientation and randomly rotate the solid-state LiDAR withmaximum rotation speed of 1.57 rad/s. The solid-state LiDARreturns to horizontal orientation and we record the angledeviation. Tracking loss is considered when the final angledeviation is greater than 10 degrees. We compare our methodwith A-LOAM and the results are shown in Table II. It canbe seen that our method has a higher success rate comparedto A-LOAM. V. CONCLUSIONIn this paper, we present a full SLAM framework for solid-state LiDAR which is an emerging LiDAR system with higherupdate frequency and smaller FoV compared to traditionalmechanical LiDAR. Our system mainly consists of rotationinvariant feature extraction, odometry estimation, and proba-bility map construction. The propose method is able to supportreal time localization and densed mapping on an embeddedmini PC. Thorough experiments have been performed toevaluate the proposed method, including experiments on awarehouse AGV and a hand-held mobile device. The resultsshow that the proposed method is able to provide reliable andaccurate localization and mapping at high frequency. It can beimplemented on most of mobile platform such as UAV andhand-held scanner. We have made the source codes publiclyavailable. R
EFERENCES[1] T.-M. Nguyen, Z. Qiu, T. H. Nguyen, M. Cao, and L. Xie, “Persistentlyexcited adaptive relative localization and time-varying formation of robotswarms,”
IEEE Transactions on Robotics , vol. 36, no. 2, pp. 553–560,2019.[2] H. Zou, M. Jin, H. Jiang, L. Xie, and C. J. Spanos, “WinIPS: WiFi-basednon-intrusive indoor positioning system with online radio map construc-tion and adaptation,”
IEEE Transactions on Wireless Communications ,vol. 16, no. 12, pp. 8118–8130, 2017.[3] H. Hong and B. H. Lee, “Probabilistic normal distributions trans-form representation for accurate 3d point cloud registration,” in , 2017, pp. 3333–3338. [4] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial odometryand mapping,” in , 2019, pp. 3144–3150.[5] J.-E. Deschaud, “IMLS-SLAM: scan-to-model matching based on 3Ddata,” in , 2018, pp. 2480–2485.[6] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: aversatile and accurate monocular SLAM system,”
IEEE Transactionson Robotics , vol. 31, no. 5, pp. 1147–1163, 2015.[7] T. H. Nguyen, T.-M. Nguyen, M. Cao, and L. Xie, “Loosely-coupledultra-wideband-aided scale correction for monocular visual odometry,”
Unmanned Systems , vol. 8, no. 02, pp. 179–190, 2020.[8] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale,“Keyframe-based visual–inertial odometry using nonlinear optimiza-tion,”
The International Journal of Robotics Research , vol. 34, no. 3,pp. 314–334, 2015.[9] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocularvisual-inertial state estimator,”
IEEE Transactions on Robotics , vol. 34,no. 4, pp. 1004–1020, 2018.[10] A. Pfrunder, P. V. Borges, A. R. Romero, G. Catt, and A. Elfes,“Real-time autonomous ground vehicle navigation in heterogeneousenvironments using a 3D LiDAR,” in , 2017, pp. 2601–2608.[11] A. Bircher, K. Alexis, M. Burri, P. Oettershagen, S. Omari, T. Mantel,and R. Siegwart, “Structural inspection path planning via iterativeviewpoint resampling with application to aerial robotics,” in , 2015, pp.6423–6430.[12] H. Wang, C. Wang, and L. Xie, “Intensity scan context: Coding intensityand geometry relations for loop closure detectiong,” in , 2020.[13] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and R. Daniela,“Lio-sam: Tightly-coupled lidar inertial odometry via smoothing andmapping,” in
IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS) , 2020.[14] J. Zhang and S. Singh, “Visual-lidar odometry and mapping: Low-drift,robust, and fast,” in , 2015, pp. 2174–2181.[15] L. Wallace, A. Lucieer, C. Watson, and D. Turner, “Development ofa UAV-LiDAR system with application to forest inventory,”
Remotesensing , vol. 4, no. 6, pp. 1519–1543, 2012.[16] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision LiDARodometry and mapping package for LiDARs of small FoV,” in ,2020, pp. 3126–3131.[17] G. C. Burdea and P. Coiffet,
Virtual reality technology . John Wiley &Sons, 2003.[18] P. J. Besl and N. D. McKay, “Method for registration of 3-D shapes,”in
Sensor fusion IV: control paradigms and data structures , vol. 1611.International Society for Optics and Photonics, 1992, pp. 586–606.[19] J. Zhang and S. Singh, “LOAM: Lidar Odometry and Mapping in real-time.” in
Robotics: Science and Systems , vol. 2, no. 9, 2014.[20] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimizedlidar odometry and mapping on variable terrain,” in ,2018, pp. 4758–4765.[21] D. Chetverikov, D. Svirko, D. Stepanov, and P. Krsek, “The trimmediterative closest point algorithm,” in
Object recognition supported byuser interaction for service robots , vol. 3. IEEE, 2002, pp. 545–548.[22] K. Koide, J. Miura, and E. Menegatti, “A portable three-dimensionalLiDAR-based system for long-term and wide-area people behaviormeasurement,”
International Journal of Advanced Robotic Systems ,vol. 16, no. 2, p. 1729881419841532, 2019.[23] D. Rozenberszki and A. Majdik, “LOL: Lidar-only Odometry andLocalization in 3d point cloud maps,” in , 2020.[24] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision meets robotics:The KITTI dataset,”
The International Journal of Robotics Research ,vol. 32, no. 11, pp. 1231–1237, 2013.[25] T. D. Barfoot, “State estimation for robotics: A matrix lie groupapproach,”
Draft in preparation for publication by Cambridge UniversityPress, Cambridge , 2016.[26] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard,“OctoMap: An efficient probabilistic 3D mapping framework based onoctrees,”
Autonomous Robots , vol. 34, no. 3, pp. 189–206, 2013.
IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2021 [27] H. Moravec and A. Elfes, “High resolution maps from wide angle sonar,”in
Proceedings. 1985 IEEE International Conference on Robotics andAutomation , vol. 2, 1985, pp. 116–121. [28] M. Quigley, B. Gerkey, and W. D. Smart,