Registration of Low Cost Maps within Large Scale MMS Maps

Transcription

1 2016 IEEE 18th International Conference on High Performance Computing and Communications; IEEE 14th International Conference on Smart City; IEEE 2nd International Conference on Data Science and Systems Registration of Low Cost Maps within Large Scale MMS Maps Simon Thompson, Masashi Yokozuka, Naohisa Hashimoto and Osamu Matsumoto Smart Mobility Research Group, Robot Innovation Center National Institute of Advanced Industrial Science and Technology Tsukuba, Japan Abstract High resolution 3D mapping of road systems is currently being carried out by expensive Mobile Mapping Systems (MMS) but coverage is limited. Recently Low Cost Sensor (LCS) systems have been developed which use common, low cost, internal MEMS position sensors from mobile phones, but such sensors come with a reduced absolute and relative positional accuracy. This study investigates the registration of LCS maps within MMS maps to improve map coverage and lower costs. MMS and LCS maps of a real world environment are made and registration is performed using feature matching and Iterative Closest Point alignment. Accuracy of ICP alignment is approximately 10cm and local convergence is possible up to 1m. A combination of feature matching and ICP is used to demonstrate accurate alignment from an initial error of 10m. An example of a LCS map aligned within a MMS map is presented to confirm the use of LCS systems to extend 3D mapping coverage. I. INTRODUCTION Large scale 3D mapping of road systems and the surrounding environment is becoming common in the drive to provide road users and autonomous vehicles with future information services such as route simulation and autonomous navigation. High resolution mapping of road systems is currently being carried out by expensive Mobile Mapping Systems (MMS)[1]. Because of the high cost of such systems, the coverage of 3D maps is limited. Recently, a Low Cost Sensor (LCS) system has been developed which can build 3D maps with common low cost position sensors inside mobile phones [2]. Such sensors however, result in sparser maps of lower absolute and relative positional accuracy in comparison to MMS maps. Maps can also be constructed at different times, causing significant inconsistencies between them. The question arises, can LCS maps be merged with MMS maps? If so, total 3D map coverage could be significantly extended by lowering the cost of mapping systems and allowing smaller enterprises to make local LCS maps and merge then with larger, national scale, MMS maps. In this work, MMS and LCS maps were constructed one month apart and registration of LCS maps within the MMS map was attempted using known techniques such as feature matching and the Iterative Closest Point algorithm. Accuracy of registration and area of local convergence are reported. The contribution of this work is to demonstrate the possibility of registration of LCS maps within high resolution MMS maps to correct LCS positional inaccuracy, improve total map coverage, and thus lower 3D mapping costs. II. 3D MAPPING A. MMS Maps To facilitate autonomous navigation of vehicles detailed geometric maps of the environment are required. Such maps should include 3D modelling of the environment as well as be accurately aligned to global coordinate systems. Recently Mobile Mapping Systems (MMS) have been developed to produce such maps, typically using laser scanners to measure 3D shape and high accuracy positioning devices, such as RTK-GPS, fibre optic gyroscopes and wheel encoders, to map this data to global coordinates [1]. These systems typically mount 2D laser scanners vertically on a vehicle and accumulate laser scans while calculating accurate position information along the vehicle s trajectory. Such systems are expensive, because of the high accuracy of the positioning devices employed, and most are deployed by large scale companies seeking to provide information services to the emerging automated transport industry. B. Low Cost Sensor Maps To overcome the high cost of such systems, and to make mapping accessible to smaller enterprises, [2] propose a LCS mapping system which uses low cost, and widely available, positioning devices to perform dense 3D mapping. The system employs the usual 2D laser scanner, but uses microelectromechinal system (MEMS) positioning devices found in consumer products such as smart phones. Using the MEMS gyro, accelerometer and GPS unit of a Sony Xperia Z Ultra smart phone, and the velocity of the vehicle (obtained through the CAN bus communications) the system estimates vehicle trajectory and the 3D map of the surrounding environment to a standard that is permissible for auto-driving use. This standard is defined as having a relative trajectory position estimation error of less than 10cm. Simulated results suggest that this error can be achieved using low cost GPS sensors with less than 10m standard deviation in position, well within the accuracy limits of recent consumer products. The absolute trajectory error of the system is reported to be about 1m. Such a system /16 $ IEEE DOI /HPCC-SmartCity-DSS

2 Figure 1: Registering a Low Cost Sensor (LCS) map of a side street within expensive Mobile Mapping System (MMS) built maps of main road. could increase the coverage area of 3D maps by increasing the number of mapping systems available to deploy or by enabling access to mapping technology to small enterprises or individuals who might have more incentive to map low traffic areas. C. Registration of LCS Maps within MMS Maps Once MMS maps and LCS maps have been constructed, the question arises can they be merged together to form larger but still consistent maps. Figure 1 shows a scenario where a LCS map of a side street has been constructed along with a MMS map of a main road. The two maps share common points where there is overlapping scan data. The goal of this study is to determine whether existing 3D map registration techniques are sufficient to align the two types of maps around the common points, and subsequently merge them, given the expected degree of relative and absolute position error reported in LCS mapping, and the between map inconsistencies due to differences in construction date and data resolution. Previous work such as [3] and [4] have demonstrated large scale registration of city scale point clouds, containing large amounts of overlapping data, all captured using highly accuracte, expensive GPS/IMU systems. They both decompose the data sets into smaller loops and perform registration within loops and then between them. The current study specifically looks at the case where overlap is minimal, and one data set is constructed using LCS mapping systems. D. Overview The rest of the paper is as follows: Section III describes the well known registration techniques that are used in this study. Section IV describes the experimental setup, including the experiment location and details of the MMS and LCS maps. Section V presents the results of the registration process. Finally Section VI provides some conclusions and discussion of future work. III. REGISTRATION METHODS Registration, in this context, refers to the pairwise alignment of a 3D point cloud with another reference 3D point cloud. There is an assumption that a significant amount of the point data in each cloud represent common surface elements of the mapped environment. The output of this process is an estimate of the positional and rotational transformation needed to align the point data. A common approach to this problem is the Iterative Closest Point algorithm (ICP) [5], which, along with a variant point-to-plane ICP [6], is detailed below. ICP works by iteratively finding the corresponding (in this case closest ) data points in the point cloud data sets and then estimating a transformation that will reduce the alignment error between the two sets. The convergence of this process is subject to local minima and as such an accurate initial estimate is required. In the absence of a suitable initial estimate, feature based matching between data sets can be applied to produce an improved initial estimate which can be used as input for ICP. In the current work, the initial estimate is generated by GPS devices and the trajectory estimation process of the two mapping systems. The implementation of this work makes use of the PCL software library [7]. A. Iterative Closest Point (ICP) To align two point clouds, P = {p 1,...,p np } and Q = {q 1,...,q nq }, we must calculate the translation t and rotation R that minimises the sum of the squared error between corresponding points in each data set: E(R, t) = 1 N p p i Rq i t 2 (1) N p i=1 For ICP, the corresponding points p i and q i are the closest points found using a k-d tree data structure and nearest neighbour search. For each point cloud, the center of mass (the means μ p, u q ) is subtracted from every point to give centered point clouds P,Q. Then, If we let N p W = p iq T i (2) i=1 then the singular value decomposition (SVD) of W is: W = U σ σ 2 0 V T (3) 0 0 σ 3 where σ 1 σ 2 σ 3 are the singular values of W. Using this, we can solve for R and t: R = UV T t = μ p Rμ q and apply this to align the data sets. Because the corresponding points are not generally exact correspondences, it is necessary to iterate this process until an error threshold is reached or convergence on a local minima occurs. 1305

3 B. Point-to-Plane ICP One variant to ICP which is particularly important in applications in which point clouds contain representations of large planar surfaces is Point-To-Plane ICP [6]. Instead of using closest point to determine correspondences, here point to plane distance is used, allowing points to slide along planar regions when converging. The surface normal of a given point is calculated by analysing the eigenvectors and eigenvalues of the covariance matrix of the surrounding points. In this case we replace Equation 1 with: E(R, t) = 1 N p η i (p i Rq i t) 2 (4) N p i=1 where η i is the surface normal at q i. C. Feature Matching If the initial alignment estimate lies outside the ICP convergence basin of the correct alignment, feature based matching can be used to produce a more accurate initial estimate. In this case we use 3D Harris features [8] to identify key-points P K,Q K in each point cloud. Fast Point Feature Histogram (FPFH) feature descriptors [9] are extracted for each key-point. Then we apply a sampling consensus algorithm to estimate an initial alignment between the two point clouds as described in [9]: 1) First select a set of sample points s P from P K, ensuring the pairwise distance of all points are greater than a set threshold. 2) Then for each s P i in s P, using the FPFH descriptors, find the top N similar features from Q K, and randomly select one of these, s Q i to nominate as a corresponding point for s P i. 3) Finally for the set of corresponding points s P and s Q compute an associated rigid transformation and error metric. Iterating over these steps, a large number of transformation possibilities can be evaluated and the best can be selected as an initial alignment estimate to use as input for the ICP process. Applying this to point clouds with large amounts of vegetation produces noisy results. Therefore we weighted the random selection of s Q i in step 2 above by the probability points lie within the initial alignment error (10m from worst case GPS error) additionally constrained by a rough alignment (within 1m) of the ground plane in both data sets. Ground plane is naively assumed to be most frequently occurring discretised height from points around reference location. Improved accuracy in ground plane alignment is certainly possible, but this quick method works sufficiently with the point clouds described below. Figure 2: Experimental environment: approximately 1km sq. Tsukuba campus of AIST with 3 Reference Points. Figure 3: Bounding boxes of LAS files and GPS based vehicle trajectory for MMS map of experiment environment. IV. EXPERIMENT SETUP A. Experiment Environment A MMS map of the roadways within AIST Tsukuba campus was constructed. (Figure 2 shows an aerial view of the experiment location). The mapped area covers approximately 1km square. Three reference locations within the MMS map, A,B and C were identified. Two separate LCS maps were constructed, mapping the route between points A and B, and A and C respectively. The reference points were chosen due to their closeness to 3 markers whose geographical location is known with high precision. The LCS and MMS maps overlap in the vicinity of the three reference points. GPS coordinates of the vehicle paths can be used to initially align the maps, but this alignment is subject to the error inherent in the GPS device. ICP is performed only around the reference points, as in the worst case scenario, the maps will only overlap at start/end locations along the LCS path. B. MMS and LCS Maps The MMS map was constructed by Kokusai Kogyo company and is in LAS data format and consists of 458 data files each containing 3D point clouds covering 50m square 1306

4 Sub-sample (m) s=0 s=0.1 s=0.2 s=0.5 LCS 370,489 81,241 24, MMS 1,479, ,949 27, Table I: Voxel grid sub-sampling: Number of points in subsampled maps for various values of s (d =25m). scan coverage, which can reduce registration accuracy. Figure 5 shows views of the area around reference point A from both the MMS map and the LCS map. Figure 4: LCS map of experimental environment: map shows path between reference points A and B. regions of the environment. Each file contains and average of 1.06 million points for a total of approximately 484 million. Figure 3 shows the bounding boxes of each MMS data file as well as the GPS trace of the vehicles trajectory. The low cost sensor map between reference points A and B, in comparison, contains just over 17 million 3D points (see Figure 4). C. Data Preparation Due to the size of the maps it is impractical to register the entirety of the data sets. As in the case shown in Figure 1 the assumption is made that the maps will overlap at certain points, and registration should be performed at these points (in this case the 3 reference points). To prepare the data the following operations are preformed. First the maps are filtered within a radial distance around the reference points. Different radii are used to construct various sub-maps of both the MMS and LCS maps in order to investigate what size of sub-map is necessary for registration. A maximum distance of 75m is imposed as typical laser range finders have a limit of 80m, so scanned point data overlap may be limited to that distance. a) b) Figure 5: Example views at reference point A: a) MMS map, b) LCS map. Note differences in presence/absence of people, vegetation and resolution of data points. The MMS map has much higher point density. As the maps were constructed one month apart there are other substantial differences such as changes in vegetation, the presence/absence of vehicles and pedestrians, and gaps in Figure 6: Distance filtered sub-regions around ref. pt. A, for various radii d, and number of data points in LCS/MMS maps. Figure 6 shows point clouds constructed around reference point A in the LCS map. The text in the figures show the radius of the distance filter as well as the amount of points in the resulting point clouds. In addition, the distance filtered sub-maps are further reduced using voxel grid based subsampling in order to investigate point density required to perform registration. This also ensures a regularly spaced distribution of points, which may not be the case in the raw map data. Higher sub-sampling also allows for faster computation times. Voxel grid sub-sampling replaces all point data within a voxel of grid size s with the centroid point of that data. Table I shows the number of points in the point clouds resulting from sub-sampling of the LCS map (distance filtered d =25) around point A for different voxel grid sizes s. 1307

5 D. Iterative Closest Point Alignment ICP registration can then be performed on the filtered submaps. To allow for evaluation of ICP registration convergence, the LCS sub-map is first manually aligned to the MMS sub-map using visual inspection. From this true alignment, the LCS map is then transformed a known offset distance. The offset LCS sub-map and the MMS sub-map are then input into the ICP algorithm. The ICP estimated transform can then be compared to the initial offset transform to calculate alignment error. For instance, offset LCS sub-maps are constructed for offset transforms from 0.2m to 2m along the x axis, to evaluate the possibility of registration within the 1m absolute position error in LCS maps reported in [2]. E. Initial Alignment with Feature Matching To perform alignment for position errors up to 10m (if only GPS device standard deviation error is considered), an initial alignment step of feature matching may be required. As described above sub-maps are constructed around the common points and the feature matching process detailed in Section III-C is applied. V. REGISTRATION RESULTS A. ICP Registration Basic ICP registration was applied to align X axis offset reference point A LCS sub-map with MMS maps for offset values of tf =[0.2, 0.4, 0.6, 0.8, 1.0, 2.0] (meters). No subsampling was applied (s =0), and the distance filter radius was r =15m. Figure 7 shows the initial offset and ICP aligned sub-maps for the case when tf =1.0m. An overhead view is shown in a) while b) shows a close up of a concrete structure. For scale, the object to the right of image b) is the front of a car. Figure 8 shows the error in the ICP estimate over the number of iterations of the ICP algorithm for various initial values of tf. At iteration 0, the error is equal to the initial offset. As ICP progresses the transform estimate for all offset values falls, all converging except when tf = 2.0m. By iteration 5000, computation time was already greater than 4 hours and the process was terminated. The estimated transform converges at about 0.1m error from the true estimate. This could reflect inaccuracy in manually assigned true alignment, or limit of convergence due to low resolution in LCS data. The consistency in the alignment error over many trials suggests the former. 1) Point cloud sub-sampling: To reduce computation time and to ensure a regular and consistent point density sub-sampling of the sub-maps was applied. For the basic ICP algorithm however, registration with any sub-sampling (s >=0.1) failed to converge. This appears to be caused by the predominance of planar surfaces in the environment, with sub-sampled points centered within voxel grid cells along a) b) Figure 7: Example ICP result for r =15m, s =0,tf = 1.0m: a) top view of initial offset (left), and ICP aligned (right) point clouds; b) close up, before (left), and after ICP (right). Error (m) Error in ICP Transform Estimate for d=15m, All Points offset=0.2 offset=0.4 offset=0.6 offset=0.8 offset=1.0 offset= # Iterations Figure 8: Error in ICP transform estimate against number of ICP iterations for various initial offset values along X axis, distance filter d =15m, subampling s =0m). 1308

6 planes resulting in inescapable local minima. Therefore point-to-plane ICP registration was applied. B. Point-To-Plane ICP Registration Using the point-to-plane variant of the ICP algorithm, sub-sampled sub-maps of s =[0.1, 0.2, 0.5] were tested for reference point A (d =15m). All showed degradation of performance compared with no sub-sampling (s = 0.1m failed to converge for tf =1.0m), although computation time was reduced dramatically (for instance convergence of tf = 0.8m in 20s compared to 2hrs). Due to the computation savings s =0.1m was chosen to evaluate larger sub-maps despite the cost in performance. 1) Registration with different distance filter radii: Pointto-plane ICP was applied to reference point A with s = 0.1m for tf < 2.0m over various values of distance filter radii d = [15, 25, 50, 75]m. From d >= 25m, the ICP transform estimate converges at 0.1m for all tf <= 1.0m, with number of iterations, and computation time, increasing (convergence for d =75m, tf =1.0m is 4000s). After convergence the remaining error is quite similar, except for d =75m, which is 0.05m, supporting the common sense notion that a larger area of overlap between the two maps is beneficial. 2) Offset transforms along Y and Z axes: To investigate whether the same performance could be generalised to other initial offsets point-to-plane ICP was applied to offsets along the Y and Z axes. Similar convergence results were observed, and in the Y axis where convergence was achieved in the case of a tf =2m as well. Error (m) Error in ICP Ref. Pt. 2 for d=25m, Sub Sample 0.1 offset=0.2 offset=0.4 offset=0.6 offset=0.8 offset=1.0 offset= # Iterations Figure 10: Error in ICP transform estimate against number of ICP iterations for various initial offset values along X axis, distance filter d =25m, subsampling s =0m) for reference point B. Figure 11: Registration about reference point B. Inconsistencies in LCS map (pink), can be seen such as multiple vertical surfaces in middle of image. Figure 9: Distance filtered (d =25m), subsampled (s = 0.1m) maps of regions around reference points B (left), and C (right). 3) Registration at other reference points: To further confirm performance, point-to-plane ICP was applied to sub-maps extracted from around the other reference points. Using d =25m, s =0.1m sub-maps for reference points B and C were constructed as above (see Figure 9). Figure 10 shows the ICP registration results for these sub-maps for point B. Point C converged similarly to point A. However, convergence at reference point B is significantly lower than A and C. Upon inspection, this can be attributed to LCS map inconsistencies as shown in Figure 11. C. Registration with Feature Based Initial Alignment The above results show that ICP can reliably align common areas in MMS and LCS maps with initial offsets of 1m or less. For map offsets greater than this, a feature based initial alignment may be necessary before ICP can be applied. Figure 12 shows the application of feature based initial alignment to the case when the alignment offset is tf = 10m. Figure a) shows a side view of the misaligned submaps. The 3D Harris features of the two sub-maps are shown as darker blue and red points respectively. Figure b) on the left shows an overhead view of the sub-maps after sampling consensus alignment. This alignment can then be used as an initial guess for the ICP process. The right of Figure b) shows a close up of the feature aligned sub-maps before ICP is applied, and after. Accurate registration is achieved even for an initial alignment offset of 10m. 1309

7 a) (a) LCS map of path 1 b) Figure 12: Feature based initial alignment and subsequent ICP registration: a) side view of LCS and MMS maps offset by 10m along X axis. 3D Harris keypoints shown as darker, larger points; b) Overhead view after feature based initial alignment (left), and a closeup after feature alignment for input to ICP (right, top), and after ICP (right, bottom). (b) Close up of Ref. Point A D. Registration of LCS Map within MMS The above techniques were applied to registration of LCS map path 1 (from points A to B) into the MMS map. Both maps were converted into a common reference frame according to GPS data. Sub-map regions were extracted around the two reference points and registration performed as above (feature based initial alignment, then point-to-plane ICP). The two resulting transformation estimates between the LCS map and MMS map were then averaged to calculate the final transformation, which was applied to the data points in LCS map of path 1. The results of this alignment are shown in Figure 13, demonstrating the possibility of registering LCS maps within MMS maps, despite the increased alignment error at reference point B. Registering LCS map of path 2 (points A to C) produced similar results. VI. CONCLUSIONS Registration of LCS maps within MMS maps was evaluated in a test case scenario within AIST Tsukuba campus. Point-to-plane ICP registration successful aligned the two maps around reference points within a 1m convergence basin. This is sufficient for the reported absolute position accuracy of the LCS map. Feature based initial alignment and (c) Close up of Ref. Point. B Figure 13: LCS map of path 1 registered within MMS map. LCS map shown in light blue. For illustration purposes, MMS data only shows points around ref. points A and B. successful subsequent ICP registration was demonstrated for a 10m offset, sufficient to register maps based on a raw low cost GPS position reading. Registration accuracy was estimated to be within 10cm, although this is dependent on the consistency of the LCS map. Further evaluation is needed to assess registration performance over a variety of environments and conditions. A method to identify optimal registration locations in the environment/maps (map consistency, strong location cues) would possibly improve performance. Developing registration methods that are specifically designed to handle large 1310

8 changes in the environment such as vegetation growth is an avenue for further research. Also, use of the estimated alignment transforms at the common points of LCS maps as known global location anchor points to redo mapping, could improve LCS map consistency. In conclusion, registration of LCS maps within MMS maps using existing registration techniques was achieved, demonstrating the possibility of using LCS mapping technology to extend road mapping coverage, and lowering the costs for entities to engage in mapping activities. ACKNOWLEDGMENT This work was supported by Increment P Corporation, 1-24 Nisshin-cho, Kawasaki, Kanagawa, Japan REFERENCES [1] C. V. Tao and J. Li, Advances in Mobile Mapping Technology, Taylor & Francis Group, [2] M. Yokozuka, N. Hashimoto and O. Matsumoto, Low Cost 3D Mobile Mapping System by 6DOF Localization using Embedded Sensors, In Proceedings of the International Conference on Vehicular Electronics and Safety (ICVES), Yokohama, Japan, November 5-7, [3] T. Pyvanainen, J. Berclaz, T. Korah, V. Hedau, M. Aanjaneya and R. Grzeszczuk, 3D City Modeling from Street Level Data for Augmented Reality Applications, In Proceedings of the International Conference on 3D Imaging, Modeling Processing, Visualization and Transmission, pp , 2012 [4] T. Shiratori, J. Berclaz, M. Harville, C. Shah, T. Li, Y. Matsushita and S. Shiller Efficient Large-Scale Point Cloud Registration using Loop Closures, In Proceedings of the International Conference on 3D Vision, 2015 [5] P. Besl and N. McKay, A Method for Registration of 3-D shapes, IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 15. No. 2, pp , 1992 [6] Y. Chen and G. Medioni, Object Modeling by Registration of Multiple Range Images, In IEEE International Conference on Robotics and Automation, Sacramento, California USA, April, [7] R. B. Rusu and S. Cousins, 3D is here: Point Cloud Library (PCL), IEEE International Conference on Robotics and Automation, Shanghai, China, May [8] C. Harris and M. Stephens, A Combined Corner and Edge Detector, In Proceedings of the Fourth Alvey Vision Conference, [9] R. B. Rusu, N. Blodow and M. Beetz, Fast Point Feature Histograms (FPFH) for 3D Registration, In IEEE International Conference on Robotics and Automation, Kobe, Japan, May