Characterization of a mobile mapping system for seamless navigation

: Mobile Mapping Systems (MMS) are multi-sensor technologies based on SLAM procedure, which provides accurate 3D measurement and mapping of the environment as also trajectory estimation for autonomous navigation. The major limits of these algorithms are the navigation and mapping inconsistence over the time and the georeferencing of the products. These issues are particularly relevant for pose estimation regardless the environment like in seamless navigation. This paper is a preliminary analysis on a proposed multi-sensor platform integrated for indoor/outdoor seamless positioning system. In particular the work is devoted to analyze the performances of the MMS in term of positioning accuracy and to evaluate its improvement with the integration of GNSS and UWB technology. The results show that, if the GNSS and UWB signal are not degraded, using the correct weight to their observations in the Stencil estimation algorithm, is possible to obtain an improvement in the accuracy of the MMS navigation solution as also in the global consistency of the final point cloud. This improvement is measured in about 7 cm for planimetric coordinate and 34 cm along the elevation with respect to the use of the Stencil system alone.


INTRODUCTION
Over the last decade, advanced research in the field of geomatics, robotics and information technology has revealed the advantages of using multi-sensors technologies and integrated solutions for digital documentation of complex environments, in the form of accurate 3D models, useful for several applications.The multi-sensor techniques based on photogrammetry and terrestrial laser scanning are common approaches for multiscale and multi-resolution environments (Gagliolo et al., 2018).These techniques, despite the ability to produce georeferenced and accurate three-dimensional models of the environment, often require time spending operations, concerning especially the planning of the surveys, manoeuvrability matters and the acquisition of control points for alignment (Bronzino et al., 2019).All these issues are carried out very well by mobile mapping systems (MMS) which collects highly precise point cloud data by means of a laser scanning system on moving platform integrated with aiding sensors for positioning and navigation (Puente et al., 2013).These systems, mainly used for short-range acquisitions, have the advantage of being portable, fast and flexible thanks to the integration of multiple sensors.The data extracted from MMS consists of a three-dimensional map of the detected environment in the form of a dense point cloud whose accuracy and consistency strongly depends on the algorithms of data fusion and estimation of the trajectory travelled.
Usually, when these systems operate in open environments, they are integrated with Global Navigation Satellite System (GNSS) technology in order to increase the quality of the map thanks to an accurate trajectory estimation.The problem of trajectory estimation represents the core as also the origins of MMS technology as they rely on algorithms originally developed for autonomous robotics navigation (Nüchter et al., 2007).While geodesy domains take into account the problem of data registration and georeferencing (Bedkowski et al., 2017), the robotics domain develops algorithms mainly focused on how to get their autonomous vehicles to move in indoor environment, where they couldn't rely on GNSS for navigation (Thrun, 2007).Among the many, the most important are the so called Simultaneous Localization and Mapping algorithms (SLAM) (Dissanayake et al., 2001), which simultaneously allows to estimate the position of the instrument thanks to the temporal features detected on the digital model generated during the motion (Tucci et al., 2018).These features are usually visual features acquired by a camera sensor or three-dimensional features extracted from the LiDAR point cloud (depth maps) which are used with the motion information obtained by an integrated Inertial Measurement Unit (IMU) and an Odometer.The integration of all these sensors, together with the use of the GNSS technology outdoor, makes MMS a great tool for seamless navigation.
Seamless positioning and navigation is the capability to estimate continuously the location of a body in the transition between outdoors spaces and indoors environments assuring accuracy, availability, continuity, reliability and integrity at different levels in function of the application requirements.Usually MMS address this problem estimating the platform staring pose outdoor, where they can rely on GNSS observations, then moving indoor where robust SLAM implementations (usually based on Extended Kalman Filters EKF) allows to estimates a fully correlated posterior over feature maps and vehicle poses (Kim et al., 2008).Unfortunately, while outdoor the GNSS provide reliable information for the robustness of the estimations, indoor the EKF-SLAM is inconsistent and introduce errors drift over the time (Bailey et al., 2006) (Huang, Dissanayake, 2007).
A solution to this problem is to hybridize the MMS with an indoor positioning technology which provides independent positioning solution as the GNSS outdoor.Among the many, an interesting solution is represented by Ultra-Wideband systems (UWB), a very popular indoor positioning and navigation systems based on impulse of radio frequency carrier-less signals (Oppermann et al., 2005).Being these impulses very short in time, the system operates in a very wide spectrum band which means the capability to measure and discretize transmission and reception times with high accuracy also in presence of obstacles.High time resolution means also precise range measurements and consequently, the possibility to set-up a network of transmitting sensors with well-known position which estimates the pose of a receiver trough multi-lateration (Dabove et al., 2018).These characteristics allows to develop a relatively simple hardware architecture which makes UWB a low-cost technology.Therefore, a network configuration is easily implementable and scalable in function of the coverage needs for indoor positioning (Sakr et al., 2020).This network of sensor can be seen as a constellation of fixed satellites with known position, therefore the UWB can easily replace the GNSS to support the SLAM algorithm in indoor environment.
The main aim of this research is to propose a preliminary framework for seamless navigation to assure a continuous positioning of an handheld mapping system thanks the integration of different positioning technologies.The MMS analyzed is the KAARTA Stencil 2-16 (Zhang et al., 2016), a commercial portable rapid mapping system, based on the localization and simultaneous mapping (SLAM) algorithm, which allows to produce rather dense and detailed 3D point clouds of the environment.This system has been integrated with GNSS and UWB technology in a multi-sensor platform able to acquire synchronized different data for further data fusion analysis.The GNSS receiver used is a dual-frequency, multiconstellation u-blox Neo M8T while the UWB system is the Pozyx accurate positioning system (Barral et al., 2019).Being a preliminary work, this paper presents the analytical measurement principles of the MMS, its characterization in term of accuracy and precision of the trajectory estimation and an analysis on the possible benefits of integrating this system with the aiding positioning technologies.The remainder of this paper is organized as follows.In Sec.II the Material and Methods used to perform the seamless navigation are provided.Hardware and software used to acquire real time data are well described together with the theoretical approach of the present work.Sec.III describes the experimental setup, the georeferencing procedure of the test area and the acquisition of the reference solution.In Sec.IV the results of the positioning estimation and his validation is discussed.

MATERIALS AND METHODS
In this work, positioning and navigation technologies have been applied in order to solve different aspects of seamless navigation.The choice of these technologies is due to the various aspects that make them complementary to each other and, in particular, to their operation in outdoor or indoor environments.The various sensors have been assembled into a single portable platform for pedestrian navigation capable of acquiring several information (Figure 1).The variety and the number of such information allow to investigate and apply different integration methodologies and algorithms to carry out the task of seamless navigation.The core of the multi-sensor platform is the KAARTA Stencil 2 mobile mapping system from Real Earth Company, composed by a Velodyne VLP-16 LiDAR system that simultaneously scans 16 rotating lines with different orientations, while the operator manually transports the device to acquire dense 3D data.The algorithms exploited by this technology consider the user motion through the rapid feature tracking, thanks to an implemented optical sensor, and an integrated low-cost Inertial Measurement Units (IMU).The KAARTA Stencil 2 can be integrated with a GNSS receiver to overcome the positioning problem and for geo-referencing the whole survey.In this work the GNSS module is the u-blox NEO-M8T, a dual frequency, multi-constellation receiver developed for automotive application.The multi sensor platform was composed by two of these sensors, one directly connected to the MMS, while the other operating in standalone configuration and managed by a Raspberry Pi 3 bi+.The same computational board is in charge to manage the UWB positioning system.In particular the Pozyx positioning system Developer Kit was used, a Real Time Locating System (RTLS) based on Two Way Ranging techniques.The inertial acquisitions are demanded to an IMU composed by a three-axis accelerometer, three axis gyroscope and three axis magnetometer and also a micro-barometer integrated on the Pozyx rover board.The details of the main characteristics of the hardware are reported in Table 1.The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLIII-B1-2020, 2020 XXIV ISPRS Congress (2020 edition)

Methodology / Framework
The integration algorithm is an estimator, i.e. a mathematical algorithm providing a systematic framework by which information from multiple source can be used.The framework must be structured in order to consider the type of information (i.e.sensors) used to perform the navigation task.Furthermore, the choice of sensors depends on numerous factors, such as the cost, the acquisition rate, the number of information provided, etc.Among these, one of the most important is the environment in which the sensors operate.For example, a GNSS receiver can be selected only to perform outdoor navigation while indoor a different technology is required.In this work, as already state, the multi-sensor platform operates both indoor and outdoor continuously to perform a seamless navigation solution.Therefore, two different setups have been used in indoor and in outdoor, both using the MMS as core of the solution (Figure 2).In these cases the most widely used estimator is the Extended Kalman Filter, a recursive filter which allows to incorporate information about past state and predict new state combing real time acquired measurements and dynamic motion models (Simon, 2006).Although the general idea is to fuse all the data in a single estimation procedure, in this preliminary work, the sensor integration has been made combining the single technology solutions (trajectory from GNSS, UWB and Stencil) using the tools provided by the MMS.The Stencil 2 processing framework is called Visual-LiDAR Odometry and Mapping and is based on the work reported in (Zhang, Singh, 2018).In general, the MMS data processing follows a two-step workflow: the visual-inertial odometry algorithm followed by the LiDAR odometry algorithm.The first algorithm perform a frame to frame motion estimation where each feature is associated with the depth map information obtained by the LiDAR.The visual odometry is supported by the IMU for optimization.The LiDAR-odometry step use the speed information deriving from the odometer to register the laser points in a local system.A mapping algorithm detects the geometric features in the point cloud and matches them in order to optimize the registration, using the pose constraints deriving from the previous step.This two steps solution therefore allows to compensate for each problem related of data loss, bad conditions, outliers, etc. related to any technology alone.

TEST SETUP
The above methodology has been applied to a real case study based on the acquisition of data from the multisensor platform, in continuous acquisition during a pedestrian motion.In order to acquire representative data of seamless navigation, the platform carried by the pedestrian user has followed a path that, starting from an open environment, develops towards an indoor space, and then returns outdoors.The site selected for this path is the geomatics laboratory of the Politecnico di Torino (Italy, Lat 45.063332°, Lon 7.660458° considering the WGS84 reference system with the UTM32N projection), an experimental laboratory that overlooks an outdoor terrace.The trajectory trave1led, in addition to presenting several changes of direction, also presents a variation in level in conjunction with the passage from the terrace to the laboratory.

Ground truth
In order to validate the results of the trajectory estimate according to the illustrated methodology, a reference trajectory was measured simultaneously with the data acquisition.This trajectory, used as ground truth for statistical analyzes, must be of greater accuracy than the solution to be validated.Therefore, a 360 degrees prism (Leica GRZ122), positioned on the multisensor platform, was traced with an acquisition rate of 1 Hz from two total stations able to autonomously lock to and track a prism target.The total stations used are the Leica MS50 and the Trimble S7, which have been located on the vertices of a small georeferenced topographic network.The georeferencing of this network took place by positioning two dual frequency geodetic GNSS receivers (Leica GS14 and GS18), in static acquisition for several hours on the materialized vertices.The observations were subsequently post-processed with a differential approach and the network compensated in order to obtain the coordinates of the points in UTM WGS84 32N.Table n shows the coordinates obtained.Having georeferenced the network, the components of the position of the prism also appear to be in cartographic coordinates.

UWB system setup
The UWB positioning system requires the installation of a network of fixed sensors (anchors) placed on the edge of the system's operating area, whose coordinates are known with respect to a local reference system.The knowledge of these coordinates, together with the range measurement between the mobile sensor (Tag) and the aforementioned anchors, allows to compute the position through different estimation procedures (tri-lateration, multi-lateration, etc.).In this work, 6 anchors were placed inside the laboratory and their position was calculated with high accuracy through detailed measurements (angles and distances) made with a total station.Again, the total station was placed on a known point belonging to the topographic network previously described.In this way it is possible to trace the local system back to a global georeferenced system.Consequently, also the tag position estimation results to be in cartographic coordinates.The coordinates in UTM WGS84 32N are shown in Table 2.
Figure 3. Graphical representation of the test with the pedestrian path moving from an outdoor space to an indoor environment.
The topographic network, the UWB anchors location and the total stations position are also shown.

KAARTA Stencil 2 data acquisition
The KAARTA Stencil 2-15 MMS acquire and store several data acquired from the different sensor integrated on the platform.

RESULTS AND DISCUSSION
As stated in the introduction section, this work provide preliminary results on trajectory estimation capabilities of the Kaarta Stencil 2 when it is used in continuity between outdoor and indoor spaces.Used alone, the mobile mapping systems provides algorithms and tools useful to obtain accurate point cloud of the environment and consistent localization of the sensor although not in absolute reference system.Section 4.1 discuss these results.The integration of indoor 3D models georeferenced in the geodetic coordinate systems and topologically connected to the outdoor maps is one of the major challenges to be resolved in order to ensure a spatial continuity between the indoor and outdoor conditions.In Section 4.2 these problems are addressed.In particular, the analysis has been focused on the different procedure to georeferencing the mobile mapping data which relies on GNSS technology outdoor and UWB technology indoor.A time-synchronized trajectory has been obtained by GNSS receiver and UWB tag attached to the platform.Figure 4 graphically separate the test site in three areas which represent the type of data acquired for the following analysis.
Figure 4. 3D point cloud of the environment.The blue area represents outdoor space where GNSS is available, the green area is the indoor environment covered by the UWB network, while the red one is the indoor space outside the UWB network.

Results of Mobile Mapping System processing
The raw data acquired during live scanning can be stored for further processing using different tools provided by the Stencil 2. Among the many, one of the most important is the Adaptive Data Replay tool which allows to replay the live scanning at lower speed and changing several parameters in order to maximize the scan matching process.This tool must be used to take into account the environmental variability which can affect the benefits of each raw data in the integration algorithm.For example, replaying data disabling the feature tracker could yield better points cloud and trajectories if the environment present strong lighting variations.Or, replaying data at lower speed could increase the scans registration quality in case of a fast change in the pedestrian movement.In this work the data replay tool has been applied maintaining all the raw data and changing only the processing speed from 5Hz to 10 Hz.At the end of the data post-processing, new point cloud and trajectory are automatically stored.
The Stencil 2 provides a metrics of how the incoming scans are registered with the new map data being generated during scanning.This metrics is called Localization Confidence and higher is the value, better is the registration of the scans along the acquisition time.From Figure 5 is possible to observe how the confidence metrics is higher for post-processed data (replay) with respect the real time, mainly at the end of the acquisition.Observing the estimation trajectory in Figure 7 seems that the mobile mapping system doesn't suffer of location drift along the planar axes as the starting point and the ending point are almost the same.These is true both for real time acquisition and for the post processing trajectory.Is also important to point out that the followed path presents several direction changes and it takes about 10 minutes to complete, therefore the results are good.The drift is instead present in Figure 8, which shows the trajectory plots along the X-Z axes.The benefits of the postprocessing here is evident with an elevation variation of about 2 meters.

Results of GNSS and UWB integration
Another tool for post processing provided by Stencil 2 is the loop closure tool which uses a set of functions in order to increase the overall consistence of the scan registration and trajectory estimation.The global drift errors are corrected using trajectory path that comes near each others performing a scan match.Moreover, the loop closure is composed also of a tool to process the Stencil raw data together with the external information of a positioning technology, like GNSS or UWB.This integration could be a rigid 3D roto-translation of the scans trajectory to the reference one in order to minimize the Euclidean distance between common points (georeference), or could be based on a fusion algorithm which consider the external positioning data as weighted observation to insert in the overall estimation function (georegister).The first approach doesn't attempt to morph, rubber sheet or deform the point cloud or trajectory in any way.The second process involves realign the scans in such a way as to align the trajectory with the external coordinates (GNSS or UWB) while still maintaining accurate scan matching between frames.
In this work the data acquired outdoor by the GNSS and indoor by the UWB system (both in the same geographic reference system WGS84 -UTM 32N) have been used to perform the algorithm described above.In particular the real-time and replay trajectories have been rigidly rotated and translated to be aligned with the GNSS/UWB solution.Then the mobile mapping system raw data have been post-processed together with the GNSS/UWB raw data to perform the loop closure algorithm and therefore the geo-registration of the data.GNSS and UWB data typically has various levels of accuracy, dropouts and outliers, especially in elevation, therefore the loop closure requires some weight factors to maintain the local consistence of the point cloud.Several processing have been performed in order to identify the best wight factors for the dataset.The final solution, reported in the work, has been obtained using an horizontal wight factor of 0.5 and a vertical weight factor of 0.1 which ensured a good geo-registration without overcorrecting the scans alignment.In order to assess the quality of the procedures to estimate a correct trajectory and therefore a correct 3D map of the environment, the navigation solutions have been compared with the ground truth solution obtained with the total station prism tracking.Figure 9 present the top view of the trajectory estimated with the previous described procedures together with the reference trajectory.Figure 10 shows the same trajectory from the North-Up view where is possible to observe the height drift for all the trajectory with respect to the reference one (dotline black plot).As previous observed, for real-time trajectory estimation (purple line), the drift is located at the end of the path, after the stairs.Observing replay trajectory (red line), this issue is solved although is still present a height deviation visible on the right part of the plot.Graphically, the green line representing the georegistered solution obtained with GNSS and UWB data, seems to solve most of the drifts.One should note that the WGS84 -UTM 32N coordinates have been translated in order to match the starting point of the route with the origin of the coordinate system.The error analysis on these reference points is provided from Table 5 to Table 7 while positioning accuracy specification are listed in Table 8.The root mean square errors (RMSE) along the East, North and Up coordinates are 24 cm, 35 cm and 50 cm respectively for the live scan trajectory rotated and translated rigidly to be aligned with the GNSS/UWB trajectory.These values decrease to 17 cm, 35 cm and 37 cm for the postprocessed trajectory (replay) providing an error drift correction along the altitude of about 13 cm with respect the real-time trajectory.These results, considering the acquisition time of about 10 minutes and the complexity of the test site, demonstrate the good performances of the Stencil 2 mobile mapping system.The root mean square errors of the trajectory obtained using the GNSS outdoor, the UWB indoor and the Stancil 2 raw data fused in the loop closure algorithm are 17 cm in East, 27 cm in North and 16 cm in Elevation.Along all the three components, the data fusion approach has provided a better solution estimation, demonstrating the benefits of using aiding data both for georeferencing the mobile mapping system scans and for obtain a better trajectory.Consequently, also the registration of the scans and the global consistency of the resulting point cloud benefits from this integration.

CONCLUSIONS
In this paper a commercial mobile mapping system, the Kaarta Stencil 2-16 is used as tool to address the problem of the trajectory estimation of a pedestrian moving in a complex environment composed by outdoor spaces and indoor areas.This problem, known as seamless navigation, consist in analyzing the environment, define the requirements and constrains of the positioning system and provide a procedure of sensors integration and data fusion.To do this, a multisensor platform consisting of several positioning technologies, was assembled and used to acquire numerous different data in a testsite.The Stencil 2 MMS is itself a multi-sensor platform which already solve well this problem, as it integrates different positioning technologies (LiDAR, Odometer, IMU, Camera sensor) and algorithms to provide a continuous solution also in the passage from outdoor to indoor.Unfortunately, the data provided by the system are in a local reference system, therefore it is customary to integrate these platforms with GNSS technology outdoor and other aiding systems indoor.In our case, the indoor positioning technology selected was the UWB systems which provide accurate position estimation (about 20 cm) in a global reference system.This paper is a preliminary analysis on the integrated indoor/outdoor seamless positioning system.In particular the work is devoted to analyze the performances of the MMS in terms of positioning accuracy and to evaluate its improvement with the integration of the aiding sensors.As preliminary step, the data fusion algorithm used are based on the tools provided by the Stencil 2 platform, while further investigation will take advantage of the numerous data collected to test other estimation algorithms.
The data acquisition consists in the platform carried by the pedestrian user following a path that, starting from an open environment, develops towards an indoor space to return back outdoors.A reference trajectory is acquired by two total station located on the vertices of a georeferenced topographic network.This reference solution allows to perform the accuracy analysis.Firstly, the MMS is used alone to analyze the discrepancies between the real-time trajectory estimation and the postprocessed one.The results showed that in real-time the MMS perform well although a drift of about 2 meters is present along the Z direction during the descent of the stairs.After applying the Data Replay tool, this drift is corrected.
GNSS data outdoor and UWB data indoor are acquired together with the MMS data during the test.All the data are time synchronized and the acquisition time offset is computed.Thanks to this, all the data can be fused to obtain a georeferenced seamless solution.In this preliminary work, two integration approaches are validated; a) a rigid roto-translation of the MMS data in order to fit the GNSS/UWB position (georeferenced) and b) an loop closure based integration which consider the external positioning data (GNSS/UWB) as weighted observation to insert in the overall estimation function (georegister).
The results showed that, if the GNSS and UWB signal are not degraded and therefore the position accuracy is good enough, using the correct weight to this observation, is possible to obtain an improvement in the accuracy of the MMS navigation solution as also in the global consistency of the final point cloud.This improvement is measured in about 7 cm for planimetric coordinate and 34 cm along the elevation.

Figure 2 .
Figure 2. Framework of seamless navigation.GNSS outdoor and UWB indoor are fused with the data acquired by the MMS during the pedestrian movement.The first setup, used outdoor, integrates GNSS with KAARTA Stencil 2-16 MMS while the second exploits the UWB positioning technology again combined with the one of MMS.In these cases the most widely used estimator is the Extended Kalman Filter, a recursive filter which allows to incorporate information about past state and predict new state combing real time acquired measurements and dynamic motion models(Simon, 2006).Although the general idea is to fuse all the data in a single estimation procedure, in this preliminary work, the sensor integration has been made combining the single technology solutions (trajectory from GNSS, UWB and Stencil) using the tools provided by the MMS.The Stencil 2 processing framework is called Visual-LiDAR Odometry and Mapping and is based on the work reported in(Zhang, Singh, 2018).In general, the MMS data processing follows a two-step workflow: the visual-inertial odometry algorithm followed by the LiDAR odometry algorithm.The first algorithm perform a frame to frame motion estimation where each feature is associated with the depth map information obtained by the LiDAR.The visual

Figure 5 .
Figure 5. Localization confidence plot for live scanning and data replay.The higher confidence means a higher local consistence of the scan registration as also an higher control on the trajectory estimation drift.Comparing previous figure with Figure 6 is possible to observe that the lower confidence value at the end of the scan acquisition in real time correspond to an error in the trajectory estimation along the Z axis which is highly reduced in the replay data.Zooming on this part of the plot is possible to observe the behaviour of the Z component which shows the user going downstairs.It is evident that the estimation of the position is more complex in this type of situation, where the movement of the person can change quickly and the odometer can measure wrong data.

Figure 6 .
Figure 6.Z component of the estimated solution during the movement.

Figure 7 .
Figure 7.The top-view trajectory estimation comparison between real-time and replay solutions.

Figure 8 .
Figure 8.The X-Z view trajectory estimation comparison between real-time and replay solutions.

Figure 9 .
Figure 9.The top-view trajectory estimation comparison between real-time, replay and GNSS/UWB solutions.

Figure 10 .
Figure 10.The X-Z view trajectory estimation comparison between real-time, replay and GNSS/UWB solutions.The difference between the mobile mapping position estimation and the reference position is computed for each component (East, North, Height) at the synchronized acquisition time.The trajectory points measured by the total stations during the pedestrian test are 1103 and they cover most of the path.Some sections of the route have not been measured as they are not visible from the two total stations (Figure11).

Figure 11 .
Figure 11.Positioning estimation errors of the three reference system components for real-time, replay and GNSS/UWB processing.

Table 1 .
Characteristics of the technologies, hardware and sensors used in the multi-sensor platform.

Table 2 .
Coordinates of georeferenced topographic network and UWB anchors in UTM-WGS84 32N.

Table 3 .
Table 3 summarize which information are provided by the sensors.Data provided by Stencil 2 MMS.

Table 4 .
Type of processing used to combine KAARTA Stencil 2 raw data, GNSS data and UWB data.

Table 8 .
Accuracy of the estimation.