Long-term map maintenance pipeline for autonomous vehicles
Julie Stephany Berrio, Stewart Worrall, Mao Shan, Eduardo Nebot
JJOURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 1
Long-term map maintenance pipeline forautonomous vehicles
Julie Stephany Berrio,
Member, IEEE,
Stewart Worrall,
Member, IEEE,
Mao Shan,
Member, IEEE, and Eduardo Nebot,
Member, IEEE
Abstract —For autonomous vehicles to operate persistently ina typical urban environment, it is essential to have high accuracyposition information. This requires a mapping and localisationsystem that can adapt to changes over time. A localisationapproach based on a single-survey map will not be suitablefor long-term operation as it does not incorporate variationsin the environment. In this paper, we present new algorithmsto maintain a featured-based map. A map maintenance pipelineis proposed that can continuously update a map with the mostrelevant features taking advantage of the changes in the sur-roundings. Our pipeline detects and removes transient featuresbased on their geometrical relationships with the vehicle’s pose.Newly identified features became part of a new feature map andare assessed by the pipeline as candidates for the localisationmap. By purging out-of-date features and adding newly detectedfeatures, we continually update the prior map to more accuratelyrepresent the most recent environment. We have validated ourapproach using the USyd Campus Dataset [1] [2], which includesmore than 18 months of data. The results presented demonstratethat our maintenance pipeline produces a resilient map whichcan provide sustained localisation performance over time.
Index Terms —long-term localisation, feature-based map, mapupdate.
I. I
NTRODUCTION A N autonomous vehicle (AV) software stack has four maincomponents: perception, localisation, planning/control,and mapping. Perception provides information about the en-vironment in proximity to the AV. Localisation provides theposition of the vehicle within a local or global context. Plan-ning and control is responsible for the planning of trajectories,decision making and control algorithms. Finally, mapping isa requirement for each of the previous components [3]. Thelocalisation system requires an accurate map to determinethe vehicle’s position with centimetre-level accuracy. Themap can be also used for locating different dynamic objectsdetected by the perception system to plan the future vehiclemanoeuvres and share this information with other vehicles. Forthese reasons, maps are a fundamental component requiredfor the development and deployment of AVs. Building andmaintaining the maps to centimetre-level accuracy is essentialfor the reliability and safety of the system.Before AVs are deployed to public streets, a mappingprocess must be performed by estimating the location ofunique features (within a reference frame) which describe
J. Berrio, S. Worrall, M. Shan and E. Nebot are with the Aus-tralian Centre for Field Robotics (ACFR) at the University of Syd-ney, NSW, Australia. E-mails: { j.berrio, s.worrall, m.shane.nebot } @acfr.USyd.edu.au .Manuscript received on April XX, 2020; revised on Month XX, 2020. Figure 1:
Evolution of a construction site (closure, demolition andbuilding work) recorded in the Usyd Campus Dataset. the driving environment. The map must be detailed enoughto allow the localisation algorithm to match the observationsand identify the vehicle’s precise pose within it. Map buildingremains an arduous task due to a lack of standardizationfor the representation of the map, the amount/quality of datarequired, the cost of operating data-collection platforms, andthe processing time.For most AVs, computation power and storage capacityis limited, making the selection of the map representationimportant for real-time system performance. Different map-ping companies such as Tomtom [4] with ”RoadDNA” andCivilMaps [5] with ”Fingerprint maps” have achieved a signif-icant reduction in the amount of data required to perform mapmatching and maintain an acceptable localisation accuracy.These maps are built using a distinctive and detectable patternof features for the urban driving environment. It includes poles,traffic signals, road markings and/or voxel-based fingerprints.Feature-based maps for autonomous driving purposes can beas lightweight as 100 KB per km. In this way, the maps couldbe stored on-board, shared, and downloaded from the cloudefficiently using wireless communication.One of the main challenges in using maps for autonomousdriving applications is that a map created from data collectedon a given day might not be entirely valid the next week.This is due to potential changes in the environment suchas construction sites as shown in Fig. 1 or other temporarystructures. a r X i v : . [ c s . R O ] A ug OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 2
Figure 2:
Layers for long-term map maintenance. The prior maplayer at the bottom contains the output of the mapping process, thefeatures’ location within a global frame and the location from whichthey were detected. The new feature map layer in the middle consistsof newly detected features and their characteristics. The sensor modellayer on the top describes how the vehicle’s sensor system identifiesthe different features.
Maps require continuous updates because unexpectedchanges to the environment could cause problems with lo-calisation, or other system components, potentially resultingin an accident. It is essential to have algorithms capable of in-corporating the environmental changes into the map. There aretwo standard approaches to implement the map maintenanceprocesses. One is based on cumulative adjustment; actionsthat lead to preserve the original features while adding newfeatures in response to the changes. This approach has twomain disadvantages. Since all new features are added to themap, it grows without limit. At the same time, this makesthe data association more difficult between the observationsand the map. The second approach is based on transformativeadjustments; operations that modify the initial map structureto incorporate environmental variations. Besides adding newfeatures into the map, this approach also deals with theremoval of transient map components. The main differencebetween these two methodologies is the deletion of transitoryfeatures. While both approaches store all of the newly detectedfeatures, the second approach also removes features that nolonger exist making it more appropriate in terms of managingthe map size and simplifying the data association task.There are different approaches that are currently capableof solving one of the two problems under certain conditions;removal of transient features or addition of new features intothe map. Nevertheless, there are only a small number of state-of-the-art approaches which can handle both tasks. To the bestof our knowledge, none of these can be easily adapted todifferent localisation algorithms or can operate independently,meaning that the mapping pipeline requires information fromthe data association and the vehicle’s pose which are genericin any map-based localisation system.The proposed pipeline aims to solve both problems; removalof transient features and addition of newly detected features.We present a layered-map approach to storing information about the detected and matched features, as shown in Fig. 2.Our approach incorporates a pipeline that works in conjunctionwith the localisation algorithm, as shown in Fig. 3. In the priormap layer, we have a feature-based localisation map in a globalreference frame in which we update the features’ propertiesto detect and remove transient features.A generic localisation system is usually composed of twomodules: a data association module in charge of identifying thecorrespondence between the observations and the map, and anestimation algorithm which calculates the a posteriori state ofthe vehicle based on different sensors and the output of the pre-vious module. The data association module can be adjusted toretrieve three pieces of information; the successfully matchedand unmatched map features, and the observations which weredetected but not associated to corresponding features in thecurrent map. This last piece of information is stored andevaluated in the new feature map layer of our pipeline to thenadd potentially valuable features for localisation to the priormap.The sensor model layer of our pipeline is a 2D grid centredin the vehicle’s coordinates frame. This model depicts theprobability of the feature extractor detecting different land-marks in the area around the vehicle. The proposed pipelineuses different algorithms and data structures to store, retrieveand update each of the layers. The pipeline is independent ofthe localisation algorithm used. It will only require informationabout the current vehicle pose, matched and non-matched mapfeatures, and non-matched observations. This paper proposesa comprehensive and novel probabilistic pipeline for feature-based map maintenance which addresses the removal of tran-sient features and inclusion of new detected features. Themajor contributions in this paper are: • A grid-based methodology to detect and remove transientfeatures in a localisation map. • A procedure to include newly detected features into thelocalisation map. • A demonstration of the two previous contributions in theform of a pipeline, capable of modifying a prior mapbased on changes to the environment. This is demon-strated using a long term dataset.We demonstrate capabilities of the proposed algorithms us-ing the USyd Campus Dataset [1] and a localization algorithmwhich includes a prior feature-based map, a localization filterFigure 3:
High-level flowchart of the process.
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 3 and a data association method. The paper is organised asfollows: In the next section, we present the state-of-the-artin map maintenance. In Section III, we present the proposedalgorithms for the long-term map maintenance pipeline. InSection IV, we explain the dataset, the initial map and thelocalisation algorithm used to test and evaluate our pipeline.We demonstrate the performance of the algorithms by exper-iments with the results presented in Section V. In Section VI,we present further discussions about the results. Finally, theconclusion and future work are presented in Section VII.II. RELATED WORKA map is required to perform accurate global localisation inan urban environment where GNSS is not always available. Afeature based map can be built using landmarks which bestrepresent the operational environment, also considering thememory and computation resources of the platform. The mapalso needs to be updated as the environment changes; addingnew information into the map when discovered, and rely ondata association/matching algorithms to reject information thatis no longer valid (cumulative adjustments). On the other hand,a more complex approach is to remove invalid landmarks, andincorporate new landmarks into the map as the changes aredetected (transformative adjustments).Churchill and Newman in [6] presented plastic maps forlifelong navigation. This method adds distinct visual experi-ences to the map when there is a change in the scene (repre-sented as a failure in localisation). The different experiencesare linked by places, which are created when the agent can belocalised in more than one experience. In [7] authors describea graph-based SLAM [8] approach which was designed toallow merging of new map fragments to the localisation map.MacTavish et al. in [9] present a methodology to recall rel-evant experiences in visual localisation, the multi-experiencelocalisation system stores every visual experience in a layerand selects the one more suitable for the current view.In turn, the transformative adjustments can be classified intotwo groups: those which only perform removal of features, andthose which also simultaneously add new information into themap. Amongst the first approaches, we can find algorithmsthat evaluate the temporal properties of each feature. Biberand Duckett in [10], [11] represented the environment usingmultiple and discrete timescales, where depending on thesemantic category of each feature, a vanishing rate time isestablished. Later, Rosen et al. in [12] presented a probabilisticmodel based on the feature persistence that illustrates thesurvival time of each map component. In [13], the authorsextended [12] by adding a module able to capture the potentialcorrelation between features, allowing modelling their persis-tence jointly. In [14], we exploited the geometric connectionsbetween vehicle’s pose over several drives and the observationof features to define the visibility of landmarks. Changes inthe area of visibility, specifically reduction (caused by non-detection), leads to the removal of the feature.Rating and selection of features is another approach used forlong-term localisation. This methodology ranks the landmarksbased on their value for localisation. The score function of each feature depends on different predictors. Hochdorferand Schlegel in [15] clustered landmarks that covered ap-proximately the same observation area, and calculated theinformation content of each landmark base on its uncertainty.In order to remove landmark, each cluster is assessed and theones with the lowest localisation benefit are deleted.An approach based on scoring the features by the numberof detection was presented in [16]. The authors chose thisparticular predictor due to its correlation with the averagenumber of matches in different datasets. Nevertheless, thisparticular predictor is susceptible to factors like vehicle speedand areas visited. In [17], authors used a relation betweenthe number of observed trajectories and its expectancy. Thiswork was expanded in [18] where the authors combined thepredictor as: distance travelled while observing a landmark,mean re-projection error and the classification of the de-scriptors appearance as ranking function. A similar approachwas presented in [19]. The ranking scheme consisted of aregression model that was trained with predictors describingthe density of landmarks within the map and the way theywere detected by the sensor system.Some approaches have demonstrated persistent long-termlocalisation by selecting the appropriate features to createthe map. In [20], the authors assess the stability of visualfeatures by exploiting their distinctiveness and the robustnessof image descriptors. In [21], the uniqueness of the descriptorsis evaluated to determine whether features should be includedinto the map. A learning-based approach to select features thatare robust to varied lightning conditions has been implementedin [22] to achieve long-term localisation.Methodologies which allow the addition and deletion offeatures to the map are more suitable for automotive appli-cations as this can minimise the size of the map withoutaffecting the localisation accuracy. Konolige and Bowman in[23] presented a view-based map derived from FrameSLAM[24]. This approach allows the inclusion of new features whiledeleting views. This last step is performed by calculating thepercentage of the number of matches of each view and deletingthe least-recently used (LRU) views. In [25], Egger et al. chosenodes 3D surfels to represent the environment. A C-SLAMalgorithm is used to integrate new nodes to replace those whichhad changed.During the last few years, new approaches related to theperiodicity of features have been published. These worksassume that some of the detected and mapped landmarks aredetected at a predetermined frequency in the environment. Thisis the case of [26], Krajnik et al. introduced FreMEn, wherethe probability of the map elements’ states are representedby a combination of functions with different amplitudes andfrequencies which represents the changes in the environment.The authors extended their previous work in [27] by usinga spatial-temporal representation to calculate the probabilisticdistribution of the features in time.A number of the long-term techniques used for visuallocalisation requires the storing of different versions for thesame place, and identify which one is the most suitable forthe current circumstances in order to localise within it. Thesetechniques are particularly suited to localisation with sensors
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 4 such as cameras, in which measurements change over thecourse of a day due to time dependent lighting conditions.In this paper, we assume the map features are illuminationinvariant, e.g., lidar/radar features.The solution of the map maintenance problem has becomean essential requirement for the deployment of AVs. In [28],the authors present an approach to detect changes in HDmaps [29] on highways and consequently replace identifiedzones (based on the nature of localisation algorithm) with lanemarking patches to keep the map updated. In contrast, ourfeature-based map is suitable for urban environments wherethe environment is more structured and where lane markingscannot always be detected on the road.So far, we have presented a discussion about differentapproaches to map maintenance which are capable of re-moving transient features, and others that add new featuresinto the map. Nevertheless, in the literature, there are noexisting approaches which run completely independently ofthe localiser used while addressing all these issues together.The majority of these works are specific to the implementationof the localisation algorithm or filter to be able to performthe map updates. In the following section, we present theproposed methodology to update feature-based maps, followedby modules that are used to develop and evaluate the pipeline.III. MAP MAINTENANCE METHODOLOGYIn this section, we present a novel pipeline to update a priorfeature-based map by removing transient elements and addingnewly detected features. The proposed pipeline consists ofthree map layers. The prior map layer represents the localisa-tion layer consisting of the prior map. This layer is updated bymaking use of map matching information generated by the dataassociation algorithm. Through the maintenance of this layer,we aim to detect which features are no longer existent andto remove them from the localisation map. The new featuremap layer represents a structure where we save and update allFigure 4:
Flow chart of the process. Figure 5:
Sensor model; the grid map is coloured by the probabilityof each cell to detect a feature within it: red colour represents aprobability of 1.0 while magenta 0.0 probability of detecting acorner • or a pole • . observations which have not been associated with an existingmap element.In a parallel process, the current observations are alsocompared against this map to identify the components whichare recurrently observed. The sensor model layer correspondsto a grid map which stores lidar detections of features aroundthe vehicle. The prior and new feature map layer are assessedin order to keep or remove features from the respective layers.The permanent and relevant features form the prior and newfeature map layer, respectively, and are ultimately mergedtogether in a new and updated map through an optimizationalgorithm. Fig. 4 shows a flow chart which contains the mainprocesses and their connections to perform the maintenanceof the map. Each element of the long-term pipeline will beexplained in the following subsections. A. Sensor model layer
The sensor model layer represents how the sensor systemperceives the features from its surroundings. It consists of a2D grid which is centred on the vehicle’s coordinate frameas shown in Fig. 5. In our experiments, the size of the gridis set to 60 meters; selected due to the capabilities of thelidar to detect features of interest at longer ranges. For thecase of the Velodyne VLP-16 lidar, poles and corners aregenerally detected within a 30 meters radius of the vehicle.Every time a detection or misdetection of a feature occurs,the cell corresponding to the location of the feature in thevehicle frame is updated. The update is implemented using abinary Bayes filter. The value in each cell l ct denotes a log-odds representation of the probability of observing a featurewithin that region. l ct ( v | z t ) = l ct − ( v | z t − ) + l c ( v | z t ) (1)In our experiments, we set log-odds values of l c ( v | z t ) = 0 . for detection and l c ( v | z t ) = − . for misdetection. OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 5
B. Prior map layer
This layer is composed of the localisation map comple-mented by a measure of the visibility of each feature. Thevisibility of a feature is defined by two vectors. Each elementof the vectors corresponds to a discretised angle between 0and 360 ◦ . The first vector contains the maximum range fromwhere the feature was detected at a particular angle. Fig. 6shows a representation of the range vector for a single feature.The second vector consists of the probability of detection atthat particular angle. The number of elements in the vectordepends on the angular resolution. (a) Single pair detection range - probability of detection.(b) Depiction of the detection range visibility vector. (c) Figure 6:
Depiction of the visibility vectors for one single feature(in red). Fig. 6a illustrates a the detection range and the probabilityof detection for one discrete angle θ . Fig. 6b shows the trajectory ofthe vehicle, the area under the curve corresponds to the region fromwhere the feature can be detected by the vehicle. The range vector is updated each time the feature is matchedwith the observations and the current detection range for thesame angle is larger than the previous one. In that case, theelement of the vector will take the current range value. Aparticular element of the range vector will also be updated ifthe following three conditions are met: the map feature has notbeen matched, the corresponding range in the vector is largerthan current and the feature is not occluded by any obstacle.The second vector represents the visibility and correspondsto the detection probability of each feature l pt at specific angle.Similar to the update of the range vector, one cell of the vectoris updated with either a match, or when a feature is both notdetected and not occluded. We use the log-odds representationof the Bayes filter to perform this task: l pt = l pt − ± | l ct ( v | z t ) | (2)The l pt is given by its previous value plus (for detections)or minus (for misdetections) l ct , which is the log-odds valueof the corresponding cell in the sensor model layer. The data association algorithm outputs both the successfullymatched, and non-matched features. This method does notconsider if the feature is occluded. Urban environments areoften characterized by having many dynamic objects; vehicleson the road or pedestrians on the sidewalks. These non-staticobjects could partially or completely occlude the localisationfeatures, and this occlusion could be permanent or temporarydepending on the nature of the object.To check if a particular non-matched feature was occludedor not, we consider the objects in the environment by evaluat-ing the line of sight from the vehicle to the feature. We detectthe obstacles in the environment by maintaining a 2.5 D gridmap [30] centred in the vehicle’s footprint frame. Initially, thesurrounding environment is divided into a grid of r x r metersand n x n cells. The lidar point cloud is transformed into thefootprint frame and then projected to the grid map. The highestand lowest 3D points in the z-axis are obtained for every cellof the grid map. We then compute the difference between themaximum and minimum value in the z-axis. Those cells inwhich the height difference is greater than a threshold th gm ,are considered to be occupied by an obstacle.Once we have obtained the obstacles from the environmentin a grid map format, we perform a 2D ray casting. Theorigin of all rays is fixed relative to the obstacle grid, andit corresponds to centre point of the vehicle’s frame. Theray casting algorithm generates an output similar to a 2Dlaser scan, where a range value is associated with eachdiscrete angle. Since we know the position of the features withrespect to the vehicle’s frame, we can establish if there is anobstacle between the lidar and the non-detected features. Thefeatures in the vehicle’s frame are converted from Cartesianto polar coordinates and then compared with the ray castingalgorithm output to check for occlusion. Fig. 7 illustrates themethodology used to identify obstacles between the vehicleand the features detected.Verifying for occlusion will allow us to perform a morereliable update of the visibility of the map features. TheAlgorithm 1 shows a summary of the process for updatingthe visibility vector representing a particular vehicle pose.By assessing the evolution of the visibility vector, we canselect the transient features which will be removed from themap. We calculate the visibility volume as: V f = α =360 (cid:88) α =0 . ∗ range ( α ) ∗ P pt ( α ) , (3)where: P pt = 1 −
11 + e l pt . (4)We calculate the difference between the visibility areabefore and after each drive of the dataset. A reduction invisibility for a particular feature implies that the feature isno longer observable. It means that a feature which was notoccluded could no longer be detected by the lidar given thatit was observed before from the same angle and a larger orsame range. If the reduction in the visibility value is greaterthan a threshold th vis , the feature is removed from the map. OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 6 (a) Stitched images for a 350 ◦ field of view of the surroundings.(b) Point cloud (c) 2D grid obstacles and PC (d) 2D grid obstacles (e) Ray casting Figure 7:
Occlusion detection process. The reference image of the surroundings is shown in Fig. 7a. Fig. 7b, 7c, 7d and 7e depict the pointcloud occlusion handling process where the vehicle is represented by a black star facing in the upwards orientation.
Algorithm 1
Visibility Update procedure V ISIBILITY V ECTORS U PDATE2: map features ← [ X f , Y f ] vehicle pose ← ( x v , y v ) number features ← size ( map features ) i ← while i < number features do angle ← atan2 (( y v − y f i ) / ( x v − x f i )) disc angle ← round ( angle ∗ / π ) + 180 range ← (cid:112) ( y v − y f i ) + ( x v − x f i ) if Matched then if Ranges ( disc angle ) < range then Ranges ( disc angle ) ← range . l pt ( disc angle ) = l p t − ( disc angle ) + l ct else if non-Occluded then if Ranges ( disc angle ) > range then Ranges ( disc angle ) ← range -1 . l pt ( disc angle ) = l p t − ( disc angle ) − l ct i ← i + 1 .We set the threshold according to [14] where showed thebest performance. C. New feature map layer
The new feature map layer stores the observations of newfeatures which have not been matched against the prior map.New features can appear due to a structural change of theenvironment, or because they were occluded in the previousdata that was used to create the prior map. In our experiments,for the first month of operation we set the minimum heightparameters of the feature detector the same as the ones used tobuild the map. The map building process is especially sensitiveto the features’ parameters, the values of 1.8 m and 1.6 m for each type of features ensure the successful loop closure, noisereduction and optimization.After building the initial map, for usual operation andlocalisation purposes, we loosen the constraints of the featuredetection algorithm. A consequence of these changes is thatnew features can be detected which were previously out ofthe range given the previous height threshold limit. With thischange, the existing features can also be observed from a largerrange, benefiting the data association algorithm and leading toimproved localisation accuracy.Typically, these newly observed features would be discardeddue to not being already included in the prior map, meaningthey would not be matched. In this paper, we propose to buildan alternate, new feature map to store this recent information.When the localisation algorithm is initialised, the detectedobservations which are not matched to the prior map aretransformed into the global frame, and included in the newfeature map layer.The new mismatched observations are compared against thefeatures in the new feature map layer, for this purpose we usean extra iterative closest point (ICP) based data association anda Euclidean clustering algorithm. This information is neededto calculate evaluation parameters for each feature, besides thefinal building a graph to optimize the new features’ position.At the end of each week, we performed Euclidean clusteringof all observations from the new feature map layer. In casethe ICP data association algorithm fails, observations whichbelong to a cluster in which standard deviation is lower than15 cm are linked together.We demonstrate that it is possible to quantify the local-isation relevance of a feature by using different predictors[19]. This requires the output of the data association processand the vehicle location with respect to the features frame ofreference. We used two predictors to assess the quality of the
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 7 (a) Concentration ratio(b) Distance travelled
Figure 8:
Variables involved in the selection of features from thenew feature map layer. In Fig. 8a the density of features is higherwhere the concentration ratio is lower (in blue) and vice-versa onthe red features. In Fig. 8b the distance travelled while detecting thefeature is greater than 40m for the features coloured in red, whileshorter than 5 meters for the dark blue ones. new features to determine whether they should be included inthe localisation map.For each timestep, we obtain the data about the geomet-ric relationship between the vehicle pose and the observedfeatures. With this information, we calculate metrics such asthe distance travelled while the feature can be observed, andthe ratio describing the concentration of features within thelocalisation map.The measure of distance travelled while detecting a givenfeature is calculated by monitoring the set of vehicle posesfrom where a particular feature was observed. The importanceof the feature increases if the feature can be observed overa longer distance; this would result in more opportunities toupdate the vehicle pose within the localisation framework. Theconcentration ratio on the other hand, is a measurement of thedistribution of the features.Concentration ratio = max ( df n ) (cid:80) df n (5)The concentration ratio is calculated per feature and isdefined as the ratio between the distance to the furthestlandmark max ( df n ) , and the sum of the distances df n to allother features within a specified range. The lower this valueis, the higher the density of features is. A concentration ratio Figure 9: Optimized pose graph of new detected features. approaching one indicates that the landmark density is sparse.With this variable, we look to prioritize the inclusion of newfeatures where the distribution is low.We select the features from the new feature map layer thatwas visible to the vehicle for more than one-metre distance.We found that for our data, one-meter distance allows us toinclude corners which are important for localisation but notoften detected. From this we construct a graph with nodes arecomposed of feature locations and edges from the vehicle’sposes. The graph is then optimized to adjust the coordinatesof the features as shown in Fig. 9. For each of the resultingfeatures, the concentration ratio is calculated within the map.Those features which have a score of less than 0.4 (belongingto a high-feature dense area) are discarded since we don’twant overcrowded regions in the map (making the map moreprone to data association mistakes), instead, a more distributedfeature map. Then, the remaining features are merged into theprior map layer.IV. MAP EVALUATION METHODOLOGYIn the previous section we presented the methodology for apipeline capable of updating a featured-based map. In this sec-tion, we present three components used to analyze and evaluatethe long-term mapping solution. The first element is a datasetcomprising of an extensive set of repeated vehicle trajectorieswith an array of sensor readings collected over a long period oftime. The time span of the dataset covers structural changes inareas that were repeatedly visited. This component is essentialto evaluate the changes in the environment over time.The second component corresponds to an initial localizationmap created with the first set of data, during a single drivearound the area where no special traffic conditions wereenforced. This map provides us with a starting point to testour pipeline.The third element is a localisation algorithm capable ofcomputing the global pose of the vehicle within the prior mapbased on the available sensor data.
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 8
A. Dataset
The evaluation dataset was obtained with an electric ve-hicle (EV) retrofitted with multiple sensors, and covers atimespan of more than 18 months. The weekly trip coveredapproximately the same route around the University of Sydneycampus. The dataset includes diverse weather/environmentalconditions, variation in illumination, the creation and removalof structures and buildings, and diversity in the volume ofpedestrian and vehicle traffic.Figure 10:
The partial trajectory of the University of Sydney (USyd)campus dataset used in this work. The highlighted zones 1-4 inorange correspond to urban development and construction sites(renovations to facades, new building constructions, shut-downs, etc.)which changed considerably over the course of the data collectionperiod. Zone 5, on the other hand, is predominantly a pedestrianspace, where frequent university events often result in changes to thesurroundings.
While driving, we logged the readings from various sensormodalities sensing, which allowed us to develop and testdifferent algorithms (perception, localisation, navigation, etc).Fig. 10 shows the subset of the trajectories from the datasetused in this paper (this portion has roughly the same qualityof GNSS data).The EV used for the dataset is equipped with: • One Velodyne Puck LITE - 16 beam lidar sensor placedon the vehicle’s front roof (tilted ◦ downwards), itprovides 3D information of the environment in form ofpoint cloud at a frequency of 10 Hz. • Six NVIDIA 2Mega SF3322 automotive GMSL camerasmounted around the vehicle for a complete ◦ coverageof the vehicle’s surroundings. Images were recorded at 30frames per second with 2.3 M pixel resolution. • One VN-100 Inertial Measurement Unit (IMU) whichcomputes and outputs real-time 3D orientation. • Four wheel incremental encoders built into the motors,which allow the speed estimation of each individualwheel. • One potentiometer joined to the steering wheel to calcu-late the front wheels’ steering angles. • One U-Blox NEO-M8P GNSS module which specify thegeo-spatial positioning in longitude, latitude, and altitudecoordinates.The location of the different sensor modalities on thecollection platform is depicted in Fig. 11.Figure 11:
Sensor arrangement on the data collection platform.
Fig. 12 shows some of the structural changes that tookplace during the time of the dataset, which significant changesoccurring in each of the different zones.
B. Initial Feature Map
The initial 2D feature-based map used as a prior for themap maintenance process was created using the data from thefirst dataset collected at the University of Sydney campus. Theprior map consists of features obtained from a labelled pointcloud [31] with its position optimised using a graph-basedSLAM algorithm.
1) Labelled point cloud:
We added semantic labels to thelidar point cloud by following the approach presented in [32].The cameras on the vehicle were intrinsically calibrated usinga modified version of the camera calibration
ROS package[33], which was changed to use a generic fish eye cameramodel [34]. The extrinsic parameters of the lidar-camera pairswere calibrated as specified in [35].Images of the environment are processed by a CNN whichoutputs a semantic label for each pixel. The CNN discriminatesbetween twelve different semantic classes: pole, building,road, vegetation, undrivable road, pedestrian, rider, sky, fence,and vehicle and unknown (unlabelled and void). As the lidar ismechanically scanned, we apply a correction to the lidar pointcloud to compensate for the vehicle’s motion. This correctionalso is constructed to align the lidar scan to the timestamp ofthe camera images. We then transfer the semantic informationfrom the labelled images to the lidar point cloud using thecamera calibration parameters, and the geometric relationshipwith the lidar. During this process, a masking technique isperformed to avoid labelling lidar points which are not visibleto the camera [32].
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 9
Figure 12:
Structural changes discovered in the USyd Dataset. Each row of images shows a specific architectural/environmentaltransformation for each zone 1 through 5.
2) Feature extraction:
The features used for the mappingprocess are extracted from a semantic labelled lidar pointcloud. These features are classified as poles or corners basedon their geometrical shape. Poles are defined as cylindricalstructures with an established minimum height and maximumwidth. Corners, on the other hand, are characterized as avertical stack of the intersections between detected straight linesegments with similar properties. In the urban environment,the straight lines are often observed from lidar returns on thewalls of buildings [36]. It is worth mentioning that for theinitial construction of the map, the parameters for the featuredetectors were set to be very rigid in order to ensure thatthe features included in the initial map were stationary andincrease the chance that the loop closure is successful. Inour case, we consider valid poles to be taller than 1.8 meterswith a diameter smaller than 0.3 meters, and valid corners areconsidered to be those taller than 1.6 meters.Fig. 13 shows a example image/point cloud from the datasetwith the features superimposed over the image. The featuresare constructed from stacks of vertical 3D points that areprojected into the vehicle ground plane as a single 2D point(as if from a top down view). Every projected 2D pointconsists of: ( x, y ) coordinates in the vehicle’s frame, height,predominant semantic label, type of feature and a geometricproperty (diameter for poles and angle for corners).The map building process uses a graph-based SLAM al-gorithm, integrating the detected lidar features, IMU, wheelencoders and GNSS data. An ICP data association methodis used to find the correspondences between the different (a) Pole and corner features detected from the point cloud.(b) Front 160 ◦ FOV image from the surroundings.
Figure 13:
Features extracted from the lidar point cloud, poles incyan , corners in green . observations of lidar-based features. The GNSS information isused in two steps. The first step is to find the transformationmatrix between the relative vehicle’s pose and the globalreference frame, to then apply the corresponding rotation andtranslation to the features’ local position. In the second step,the vehicle’s pose vertices are loosely constrained by the
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 10
GNSS (due to its high uncertainty). Orthophotos are usedto label poles and obtain their map-feature correspondenceby a nearest neighbour algorithm. These relationships areincorporated as graph edges with tight constraints, and areadded to the optimisation algorithm in order to adjust thefeatures to the aerial image [37].
C. Localisation algorithm
As the primary purpose of the feature-based map is tolocalise the vehicle within it, we tested the map maintenanceprocess through the localisation algorithm’s performance. Thelocalisation algorithm is initialised using GNSS information.We used two successive GNSS readings to obtain a reasonableapproximation of the vehicle’s heading angle. These GNSScoordinates are only collected when the vehicle speed exceedsa certain threshold (more than 3 m/s, determined empiricallybut related to the noise of the GNSS). The initial vehicleheading angle is then set as the angle of the vector formedby these two geo-referenced points. An unscented Kalmanfilter (UKF), integrates the GNSS, IMU and wheel encodersto estimate the position of the vehicle within a global frame.The existing features from within a 40 meter range of theestimated vehicle pose are retrieved from the prior map, andthen compared to the currently observed features. Once thefirst successful map correspondence is found, the UKF updatesthe vehicle position using the additional observations from theICP map matching algorithm.An ICP matching algorithm is used for the data associationtask. It provides the UKF with a transformation betweenthe current observations and the globally referenced featuresfrom the prior map that were close to the vehicle’s estimatedposition. In addition to the ICP transformation, the outputof the matching process includes whether each feature wasmatched or not. This information is needed for the long-termmap maintenance pipeline. The outputs from the ICP matcherconsists of two vectors of features as shown in Fig. 14. Thefirst vector includes the features that have been successfullymatched against the observations. The second vector containsFigure 14:
ICP matcher’s outputs. the observed features which were not associated to any featureswithin the map. Non-matched observations are the pole andcorner features detected by the vehicle sensors which are notincluded in the localisation prior map. This information isparticularly relevant when considering adding new featuresinto the map. For both the matched and non-matched featureobservations, the output vectors include position, feature type,semantic label, height and geometric parameters. This data isuseful to update existing, and initialise new features.The lack of features in certain areas, or an incorrect data as-sociation can have catastrophic consequences in an operationalplatform. Given that we know beforehand the areas where thevehicle will be operating, we can mitigate this problem bysetting boundaries to indicate the valid drivable space. Thedrivable zones are specified as lanelet areas which enclose theroads visited by the vehicle in the dataset that do not belong tobuildings or other undrivable areas (as parks or soccer fields).Fig. 15 shows an illustration of the lanelets used to representthe roads visited in our dataset, the red and black areas areused to indicate an invalid position and cause the UKF moduleto be reset if the vehicle pose estimate is within these areas. Areset signal is used to trigger the UKF module to re-initialiseall variables and reset the vehicle’s uncertainty estimate.Figure 15:
Valid drivable areas. The area enclosed with red bound-aries (excluding the building area in yellow) corresponds to the validroads visited during the collection of the dataset.
V. E
XPERIMENTS AND R ESULTS
This section presents a number of experimental results thatdemonstrate the functionality of our proposed pipeline usingthe evaluation modules introduced in the previous section. Weran the proposed pipeline, and the localisation module over sixmonths of data from the USyd Campus Dataset [1]. For thefirst week’s data, we constructed the prior map as described inthe previous section. This resulted in a feature map including405 pole and corner features. From the second week, the mapmaintenance pipeline was run. During the experiment, we havealso reduced the restrictions of the feature detector algorithm.The parameter affected in this case is the minimum height of
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 11
Table I:
Number of features
W Height parameter Reset N. Features R. Features TotalPole Corner1 1.6 1.8 - 405 - 4052 1.6 1.8 3 20 22 4035 1.6 1.8 2 10 5 4088 1.6 1.5 2 8 2 41411 1.5 1.5 0 17 6 42515 1.4 1.5 0 9 5 42918 1.4 1.4 0 17 3 44321 1.4 1.4 0 6 2 44924 1.4 1.4 0 5 2 452 each feature. We did this to be able to detect new featureswhich can be beneficial for the localisation algorithm. Afterthat, we run the algorithm every 3 weeks, in each of theseweeks we evaluated the number of new features included inthe localisation map and the number of features removed fromit. Table I shows the number of new and removed features(denoted as N. and R. Features respectively in the table).The total number of elements in the map is displayed inthe last column. For every week in the first column, theminimum height parameter of the feature detector is listed.In the fourth column, we indicate the number of times thelocalisation filter was reset during that specific week. We usethe number of times the filter resets as a proxy to measurethe ability of the localisation filter to work given a specificprior feature map. During weeks 2, 5 and 8, two or morereset signals were generated, this occurred in areas with smallnumbers of detectable features. These regions did not havestrong feature patterns in these locations, which resulted ineither incorrect data association or a large accumulation oferror from the odometry. After week 11, there were sufficientfeatures included in the map due to the relaxation of the heightparameter for the feature detector. The additional featuresin previously low density areas resulting from this changeimproved the localisation resulted in no further resetting ofthe filter.Another metric we use to measure the quality of the mapfor localisation is the number of large position correctionsdue to the update step of the filter. There are a number ofreasons why there would be large corrections related to mapand sensor noise. When the vehicle is unable to feature matchFigure 16:
Strong corrections in positioning.
Figure 17:
Number of strong corrections per updated map. Thenumber of weeks lies on the x-axis while the number of correctionsis set in y-axis. for a period of time, the error caused by dead reckoningincreases over time. The discontinuity in position estimate ismore likely to occur when a successful match takes place aftera period of time with no matches. Incorrect data association,or incorrectly positioned features within the map can alsocause the previously discussed behaviour. For the experiment,we count the number of strong corrections to demonstratethe ability to match observations to a specific map. Wedefine strong corrections as changes in the position with alarge lateral translation, which is not physically possible dueto holonomic constraints. Fig. 16 shows some examples ofdiscontinuities in the vehicle trajectory.After updating the map, we run the localisation algorithm tolocalise the vehicle within the updated map in the subsequentweeks. Fig. 17 shows the number of times a strong correctionoccurred per experiment per map. In this figure, map 2 is theupdated version of map 1; map 3 is the updated version of map2 and so on. We can see that the number of corrections acrossdatasets using the same map are relatively stable, and for somedatasets, it is even lower than the initial value. The reduction ofthe number of strong corrections are mainly due to the changeof height parameter for the feature extractor. When the vehicleis able to identify shorter poles and corners, the featureswithin the map can also be detected from a larger distance,generating more updates and smaller corrections. Overall, theperformance of each updated map shows an improvement onthe previous map across the dataset. This can be seen in Fig.17 that all graphs generated by updated maps have a consistentsmaller number of strong corrections.Qualitative results are shown in Fig. 18. In this figure, weoverlay all of the detected features on an orthophoto of thearea. Initially, the area contains few features to match against,causing lower quality feature matches and increased relianceon dead reckoning. In Fig. 18a, 18b and 18c we can see thata number of detected features (in yellow) are shifted witha bias towards the bottom of the image. This deviation isin the vehicles direction of travel and is a consequence ofthe odometry errors accumulated over time. After Fig. 18da number of new features were detected and included inthe map. The localisation algorithm was able to match moreobservations against the updated map. As a result of this, the
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 12 (a) Week 2 (b) Week 5(c) Week 8 (d) Week 11(e) Week 15 (f) Week 18
Figure 18:
Evolution of the overlap between detected features andsatellite orthophotos within an initially featureless area. localisation filter benefits from the improved output of the dataassociation algorithm to estimate a more accurate position. Byreducing the localisation error, the observed features translatedinto a global reference frame have an improved overlap withthe orthoimagery of the area. As more features are included inthe map, the localisation become more accurate. Fig. 18e and18f demonstrate the improvement of the localisation in termsof the correspondence between the detected features and thereference orthophoto.Finally, we use the semantically labelled point cloud togenerate the transformations between labelled images and thelidar point cloud. Fig. 19 shows the normalised histogram ofthe semantic labels for each type of feature. As expected, thecorner features were mostly detected correctly from structures Figure 19:
Labels in poles and corners. identified as buildings in the labelled image. Pole features weremostly labelled as either pole and vegetation with a few outlierlabels. This is because many of the pole features were fromthe trunks of trees.VI. C
ONCLUSIONS AND F UTURE WORK
In this paper, we have presented a pipeline for long-termmaintenance of feature-based maps. Our approach is basedon a layered structure, where a prior map layer containsthe positions and attributes of each feature. These attributesinclude a measure of the visibility of each map feature. Thisvisibility vector allows us to characterise the locations fromwhich the vehicle has previously detected the features, andthe frequency of these observations. Subsequent visits to thesame locations are used to re-evaluate the visibility vector;determining how often and from where the features are re-observed. The visibility volume metric presented in this paperis used to assess the stability of the features. By evaluatingthis metric we are able to remove features that no longer existfrom the next iteration of the prior map layer.We built and maintained a new feature map layer from theobservations that could not be matched against the prior maplayer. We selected the most relevant features and adjusted thembased on a graph optimisation algorithm. Moreover, we eval-uate the density of the features in the different areas throughthe concentration ratio metric. This procedure allowed us toincorporate the newly detected features into the next iterationof the prior map layer while improving the distribution of mapelements.The pipeline implementation was tested using a long-termdataset containing significant changes to the operating envi-ronment over time. The performance was validated using alocalisation algorithm receiving updates from map matchingagainst the prior feature-based map.A comprehensive set of experiments were presented using adataset incorporating several weeks of data. This data includessignificant variation in the environment resulting from changesto buildings and other structures over the timeframe of thedataset. A number of metrics were presented to evaluatequantitative results showing the improvement of localisationdue to the map maintenance process. The results show thenumber of new features incorporated into the map, and the
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 13 number of older features that were deleted based on thepresented pipeline.Qualitative results were shown by registering the detectedfeatures based on the vehicle’s position and overlaying themon an orthoimage of the environment. The correspondencebetween these lidar features and orthoimage improves withthe updates to the map.We used a semantically labelled point cloud as an inputto the feature extractor in order to analyse the distributionof the feature labels in the map. As future work, we intendto incorporate the semantic labels into the pipeline as a newvariable to discriminate between static and dynamic objects.A
CKNOWLEDGMENT
This work has been funded by the ACFR, the Universityof Sydney through the Dean of Engineering and InformationTechnologies PhD Scholarship (South America) and Univer-sity of Michigan / Ford Motors Company Contract “Nextgeneration Vehicles”. R
EFERENCES[1] W. Zhou, J. S. Berrio Perez, C. De Alvis, M. Shan, S. Worrall, J. Ward,and E. Nebot, “Developing and testing robust autonomy: The usydcampus dataset,”
IEEE Intelligent Transportation Systems Magazine , pp.0–0, 2020.[2] W. Zhou, S. Berrio, S. Worrall, and E. Nebot, “Automated evaluationof semantic segmentation robustness for autonomous driving,” in
IEEETransactions on Intelligent Transportation Systems , May 2012, pp.4525–4532.[7] E. Einhorn and H. Gross, “Generic 2d/3d slam with ndt maps for lifelongapplication,” in , Sep.2013, pp. 240–247.[8] H. Kretzschmar, G. Grisetti, and C. Stachniss, “Lifelong maplearning for graph-based slam in static environments,”
KI - K¨unstlicheIntelligenz , vol. 24, no. 3, pp. 199–206, Sep 2010. [Online]. Available:https://doi.org/10.1007/s13218-010-0034-2[9] K. MacTavish, M. Paton, and T. Barfoot, “Selective memory: Recallingrelevant experience for long-term visual localization: Mactavish et al.”
Journal of Field Robotics , vol. 35, 11 2018.[10] P. Biber, “Dynamic maps for long-term operation of mobile servicerobots,” in
In Proc. of Robotics: Science and Systems (RSS , 2005.[11] P. Biber and T. Duckett, “Experimental analysis of sample-basedmaps for long-term slam,”
The International Journal of RoboticsResearch , vol. 28, no. 1, pp. 20–33, 2009. [Online]. Available:https://doi.org/10.1177/0278364908096286[12] D. M. Rosen, J. Mason, and J. J. Leonard, “Towards lifelong feature-based mapping in semi-static environments,” in , May 2016, pp. 1063–1070.[13] F. Nobre, C. Heckman, P. Ozog, R. W. Wolcott, and J. M. Walls, “Onlineprobabilistic change detection in feature-based maps,” in , May2018, pp. 1–9.[14] J. S. Berrio, J. Ward, S. Worrall, and E. Nebot, “Updating the visibilityof a feature-based map for long-term maintenance,” in , June 2019. [15] S. Hochdorfer and C. Schlegel, “Landmark rating and selection ac-cording to localization coverage: Addressing the challenge of lifelongoperation of slam in service robots,” in , Oct 2009, pp. 382–387.[16] P. M¨uhlfellner, M. B¨urki, M. Bosse, W. Derendarz, R. Philippsen, andP. Furgale, “Summary maps for lifelong visual localization,”
J. FieldRobot. , vol. 33, no. 5, pp. 561–590, Aug. 2016. [Online]. Available:https://doi.org/10.1002/rob.21595[17] M. Dymczyk, S. Lynen, T. Cieslewski, M. Bosse, R. Siegwart, andP. Furgale, “The gist of maps - summarizing experience for lifelonglocalization,” in , May 2015, pp. 2767–2773.[18] M. Dymczyk, T. Schneider, I. Gilitschenski, R. Siegwart, and E. Stumm,“Erasing bad memories: Agent-side summarization for long-term map-ping,” in , Oct 2016, pp. 4572–4579.[19] J. S. Berrio, J. Ward, S. Worrall, and E. Nebot, “Identifying robustlandmarks in feature-based maps,” in , June 2019.[20] G. Carneiro and A. D. Jepson, “The quantitative characterization of thedistinctiveness and robustness of local image descriptors,”
Image andVision Computing , vol. 27, no. 8, pp. 1143 – 1156, 2009.[21] Y. Verdie, K. M. Yi, P. Fua, and V. Lepetit, “Tilde: A temporally invariantlearned detector.” in . IEEE Computer Society, 2015, pp. 5279–5288.[22] H. Lategahn, J. Beck, B. Kitt, and C. Stiller, “How to learn anillumination robust image feature for place recognition,” in , June 2013, pp. 285–291.[23] K. Konolige and J. Bowman, “Towards lifelong visual maps,” in ,Oct 2009, pp. 1156–1163.[24] K. Pirker, M. R¨uther, and H. Bischof, “Cd slam - continuous localizationand mapping in a dynamic world,” in , Sep. 2011, pp. 3990–3997.[25] P. Egger, P. V. K. Borges, G. Catt, A. Pfrunder, R. Siegwart, and R. Dub´e,“Posemap: Lifelong, multi-environment 3d lidar localization,” in , Oct 2018, pp. 3430–3437.[26] T. Krajn´ık, J. P. Fentanes, J. M. Santos, and T. Duckett, “Fremen:Frequency map enhancement for long-term mobile robot autonomy inchanging environments,”
IEEE Transactions on Robotics , vol. 33, no. 4,pp. 964–977, Aug 2017.[27] T. Krajn´ık, T. Vintr, S. Molina, J. P. Fentanes, G. Cielniak, O. M. Mozos,G. Broughton, and T. Duckett, “Warped hypertime representations forlong-term autonomy of mobile robots,”
IEEE Robotics and AutomationLetters , vol. 4, no. 4, pp. 3310–3317, Oct 2019.[28] D. Pannen, M. Liebner, and W. Burgard, “How to keep hd maps forautomated driving up to date,” in , 2020.[29] ——, “Hd map change detection with a boosted particle filter,” in , 2019, pp.2561–2567.[30] P. Fankhauser and M. Hutter, “A Universal Grid Map Library:Implementation and Use Case for Rough Terrain Navigation,” in
Robot Operating System (ROS) – The Complete Reference (Volume1) , Oct 2018, pp. 3174–3181.[32] J. S. Berrio, M. Shan, S. Worrall, and E. Nebot, “Camera-lidar integra-tion: Probabilistic sensor fusion for semantic mapping,” in
Arxiv , 2020.[33] J. Bowman and P. Mihelich, “Ros perception:camera calibration,” http://wiki.ros.org/camera calibration, 2019, accessed: 2019-10-01.[34] J. Kannala and S. S. Brandt, “A generic camera model and calibrationmethod for conventional, wide-angle, and fish-eye lenses,”
IEEE Trans-actions on Pattern Analysis and Machine Intelligence , vol. 28, no. 8,pp. 1335–1340, Aug 2006.[35] S. Verma, J. S. Berrio, S. Worrall, and E. Nebot, “Automatic extrinsiccalibration between a camera and a 3d lidar using 3d point and planecorrespondences,” in
IEEE Intelligent Transportation Systems Confer-ence - ITSC 2019 , 2019.[36] S. Yi, S. Worrall, and E. Nebot, “Metrics for the evaluation of locali-sation robustness,” in ,2019.
OURNAL OF L A TEX CLASS FILES, VOL. 14, NO. 8, AUGUST 2015 14 [37] ——, “Geographical map registration and fusion of lidar-aerial orthoim-agery in gis,” in
IEEE Intelligent Transportation Systems Conference -ITSC 2019 , 2019.
Julie Stephany Berrio received the B.S. degree inMechatronics Engineering in 2009 from UniversidadAutonoma de Occidente, Cali, Colombia, and theM.E. degree in 2012 from the Universidad del Valle,Cali, Colombia. She is currently working towards thePh.D. degree at the University of Sydney, Sydney,Australia. Her research interest includes semanticmapping, long-term map maintenance, and pointcloud processing.
Stewart Worrall received the Ph.D. from the Uni-versity of Sydney, Australia, in 2009. He is currentlya Research Fellow with the Australian Centre forField Robotics, University of Sydney. His researchis focused on the study and application of technologyfor vehicle automation and improving safety.
Mao Shan received the B.S. degree in electricalengineering from the Shaanxi University of Scienceand Technology, Xi’an, China, in 2006, and the M.S.degree in automation and manufacturing systemsand Ph.D. degree from the University of Sydney,Australia, in 2009 and 2014, respectively. He iscurrently a Research Fellow with the AustralianCentre for Field Robotics, the University of Sydney.His research interests include autonomous systems,localisation, and tracking algorithms and applica-tions.