HYBRID MODELS FOR TRAJECTORY ERROR MODELLING IN URBAN ENVIRONMENTS

This paper tackles the first step of any strategy aiming to improve the trajectory of terrestrial mobile mapping systems in urban environments. We present an approach to model the error of terrestrial mobile mapping trajectories, combining deterministic and stochastic models. Due to urban specific environment, the deterministic component will be modelled with non-continuous functions composed by linear shifts, drifts or polynomial functions. In addition, we will introduce a stochastic error component for modelling residual noise of the trajectory error function. First step for error modelling requires to know the actual trajectory error values for several representative environments. In order to determine as accurately as possible the trajectories error, (almost) error less trajectories should be estimated using extracted nonsemantic features from a sequence of images collected with the terrestrial mobile mapping system and from a full set of ground control points. Once the references are estimated, they will be used to determine the actual errors in terrestrial mobile mapping trajectory. The rigorous analysis of these data sets will allow us to characterize the errors of a terrestrial mobile mapping system for a wide range of environments. This information will be of great use in future campaigns to improve the results of the 3D points cloud generation. The proposed approach has been evaluated using real data. The data originate from a mobile mapping campaign over an urban and controlled area of Dortmund (Germany), with harmful GNSS conditions. The mobile mapping system, that includes two laser scanner and two cameras, was mounted on a van and it was driven over a controlled area around three hours. The results show the suitability to decompose trajectory error with non-continuous deterministic and stochastic components.


INTRODUCTION
Technology progress, society needs and also a limited availability of funds, have changed the way 3D data are collected, not only from the acquisition sensors point of view but also from the platforms point of view.Examples of this manned or unmanned platforms are satellites, planes, cars, bikes or more recently rovers, trolleys or even a mobile phones.Terrestrial mobile mapping (TMM) is a technology, complementary to aerial and satellite mapping, that allows 3D georeferenced data generation from terrestrial moving platforms.TMM has gained popularity allowing easy access to geoinformation, although with low accuracy, thanks to Google street view family systems and it might be boosted with experiences such as Google tango project for indoor mapping.
Getting precise and accurate trajectories, that is, a time series of positions, velocity and attitudes, is a key step to generate precise and accurate 3D georeferenced data.A point cloud is generated combining the platform trajectory together with laser scanner measurements with direct georereferencing techniques or from a set of images with known position and attitude.Centimetriclevel discrepancies during trajectory determination can lead to * Corresponding author differences between 10 and 50 cm between point clouds of overlapping areas in an urban scenario (Angelats and Colomina, 2014).
Currently, the trajectory of high-end TMM systems is mainly estimated combining GNSS, inertial and odometer data.Robust and precise positioning in an urban scenario, faces additional challenges than an open sky scenario as partial or total GNSS occlusions, or multipath, may occur.This causes an error to the platform or vehicle trajectory determination.Other sources of error can be Inertial Measurement Unit (IMU) modelling errors or system calibration errors.The system calibration error is an error in the determination of lever arm and boresight between IMU and camera or between IMU and laser scanner.The Integrated Sensor Orientation (ISO) method has been proven to be feasible and efficient for IMU-camera boresight calibration of mobile mapping systems (Kersting et al., 2012), for the IMU-laser boresight calibration with single and multiple laser scanners (Skaloud and Lichti, 2006), (Chan et al., 2013).
The need of high performance in terms of accuracy have made apparent the potential of using measurements derived from imaging sensors for improved trajectory estimation.In other words, imaging sensors measurements can be instrumental in estimating the parameters of error models that extend the trajectory models.In particular, cameras and laser scanners can help improving the trajectory at two different levels: sensor-level error modelling or trajectory-level error modelling.In the context of aiding at sensor level, visual measurements can be used to correct the drifts of the primary navigation sensors such as an IMU.Depending on a number of factors, one error modelling strategy may be more appropriate that the other.In this article, aiding at trajectory level refers precisely to the modelling of trajectory errors (like error in position or attitude) for a given trajectory, previously estimated by other system, for instance, but not necessarily, an INS/GNSS system (Angelats and Colomina, 2014).The first approach is suitable for applications that require real time navigation.The second approach is intended for the aforementioned applications to refine the trajectory in post-processing.
The first approach, usually known as visual aiding, refers to the computation of orientation parameters through consecutive, overlapping images by means of measurement of tie points i.e., photogrammetric observations of a same object point in two or more images.Alternatively, planes or cylinders extracted from laser scanner images, can also be used as tie features, especially in urban or indoor areas, where they are very common.Feature extraction and matching algorithms are commonly combined, with RANSAC procedures that aim to perform outlier detection and removal using solely image observations by means of position and attitude estimation (Nister, 2013), (Scaramuzza and Fraundorfer, 2011).More in detail, these algorithms estimate the relative orientation between two images, also known as relative pose, using n pairs of matched features, selected randomly among the full matched pairs.(Scaramuzza, 2011) proposes a method where the camera pose can be estimated with only one point correspondence by exploiting nonholonomic constraints of wheeled vehicles and a histogram-based voting strategy.Other approaches combine derived trajectory or inertial data to predict where a point feature should appear in the second image (Veth, 2011), (Leutenegger et al., 2013).Once all outliers have been detected and isolated, the camera or vehicle trajectory can be recovered by concatenating the estimated relative positions and orientations using k inliers from overlapping images.Several methods to estimate the navigation states using only image or using image and object observations are reviewed in (Scaramuzza and Fraundorfer, 2011).These approaches are usually referred as visual odometry (VO) or Structure from Motion (SfM) in the robotics and computer vision community.Alternatively, (Taylor et al., 2011) presents two strategies to use an IMU as a primary positioning sensor and to control inertial drift with visual information during filtering step.The two approaches are implemented using an Unscented Kalman Filter estimation method.The first approach imposes a geometric constraint using image coordinates while the second one takes benefit of jointly estimating a set of object coordinates together with navigation states.(Angelats et al., 2014) presents a method to robustly detect and isolate outliers in camera images using inertial-based trajectory in a first step and to navigate in GNSS-unfriendly environments, using corresponding tie points measurements together with inertial and GNSS measurements, when available, in a second step.(Schaer and Vallet, 2016) proposes to improve point cloud registration by recomputing the initial platform trajectory with position updates derived from ground control points identified in the point cloud.
Alternatively, the trajectory can be refined, in a single network adjustment.In this approach, the complete trajectory is refined simultaneously for all epochs, using observations extracted from images or point clouds from multiple imaging sensors and also ground control points.The trajectory error can be modelled using simple error models such as linear or polynomial segments (Angelats and Colomina, 2014).(Gressin et al., 2012) proposes a method where uses the Iterative Closest Point algorithm (ICP) as initial step for trajectory improvement.The overall registration is performed improving the original platform trajectory.(Elseberg et al., 2013) deals also with laser-to-laser registration by improving the platform trajectory.The trajectory is improved using a semi-rigid Simultaneous Localization and Mapping.
The previous techniques rely heavily on a capability to extract and match visual features or on having a dense network of ground control points.In addition, they make the assumption that visual measurements can be extracted and matched properly and that they can be extracted and matched continuously along the image sequence or point cloud.A part from that, trajectory error model using in the network based approach, may locally model well the error but not globally.Regarding the sequential based approach, a not proper stochastic characterization of observations derived from imaging sensors, might produce a filter divergence, and so, an estimated trajectory worse than the original one.
For that reason, we want to understand how the behaviour of TMM trajectory in urban environment is.In this paper we concentrate on the modelling aspects rather than overall trajectory refinement, that is, to study how the trajectory error of a TMM in an urban environment is.The final goal of this research is to provide knowledge to improve a trajectory of TMM system by rigorous modelling its error in two weak scenarios: when the available ground control points are limited and in scenarios with a weak geometry or non-continuous capability to extract and match non-semantic features.
The paper is organized as follows.Firstly, the main ideas of the proposed approach are presented.Then, the definition of nonsemantic features and their role for trajectory error modelling are introduced.The next subsection describes each of the error components of trajectory.The experimental results section presents the results using real data from a terrestrial mobile mapping camping with long GNSS outages.The last section summarizes the conclusions of the proposed approach and discusses future improvements.

PROPOSED APPROACH
This paper presents an approach to model trajectory errors of TMM in an urban environment.A real vehicle trajectory is a continuous function but the estimated one will probably be a discontinuous one.The error might be caused by GNSS multipath, IMU mismodelling or GNSS satellites occlusions.We propose to model the trajectory error with an hybrid model, that includes a non-continuous deterministic component and a stochastic one.
To face this approach, we propose a workflow presented in Figure 1.Reference trajectories are estimated using the initial platform trajectories estimation together with all raw observations from imaging sensors and all available ground control information.These references will be estimated using block adjustments including observations from multiple imaging sensors.In order to have useful observations from imaging sensors, non-semantic features from a sequence of images or from point clouds will be used.These non-semantic features describe certain attributes or properties of geometric objects or entities.These features allows to identify and match common object between images, between point clouds, or for instance between images and point clouds.These common or tie entities can be related through image, object coordinates or both, with the trajectory components using well-known models such as the collinearity equations.
Given the reference trajectories for several environments the trajectories errors are obtained subtracting TMM estimated trajectories from reference once.
Finally, for each environment, the trajectory error will be modelled as a non-continuous function.The analysis of the residuals of this identification will provide the stochastic characterization of the error.In this research, we exploit the capability of identifying common or tie features between images and/or between point clouds.Instead of exploiting the semantic content, we propose to use nonsemantic features.By non-semantic features we understood those ones that provide useful data to solve a certain task, such as positioning, but without understanding the image or scene content.
Examples of common tie features, used in our approach, are basic geometric primitives such as straight line segments, points, planes and ellipses.
In the last years an extensive research for extracting and matching primitives such as points and lines from a sequence of camera images has been done.Usually, these features are referred as visual features (Weinmann, 2012).However, different features can be extracted and matched also from point clouds such as planes, cylinders, toroid to mention a few.Moreover, new models combining different and joint camera-laser features can be exploited (Angelats and Colomina, 2014).For that reason, we prefer to use term non-semantic features instead of visual features to refer to features extracted directly or indirectly from imaging sensors.
These primitives are usually described with a set of attributes such as intensity, color, locally spatial relation with their neighbours, but such attributes can also describe aforementioned primitives response to a certain frequency bands or is temporal stability.These set of attributes, are used to identify and match homologous features in a sequence of images, or between overlapping point clouds.Nevertheless, these features by themselves, cannot provide any relevant information to describe or understand the scene.

Reference trajectory generation
A reference trajectory is generated using the Integrated Sensor Orientation approach.In it, all observations from non-semantic features, coming from single or multiple sensors, are processed together with trajectory (tPA) and ground control observations, in a network adjustment.We refer to camera-ISO when the imaging observations are tie points extracted from camera images.Alternatively, the reference trajectory can be estimated using laser only (ranges and scan-angles) or combined camera-laser observations (ranges, scan-angles and image coordinates).These observations belongs to the following tie features, planar surfaces for the laser ISO, and planar surfaces and straight line segments for the camera-laser ISO.In laser-ISO and camera-laser ISO, ground control points can be used to derive indirect control observations like ground control lines and planes.The ISO might provide also corrections in the boresight and lever-arm between the IMU and the camera or between the IMU and the laser scanner and also correction on camera self-calibration parameters.A not properly geometric calibration of imaging sensors and also an erroneous system calibration values might introduce also a systematic error component.

Deterministic model
Temporal geometric variation of the GPS and other GNSS constellations, produces a shift of the estimated positions from overlapping strips.Besides temporal geometric variation of GNSS, in an urban scenario, GNSS signals may be affected in several ways.For example, some of the signals can be completely blocked in several epochs, causing than a GNSS receiver cannot compute a solution and so introducing a drift in the platform trajectory.On other hand, the GNSS receiver can receiver direct or non-line of sight multipath introducing an additional error into trajectory.
In order to mitigate the impact of these factor, we introduce as part of trajectory error model a non-continuous function that minimize the residuals when fitted with the error data.Typically this function will be build upon linear shifts, drifts or polynomial functions.The discontinuities of the function will be mainly due to significant changes of constellation or problems with the odometer sensor.

Stochastic model
The stochastic model will be determined after the analysis of the residuals generated when fitting the previous non-continuous function to error measurements.It is expected that these residuals behave as a white noise or Gauss-Markov stochastic process.

Data set description
The proposed concept was tested and validated using real data from a professional mobile mapping system campaign over a controlled area of Dortmund (Germany).The surveyed area was an urban and GNSS challenging scenario, with variable and degraded GNSS conditions.However, it was an excellent dataset from the imaging sensors point of view with many structured buildings and large variety of features.
The mobile mapping system was an Optech Lynx system, from TopScan GmbH, that includes two laser scanner and two cameras.The cameras were mounted looking to each side of a street.
The system was mounted on a van and it was driven over a controlled area during three hours, resulting in 11 overlapping strips.The interest areas were additionally surveyed to provide a dense network of ground control points.After the survey, the system trajectory was computed using a tightly coupled approach combining differential GNSS, IMU and odometer measurements.
The areas used for testing the approach are shown in Figure 3. Two different sections or blocks, marked with blue and orange ellipses, were identified.Each block configuration was selected to represent different situations/configurations that can occur in urban environments with TMM platforms.In an urban environment, a TMM vehicle can survey an interest area several times with the same vehicle driving direction (orange case) or it can survey an area in opposite driving directions, and thus, exploiting and taking benefit from geometric diversity (blue).Table 1 provides the main characteristics for each of the sections, in terms of used equipment, number of strips, ground control support and number of tie points.Images from four different strips have been used to generate the reference trajectory for the blue section.The vehicle surveyed the area in the same direction three times and a last one in an opposite direction.The surveyed street in the orange block is a one way street, thus, camera images where acquired two times in the same driving direction.
The test areas had a dense network of ground control points and their distribution also varied between sections.For the blue section, 13 points provided ground control, 7 of them place in the left side of the street.Regarding the orange block, 8 ground control points were used, 5 placed on the right side of the street and 3 of them on the left side.The number of GNSS satellites changed considerably within and between overlapping strips.The number of satellites ranged from one to seven for the blue section, with 70% of the epochs with equal or less than 3 satellites (Figure 4).The GNSS geometry for the orange section was a bit worse than the blue one with all epochs with three or less satellites.Note that in Figure 4, the tracked satellites for a blue strip with elevation angles higher than 15 degrees are shown in blue, cyan and green.The remaining colours show visible satellites below 15 degrees and not recommended to be used for position estimation.
In addition, several GNSS satellites came in or out within few epochs in the same strip.
The reference trajectory was estimated processing the collected information with the Agisoft Photoscan software (Agisoft, 2015).
From both camera images, a dense set of points was extracted to be used as non-semantic feature in the adjustment.We benefited from both cameras to work with a stronger geometry.In order to improve the adjustment the system trajectory was also used as observation.
The geometric calibration values, as well as the lever arm and boresight between the cameras, and the IMU, were previously estimated with Agisoft Photoscan using a good GNSS conditions data set.The estimated geometric calibration values are the focal length, principal point, radial and tangential distortion coefficients.That subset included several strips in different directions to help to decorrelate internal camera parameters from the system calibration and from the exterior orientation parameters.
In-house software was developed for estimating trajectory error components, this is, to compare system and reference trajectories.

Experimental results
Using the reference trajectory and the system trajectory, a set of trajectory error values were computed.Figure 5 shows the error of the cross-track axis corresponding to four overlapping strips (blue, red, magenta and green).A position mean bias between strips can be clearly identified due to temporal changes on GNSS constellation.It is important to note here that this bias is smaller between the third and the fourth street because of their time difference (less than 5 minutes).Between the first and the second strip there is a difference of 38 minutes, and between the second and the third it is of 12 minutes.Besides the bias, short drift periods broken by several peaks in the trajectory error can also be identified.These peaks are mainly caused by discontinuities in the number of visible GNSS satellites.Moreover, the entrance of a GNSS satellite can produce several epochs of instability during platform trajectory estimation.This effect can be clearly identified in the magenta strip where two big peaks are present.
The error of the heading component of the blue section is shown in Figure 6, following the strip colour scheme of Figure 5.In contrast to the position error, a significant bias between strips cannot be observed.This is because platform attitude estimation step relies mainly on IMU observations.However, a new GNSS position after several epochs of inertial-based trajectory might introduce also changes in the attitude.This might explain the relevant peak that can observed in the magenta part of Figure 6.  the orange section.The comparison of both figures reveals that, at least for the selected sections, the error behaviour is similar, with short drift periods and several discontinuities.The plots for the remaining components are not shown because their similarity with the presented one.After the analysis of the residuals generated when fitting the previous deterministic segments to error measurements, the stochas-  In Table 2 the number of selected continuous segments for each test are presented together with the standard deviation values of the stochastic part, for each strip and each component.The figures reveal the presence of a stochastic component that can be modeled, in a first approximation, as a white noise process.It can be observed that the stochastic component is lower for the height than for the planimetric components.This can be explained by the use of non-holonomic constraints to reduce the height variation during the initial platform trajectory estimation.In addition, it can observed that stochastic component is similar in terms of standard deviation for the two sections.As it was expected, the stochastic component is independent of the street or area orientation.Beside the comparison between streets, the magnitude of stochastic component, both for position and attitude, remains similar within strips of the same section.The results presented in Table 2 indicate the driving direction has not a significant impact.
Figure 9: Stochastic components of a single strip of blue section.

CONCLUSIONS AND FURTHER RESEARCH
We present a strategy to determine as accurately as possible the trajectory error of a TMM system in urban scenarios.The rigorous analysis of these reference trajectory against the TMM estimated trajectories allows us to characterize the errors of a TMM system for a wide range of environments, and allows exploring in a near term, new and innovative methods for improving TMM trajectory.
The reference trajectory is obtained using extracted non-semantic features from a sequence of images and from a full set of ground control points.The trajectory error is modelled using hybrid The proposed approach has been evaluated using real data.The data came from a mobile mapping campaign over an urban and controlled area of Dortmund (Germany), with harmful GNSS conditions.The results show the suitability to decompose trajectory error with non-continuous deterministic and stochastic components.
The work presented in this paper sets the basis for exploring new methods to improve the trajectory of TMM in urban environments.Further research will be done to explore alternative methods aiming to improve trajectory in urban scenario independently of the density and type of non-semantic features and/or to reduce the number of required ground control points.In addition, we will explore the use vehicle dynamic constraints for trajectory estimation.Last but not least, we will study the feasibility to apply the same ideas to other mobile mapping platforms that might incorporate consumer-grade sensors.

Figure 4 :
Figure 4: Example of tracked GPS L1 satellites for one strip of the blue section.

Figure 5 :
Figure 5: Error component for the y axis from the four overlapping strips of blue section.The analysis of the error lead us to model it as a discontinuous third order polynomial.The discontinuities within this function were defined regarding change on strips and also changes in the number of available satellites in each section within a strip.On the other hand, we compute a single stochastic component for each component per strip.Figures7 and 8show errors in track axis marked with blue dots for one strip of the blue and one of the orange block.The figures also show the different segments of the error function.In the examples, the deterministic component was split, within a single strip, in three different segments (red, green, cyan) for the blue section and two (red and green) for

Figure 6 :
Figure 6: Heading error component from the four overlapping strips of blue section.

Figure 7 :
Figure 7: Trajectory error and deterministic part decomposition for the track-axis component of a strip of the blue section.

Figure 8 :
Figure 8: Trajectory error and deterministic part decomposition for the track-axis component of a strip of the orange section.
show the residuals for each of the error components, corresponding to a single strip for both blue and orange section.The stochastic component of along-track axis component is shown in red, cross-track in blue while height is shown in green.

Figure 10 :
Figure 10: Stochastic components of a single strip of orange section.models, that is, combining deterministic and stochastic components.The deterministic component can be modelled as a noncontinuous function composed by linear shifts, drifts or polynomial functions.In addition, we introduce a stochastic error component for modelling residual noise of the trajectory error function.

Table 2 :
The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences, Volume XLI-B3, 2016 XXIII ISPRS Congress, 12-19 July 2016, Prague, Czech Republic Stochastic component characterization tic model was determined.Figures 9 and 10