Directional grid maps: modeling multimodal angular uncertainty in dynamic environments
DDirectional grid maps: modeling multimodal angular uncertaintyin dynamic environments
Ransalu Senanayake and Fabio Ramos The University of Sydney, Australia
Abstract — Robots often have to deal with the challenges ofoperating in dynamic and sometimes unpredictable environ-ments. Although an occupancy map of the environment issufficient for navigation of a mobile robot or manipulationtasks with a robotic arm in static environments, robots op-erating in dynamic environments demand richer informationto improve robustness, efficiency, and safety. For instance, inpath planning, it is important to know the direction of motionof dynamic objects at various locations of the environment forsafer navigation or human-robot interaction. In this paper, weintroduce directional statistics into robotic mapping to modelcircular data. Primarily, in collateral to occupancy grid maps,we propose directional grid maps to represent the location-widelong-term angular motion of the environment. Being highlyrepresentative, this defines a probability measure-field over thelongitude-latitude space rather than a scalar-field or a vector-field. Withal, we further demonstrate how the same theory canbe used to model angular variations in the spatial domain,temporal domain, and spatiotemporal domain. We carried outa series of experiments to validate the proposed models using avariety of robots having different sensors such as RGB camerasand LiDARs on simulated and real-world settings in both indoorand outdoor environments.
I. INTRODUCTIONSafe operation of robots in dynamic environments wherehumans, vehicles, and other robots operate is central to fullautonomy. Spatial information alone is not sufficient in com-plex environments. This is because the prediction of futureevents drives decision making whilst properly managing therisk of collisions. Although conventional mapping techniquesrepresent the space in terms of the probability of occupancy[1], [2], they do not explicitly capture the patterns in themotion direction of dynamic objects such as people, cars, andcyclists. Understanding and modeling directions are com-plicated and cannot be treated with conventional techniquesfrom linear statistics as the treatment of angular quantitiesrequires that distributions be mapped into hyperspheres, in aset of techniques known as directional statistics [3].In general, a robot requires a map for path planning andsafe navigation. To this end, the most common approach isto represent the actual geometry of the environment as floorplans [1] or 3D models [4], [5] in the metric space, thoughgraph-based approaches also exist [6]. These metric mapsare typically built using data collected from RGB camerasor depth sensors such as LiDAR [1], [7].The basic information about the environment a robotrequires to maneuver is to know which areas of the envi-ronment are occupied and which areas are not. To model Email: [email protected] (a) (b) * (c) (d) Fig. 1: Motivation for directional mapping. (a) Plan view of a road wheresimulated human coming from two directions walk across a crosswalkand head towards a different direction. We are interested in modelingdirections people move after taking observations over a period of time (b)The world is divided into a × grid. The directional distribution —aprobability distribution over directions [ − ◦ , ◦ ] —at different cellsmodeled using the proposed method (DGM) is illustrated using polar plots.For any location/cell in the space, such a directional distribution exists. Thecorrespondence between the direction of arrows and the direction of polarplots can be observed. (c) To elaborate, the polar plot marked in ∗ on thegrid map is emphasized. This shows the probability density for differentangles at the particular cell—the probability density increases as go fartheraway from the center. (d) The equivalent unwrapped probability functionwith support [ − ◦ , ◦ ] is given for clarity. this, in his seminal paper, Elfes [1] proposed occupancy gridmaps—the environment is divided into a grid and occupancyprobability of each cell is updated as beam reflections arecollected from a depth sensor such as sonar or LiDAR. Later,continuous scalar-field representations have been proposed[2], [8]. However, all of these methods assumed a static en-vironment where the only moving object in the environmentis the robot. To build a static occupancy map in the presenceof a few dynamic objects, [9], [10] proposed to filter dynamicobjects as a preprocessing step and then map the occupancy.More recently, rather than considering dynamic objects asnuisances, they have been incorporated into the map in order a r X i v : . [ c s . R O ] S e p o model the long-term occupancy [11]–[14] and understandoccupancy patterns [15]–[18]. Nevertheless, unlike in staticenvironments, occupancy is not the only information that canbe extracted in dynamic environments. Supplementing addi-tional information about the dynamics of the environmentcould hugely benefit path planning and object detection al-gorithms [11], [19]. For this purpose, information rich mapscan be developed by modeling the uncertainty of directions,speed, texture, etc. of all locations of the environment inaddition to occupancy information. Consider an instance asin Fig. 1 (a) where simulated humans walking in roadsidesand a crosswalk. If the robot knows about the angles peopleturn, path planning algorithms can be designed to plan aheadand to make efficient and safer maneuvers. As shown inFig. 1b, in this paper, we propose a novel technique tomodel directional uncertainty of the environment at differentlocations observed over time.[20] proposed to model human walking paths by intro-ducing a Gaussian process prior over directions and therebyimplicitly constructing a field representation of angularmovements. This formulation has three main limitations: 1)because the angle is assumed to be ( −∞ , + ∞ ) , predictedangles can be totally invalid, 2) it is assumed that movementsat a given location occur in only one direction which is notpractical for robotics applications as the robot, human, orvehicles in the environment could move in any direction,and 3) being a Bayesian nonparametric model, the algorithmbecomes slower as more data are captured. On the otherhand, the objective of all these approaches is to make short-term future predictions such as tracking rather than buildinginformation-rich long-term maps that can be used for pathplanning or navigation.In our approach, highlighting the importance of dispersionof data, the directions are represented by a probabilitydistribution that, 1) has a valid support of [ − π, π ] and 2) canmodel multi-directional movements. Having a finite numberof distributions laid over the longitude-latitude space usinga grid, it is possible to infer the probability of motionfor any direction for each such location. This directionalinformation can be plausibly used to extract paths as well asvariously regulate path planners to avoid high-risk areas or tofollow the direction of traffic. Although incorporation of suchprobability distribution into control algorithms is beyond thefocus of this paper, recent techniques have shown how toembed probability distributions to improve path planningand navigation [21]–[24]. Further, incorporating such priorinformation is the key in Bayesian statistical methods andprior information can be effectively used in online learningin robotics [14], [25]. Additionally, such probabilistic modelsnaturally account for noises and imperfections in sensors andpre-processing algorithms.Despite the importance of modeling the stochasticity ofangles in robotics, it has hardly been discussed previously.Therefore, introducing directional statistics into robotics tomodel angular data, we present a statistical method:1) to model multi-modal directional uncertainty in dy-namic environments without obtaining spurious out- puts as in current methods [20];2) to quantitatively analyze spatial variations, temporalvariations, and spatiotemporal variations;3) that does not require heuristic parameter tuning i.e.ready for real-world usage without any significantmodificationsHaving discussed the motivation for our work in Section I,Directional Statistics are introduced in Section II as prelim-inaries for the following sections. Data preprocessing stepsrequired for mapping is detailed in Section III-A. Then, thebasic method is introduced in Section III-B assuming that allmovements are almost uni-directional such as one-way roads.Next, in Section III-C, the theory is generalized to modelmulti-directional movements i.e. when there are no definitepaths or dynamic objects can move in arbitrary directionssuch as in indoor environments or sidewalks. Experimentalresults are reported in Section IV followed by discussionsand conclusions in Sections V and VI, respectively.II. DIRECTIONAL STATISTICSIn this section, we introduce directional statistics whereobservations lie on a circle of unit radius, or in highdimensional scenarios, on a hypersphere of unit vector inthe plane [3]. In order to deal with circular data, directionalstatistics was initially developed in physics and astronomy[26] [27], and have successful applications in meteorology[28], biostatistics [29] etc.Although there are several approaches to model directionaldata [30], we opted for the von Mises distribution [26]because, i) it has all advantages of the exponential familyof distributions, ii) analogous to a Gaussian distribution withmore intuitive parameters, and iii) sufficient statistics can beobtained explicitly [3]. These properties will be intermittentlydiscussed in Sections III and V. The probability densityfunction of the von Mises distribution is given by (1), VM ( θ ; µ, κ ) := 12 πJ ( κ ) exp (cid:0) κ cos( θ − µ ) (cid:1) , (1)where µ is the mean direction parameter (analogous tomean in a Gaussian distribution) and κ is the concentrationparameter (weakly analogous to the reciprocal of variance in Fig. 2: The probability density functions of von Mises directional distribu-tion (a) The effect of the concentration parameter κ for a fixed mean µ = 0 ◦ .The support of von Mises distribution is [ − π, π ] . For instance, whenestimating the direction of a moving object from noisy sensor measurements,a conventional Gaussian distribution which has a support ( −∞ , ∞ ) cannotbe used because the actual range of directions is [ − π, π ] . (b) Correspondingpolar plots. Gaussian distribution). J ( κ ) is the th order and st kindmodified Bessel function given by (2), J ( κ ) := ∞ (cid:88) p =0 p ! (cid:16) κ (cid:17) p . (2)Together with the cosine term in (1), the modified Besselfunction attenuates the function and keeps the support in [ − π, π ] . The effect of the κ parameter is illustrated in Fig. 2.Considering a dataset with N directions D = { θ i } Ni =1 , letus define the mean directional components, ¯ C := N (cid:88) i =1 cos θ i and ¯ S := N (cid:88) i =1 sin θ i . (3)Then, the mean direction is given by, ¯ θ = (cid:40) arctan( ¯ S/ ¯ C ) , if ¯ C ≥ S/ ¯ C ) + π, otherwise (4)and the mean resultant length is given by, ¯ R = (cid:112) ¯ C + ¯ S . (5)Note that ¯ R ∈ [0 , and the more homogeneous the direc-tions are, the higher the ¯ R is. The circular variance is definedas ¯ V := 1 − ¯ R .III. DIRECTIONAL GRID MAPSBeing analogous to occupancy grid maps [1], we introduce directional grid maps (DGM) in this section. To formallydefine, a DGM is a multi-dimensional field that maintainsprobability measures given by a probability density functionof the directional uncertainty of the cells in a spatial lattice.With the proposed method, we answer the following ques-tions:1) What are the overall directions of motion in differentplaces in the environment when observed over a periodof time? i.e. longterm spatiotemporal analysis;2) What is the overall direction of motion in the entireenvironment at a specific time? i.e. spatial analysis;3) What is the distribution of directions of a movingobject? i.e temporal analysis. A. Pre-processing
The inputs to build an occupancy map are occupied pointsand the free points in the line of sight of LiDAR [1],[8]. However, inputs to build a DGM are the angle ofmotion at longitude-latitude locations at different time steps: θ ( longitude, latitude, time ) . For simplicity and to be usedin collateral to occupancy grid maps, we discretize the worldand assign longitude-latitude pairs to the cell they belong to.Therefore, the inputs are θ ( cell, time ) .Depending on the sensor type, for each time frame, θ values or the optical flow can be extracted by any of thecommonly used existing methods. To name a few, dataassociation followed by direct angle estimation or Gaussianprocess regression [12], tracking algorithms such as Kalmanfilters, particle filters, mean-shift-tracking, dense optical flow,etc. [31], [32]. Once θ ( cell, time ) are extracted, for thecomputational convenience of answering questions detailed in Section III, they are stored with tracker identities, if exists,in a spatiotemporal database [33] indexed by space and timekeys. B. Learning uni-modal movements (DGM-VM)
Without loss of generality, for the sake of simplicity tointroduce the method, in this section, we assume the averagemovements in the environment occur in approximately onedirection. The more general case is introduced in Section III-C.Consider a dataset with N directions D = { θ i } Ni =1 Assuming i.i.d. of θ , the log-likelihood of the von Misesdistribution introduced in (1) is given in (6), L ( µ, κ ; D ) = log (cid:18) N (cid:89) i =1 πJ ( κ ) exp (cid:0) κ cos( θ i − µ ) (cid:1)(cid:19) = − N log 2 π − N log J ( κ ) + κ N (cid:88) i =1 cos( θ i − µ )= − N log 2 π − N log J ( κ ) + κN ¯ R cos(¯ θ − µ ) . (6)The objective is to learn µ and κ given D to maximizethe log-likelihood, i.e. maximum likelihood estimate (MLE).Intuitively, for a given dataset, MLE adjusts its parameters µ and κ to set the higher values of the probability densityfunction align with more probable data points. These optimalparameter values can be computed by (7), ( µ ∗ , κ ∗ ) = argmax L ( µ, κ ; D ) = (cid:0) ¯ θ, A − ( ¯ R ) (cid:1) (7)where A ( · ) = J ( · ) J ( · ) . To derive this, take the derivative of L w.r.t. the parameters and equate to zero, ∂ L ∂µ = κN ¯ R sin(¯ θ − µ ) = 0 = ⇒ µ ∗ = ¯ θ, (8) ∂ L ∂κ = − N J (cid:48) ( κ ) J ( κ ) + N ¯ R cos(¯ θ − µ )= − N J ( κ ) J ( κ ) + N ¯ R cos(¯ θ − µ )= − N A ( κ ) + N ¯ R cos(¯ θ − µ ) . (9)Setting ∂ L ∂κ = 0 and µ ∗ = ¯ θ = ⇒ κ ∗ = A − ( ¯ R ) . Toapproximate κ ∗ , κ ∗ ≈ (cid:40) R + ¯ R + ¯ R , for small ¯ R . − ¯ R ) − , otherwise . For empirical values of ”small,” refer [3], [27]. Alternatively,it is also possible to maximize (6) w.r.t. the parameters usingstochastic gradient descent.
C. Learning multi-modal movements (DGM-VMM)
The method in section III-B assumes that movementsoccur only in one direction. Although this assumption mightbe applicable for roads with vehicles running in dedicatedlanes, such an assumption is not generally suitable for cross-walks, sidewalks, manipulators, or aerial vehicles (Fig. 3).Therefore, in order to capture multi-directional movements,we use the convex combination of a mixture of M directional a) (b) * Fig. 3: Mapping multi-modality. (a) More human paths are added to Fig. 1so that human walk in different directions in some places e.g. crosswalk(b) DGM modeled using the mixture of von Mises distributions. Withcomparison to Fig. 1 (a) which only has a single von Mises distribution,observe that some cells in (b) have two lobes indicating the method’scapacity to learn bimodal movements. The middle-left cell shows a morecircular distribution because movements occur in different directions. distributions. The probability density function of such amixture with M von Mises distributions is given by (10). VMM ( θ ; α, µ, κ ) := M (cid:88) m =0 α m VM ( θ ; µ m , κ m ) , (10)with (cid:80) Mm =0 α m = 1 for α m ≥ to guarantee VMM is avalid probability density function.However, there is no closed-form solution to find the op-timal parameter set { ( α m , µ m , κ m ) } Mm =1 . Therefore, as withGaussian mixture models, the Expectation-Maximization(EM) algorithm can be used [34]. This is an iterative proce-dure where the posterior is estimated using the parametersestimated in the previous iteration, and the parameters areupdated using the estimated posterior in the current itera-tion. Once the parameters do not significantly change overiterations, the optimization procedure can be stopped. Thisis detailed in Algorithm 1.The naive EM algorithm only learns the parameters,not the number of mixture components M . Although it ispossible to preset it as a fixed number, in order to makethe algorithm faster and to make mapping fully autonomous,we made use of DBSCAN [35] clustering technique andinitialized { µ m } Mm =1 with cluster centers, and then optimizedusing the EM algorithm. Unlike the popular k-means al-gorithm where the user requires to provide the number ofclusters, DBSCAN determines it using the density of datapoints which is indeed our requirement.In a similar fashion to a Gaussian mixture model [36],in the E-step, the elements of the responsibility matrix arecomputed as in (11). γ mn = α m VM m ( θ n ) (cid:80) Mm (cid:48) =1 α m (cid:48) VM m (cid:48) ( θ n ) , (11)where VM m indicates a von Mises probability densityfunction of the mixture component m . Having obtained γ mn ,the objective of the M-step is to learn the parameters using(12)-(14), α m = (cid:80) Nn =1 γ mn N , (12) µ m = (cid:80) Nn =1 γ mn θ n (cid:107) (cid:80) Nn =1 γ mn θ n (cid:107) , (13) κ m = A − (cid:18) (cid:107) (cid:80) Nn =1 γ mn θ n (cid:107) (cid:80) Nn =1 γ mn (cid:19) , (14)where A − ( · ) is the inverse of Bessel function ratios asdescribed in Section III-B. Algorithm 1:
EM algorithm for multi-modal learning.
CalcRes() and
UpdateParameters() are (11) and (12)-(14), respectively.
Input: { θ n } Nn =1 { µ (0) m } ? m =1 = DBSCAN( θ ) //get density centers; M = size ( { µ m } ) //number of mixture components;Initialize α (0) m = 1 /M , for m = 1 : M ;Initialize κ (0) m (cid:38) +0 , for m = 1 : M ;Initialize (cid:15) ≈ +0 ;Initialize i = − //iterations; while (cid:107) µ ( i ) m − µ ( i − m (cid:107) ≤ (cid:15) do i ← i + 1 ;//E-step; for n = 1 to N dofor m = 1 to M do ˆ γ ( i ) mn = CalcRes (cid:0) θ n , α ( i − m , µ ( i − m , κ ( i − m (cid:1) ; endend //M-step; for m = 1 to M do (cid:0) α ( i ) m , µ ( i ) m , κ ( i ) m (cid:1) = UpdateParameters (cid:0) ˆ γ ( i ) mn (cid:1) ; endendOutput: α ( i ) m , µ ( i ) m , κ ( i ) m // VMM (10)IV. EXPERIMENTS
A. Experimental setup and evaluation metrics
As given in Table I, we used a variety of datasets fromsimulated and real-world environments having both LiDARand cameras, to validate different aspects of the proposedmethods and answer questions raised in Section III.In order to assess models, we used several metrics. In a M -mixture of distributions, the expected negative log-likelihood(ENLL) is calculated as the average negative log-likelihoodover all data points[40] which indicates the likelihood agiven data point sampled from the distribution parameterizedby { ( µ m ∗ , κ m ∗ ) } Mm =1 . For unimodal settings M = 1 . Thesmaller the NLL or ENLL, the better the model fit is.As another metric, average probability density (APD) isconsidered. Metrics are calculated with a 10-fold cross-validation procedure. For each fold of test data, the prob-ability density is calculated and averaged. Intuitively, if the ABLE I: Description of datasetsDatasets DescriptionUnimodal Similar to [20], this simulated dataset representshuman walking paths which collectively have a uni-modal directional pattern i.e. at a given location allhuman walk approximately in the same direction.Observations are assumed to be taken from the topview. (Fig. 1 (a))Multi-modal This is the multi-modal (to be exact, bi-modal)extension to the above unimodal dataset. (Fig. 3 (a))Edinburgh The publicly available
Edinburgh
Informatics ForumPedestrian Database (Aug.24) [37] is used. The setupis an highly dynamic outdoor environment with RGBcameras setup on top to track [37] people. (Fig. 5 (a))Kuka Here, we use the
Kuka robot arm in the MORSEsimulator [38]. The location of the end-effector inthe 2D space was tracked. Using only two joints, wemanipulated the robot to make planar movements tosimulate a repetitive task with normally distributedrandom perturbations to the goal locations. Becauseof this perturbations, robot’s path is slightly differentin each of the 20 iterations which results in observingdifferent angles of the end effector in the samelocation. (Fig. 6 (a))Corridor This tracks five people moving in a corridor usinga moving robot with a LiDAR [39]. (Fig. 8 (a))Intersection This is similar to the corridor dataset, however in afour-way intersection (Fig. 9 (a))Human The single trajectory of a walking human in aMORSE-simulated office environment is tracked.(Fig. 10 (a)) model has captured the full long-term distribution, it givesa higher APD score because more points are concentratedaround the vicinity if that area. Unfortunately, because ofthe problem is unsupervised learning and having a mixtureof distributions, most of the standard tests that are commonlyused in linear statistics and robotics with normality assump-tions cannot be used for this setting.A notebook computer with an Intel Core-i7 processorand an 8 GM RAM was used for experiments. For allexperiments, the tolerance parameter of the VMM algorithm (cid:15) and the density parameter of the DBSCAN were set to − and . , respectively. When using the Edinburgh dataset withGaussian process [20] is used in comparisons, a low-rankapproximation [41] had to be used because the Edinburghdataset contains 76260 data points which are not feasible tofit using the full Gaussian process model. The python codewill be available soon: github.com/RansML. B. Experiment 1: Validating the EM algorithm
Firstly, we show that the iterative EM algorithm describedin Section III-C indeed minimizes the NLL over the numberof iterations. In Fig. 4, NLL is plotted against the number ofiterations for the cell marked with “ ∗ ” in Fig. 3 (b) for themulti-modal dataset. For the Multi-modal dataset, the EMalgorithm converged within 5 iterations. The mean squarederror (MSE) of angles is reported in Table III. In DGM-VMM, the MSE to the closest mode was considered. C. Experiment 2: Modeling long-term spatiotemporal effects
In this section, we used Unimodal, Multi-modal, Edin-burgh, and Kuka datasets to build the long-term spatiotempo-ral directional grid map so as to answer the question ”What
Fig. 4: Convergence of the EM algorithmTABLE II: Mean squared error (MSE) of angles.Method Unimodal Multi-modalDGM-VMM
DGM-VM 0.239 2.484 are the directions of movements in different places of theenvironment when observed over a period?”. The resolutionsof the grid maps were kept constant for demonstration andvisualization purposes ( × , × , and × cells forUnimodal, Multimodal, and Edinburgh datasets). The mapsfor Unimodal and Multimodal datasets are shown in Fig. 1(b) and 3 (b), respectively. Around 15% of randomly selectedtrajectories are shown in Fig. 5 (a) and the correspondingDGM in Fig. 5 (b). By comparing the trajectories anddirections of lobes with intensities, it is possible to seethe model has successfully learned the directions, includingmulti-modality. In order to demonstrate that the proposedalgorithm is well suitable for other domains, we used theKuka dataset. Additionally, as shown in Fig. 6 (b), ratherthan maintaining a fixed grid, observations were taken froma few user-specified locations in the space.The quantitative aspects of different methods are given inTable III. DGM-VMM is faster because 1) its means areinitialized from DBSCAN, and 2) it has the flexibility toadjust to any shape. For the Unimodal dataset, the Gaussianprocess-based method proposed in [20] works well. Asillustrated in Fig 7, because the Gaussian process cannothandle multi-directional data, it merely averages directions,resulting in incorrect predictions. D. Experiment 3: Analyzing spatial variations
This is merely a demonstration to show that the samemodel can be used to answer “at a given time, where doeveryone in the environment move?” For this purpose, alldata points at a given time (i.e. t = fixed and all cells) in theEdinburgh dataset were considered and the EM algorithmwas run for the von Mises mixture. The resulting polar plotis shown in Fig. 5 (c). To interpret, considering the entireenvironment, many people move towards ≈ − ◦ and a fewpeople ≈ ◦ . This kind of an analysis provides a summarystatistic about the environment at a given time. Further, itis also possible to answer questions such as how fast thedistribution changes over time by quantifying using mutualinformation or KullbackLeibler divergence. E. Experiment 4: Analyzing temporal variations
In this experiment, we analyze the temporal evolution ofdynamic objects individually. For this purpose, we used theCorridor and Intersection datasets, and corresponding direc-tional distributions are found in Figs. 8 and 9, respectively. a) 100 randomly selected trajectories overlaid on thereal environment. (c) Spatial analysis (b) VMM-DGM for (a)Fig. 5: Edinburgh pedestrian dataset shows how people near the University of Edinburgh’s Atrium move from one building to another on a regular day.TABLE III: Performance metrics for different methods. The smaller the ENLL or the higher the APD, the better the model is.Unimodal dataset Multimodal dataset Edinburgh dataset Kuka datasetMethod ENLL APD Time ENLL APD Time ENLL APD Time ENLL APD TimeDGM-VMM ±
13 0.251 1.015 33 ±
27 1.483 0.251 28 ±
25 -0.087 1.190 13 ± DGM-VM 0.177 1.172 71 ±
34 0.696 0.615 115 ±
62 1.733 0.202 72 ±
72 0.747 0.699 55 ± ±
16 N/A 0.211 213 ±
120 N/A 0.124 > ± (b) Fig. 6: The Kuka robot arm (a) The approximate path of the end-effector isshown in blue arrows. Directional map of the Kuka end-effector movements.(b) Some of the data points collected over 20 such cycles with perturbationare shown in blue dots. The corresponding directional distribution atarbitrary locations are indicated by ploar plots.
Then, using the Human dataset, the temporal evolution ofthe map was analyzed in a sequential learning setting. Inthe office environment shown in Fig 10 (a), the trajectoryof a simulated human is shown in Fig 10 (b). The modelis learned in an online fashion as data are collected over286 times steps. In Fig 10 (c), the directional distribution offour such time steps are shown. Starting with a disperseddistribution (i.e. any angle is possible or κ ≈ ), theobserving robot sequentially learns the directions the humanmoves. V. DISCUSSIONSSimilar to a Gaussian distribution, the mean, mode, andmedian of a unimodal von Mises is the same. However, when (a) (b) Fig. 7: Gaussian process (GP) mapping [20] for Multimodal dataset wheremovements occur in different directions. This can be compared with Fig. 3.(a) mean direction (b) confidence as variance. Observe that the predictionsaround places where multi-directional movements occur i.e. crosswalk andlower-left sidewalk is not accurate. In such places the GP averages alldirections. As shown in (b), despite the inaccurate predictions the confidenceabout the prediction in such areas is also very high. Although angles arelimited to [ − ◦ , ◦ ] in the figure, they can be in the range ( −∞ ◦ , ∞ ◦ ) without satisfying the recurrence relationship f ( θ ) = f ( θ + 360 ◦ ) . a mixture of von Mises is considered these quantities can bedifferent. Especially, for practical applications, it is importantto determine the modes. This is not straightforward becausethe values of κ and µ determine both the number of modesand where they are. However, because von Mises belongsto the exponential family of distributions, it is possible toutilize similar algorithms that are used to find modes in themixture of Gaussians [42] or wavelet-based methods used insignal processing to find peaks [43].In the proposed algorithm, we used DBSCAN to determine a) A robot and five humans move in a corridor simultaneously. The pointcloud is shown in gray. [39](b) Directional distribution of each track is indicated by polar plots withcorresponding colors. Fig. 8: Corridor dataset(a) Five humans move in an intersection simultaneously. The point cloud isshown in gray. [39](b) Directional distribution of each track is indicated by polar plots withcorresponding colors. Fig. 9: Intersection dataset. the number of mixture components. As this is not partof the EM algorithm, the number of mixture componentsmaybe suboptimal. In order to further improve the likelihood,especially in high dimensional settings or when there isa small amount of data, taking a Bayesian approach, it ispossible to consider M as a parameter to be learned. Takingfurther advantages of the exponential family of distributions,as with the mixture of Gaussians [36], it can be easilyfactorized and apply variational inference to jointly learn thenumber of mixtures as well as mixture parameters.The one dimensional formulation can be easily extendedto higher dimensions using the von Mises-Fisher extensiongiven by 15 for ( D − dimensions, VM D ( θ ; µ, κ ) := κ D/ − (2 π ) D/ J D/ − ( κ ) exp (cid:0) κµ (cid:62) θ (cid:1) . (15)Such an extension can be used in modeling 3D spatiotem-poral dynamics or modeling joint rotational uncertaintiesin robot manipulators with high degrees of freedom. Bymodeling bi-directional 3D uncertainties of a toy dataset (e.g.a moving drone in 3D), Fig. 11 (b) illustrates that such anextension is straightforward. (a) A person walking in the officeenvironment. (b) The plain view of the trajectory.(c) The directional distribution for four tome steps. Initially, it is a uniformcircular distribution. Observe how the direction of lobes changes with thedirection of trajectories at different time steps.Fig. 10: Human datasetFig. 11: Usefulness of DGM (a) The directional distribution of Person 5 inFig. 9 with colors indicating different speeds i.e. speed for different angleswith the probability of angle (b) Demonstration of 3D lobes modeled usingthe von Mises-Fisher distribution. Interestingly, it is possible to use these directional mapsalong with other information extracted from dynamic envi-ronments to build maps that are informative. For instance,Fig. 11 (a) shows speeds (separately modeled) of differentdirections of ”Person 5” in Fig. 9. Additionally, a large DGMcan be easily bundled together with an occupancy grid mapto represent the dynamics of the environment well. Theseinformation-rich maps can then be used to make planningalgorithms robust [21], [23].One of the main limitations of the proposed method, aswith any other grid-based method [1], the independenceassumption among cells [8]. Therefore, it might be usefulto consider the directional distribution as a conditional dis-tribution or to have a more continuous representation [14],[44]. VI. CONCLUSIONSWe presented a robust algorithm to estimate angular un-certainties ubiquitous in robotics. To this end, we effectivelywe made use of directional statistics that are not typicallyutilized in robotics. Our method is generic enough to beused in any robotic platform such as mobile robots, drones,manipulators, etc. or in a variety of domains such as indoormapping, field robotics, and human-robot interaction.
EFERENCES[1] A. Elfes, “Sonar-based real-world mapping and navigation,”
IEEEJournal of Robotics and Automation , vol. RA-3(3), pp. 249–265, 1987.[2] F. Ramos and L. Ott, “Hilbert maps: scalable continuous occupancymapping with stochastic gradient descent,” in
Proceedings of Robotics:Science and Systems (R:SS) , Rome, Italy, July 2015.[3] K. V. Mardia and P. E. Jupp,
Directional statistics . New York: Wiley.Merrifield, 2006.[4] P. Henry, M. Krainin, E. Herbst, X. Ren, and D. Fox, “Rgb-d mapping:Using kinect-style depth cameras for dense 3d modeling of indoorenvironments,”
The International Journal of Robotics Research (IJRR) ,vol. 31, no. 5, pp. 647–663, 2012.[5] J. Bohg, “Multi-modal scene understanding for robotic grasping,”Ph.D. dissertation, KTH Royal Institute of Technology, 2011.[6] J. Yin, L. Carlone, S. Rosa, and B. Bona, “Graph-based robustlocalization and mapping for autonomous mobile robotic navigation,”in
Mechatronics and Automation (ICMA), 2014 IEEE InternationalConference on . IEEE, 2014, pp. 1680–1685.[7] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss, and W. Bur-gard, “Octomap: An efficient probabilistic 3d mapping frameworkbased on octrees,”
Autonomous Robots , vol. 34, no. 3, pp. 189–206,2013.[8] S. T. O’Callaghan and F. T. Ramos, “Gaussian process occupancymaps,”
The International Journal of Robotics Research (IJRR) , vol. 31,no. 1, pp. 42–62, 2012.[9] D. Hahnel, R. Triebel, W. Burgard, and S. Thrun, “Map buildingwith mobile robots in dynamic environments,” in
IEEE InternationalConference on Robotics and Automation (ICRA) , 2003, pp. 4270–4275.[10] W. Burgard, C. Stachniss, and D. H¨ahnel, “Mobile robot map learningfrom range data in dynamic environments,” in
Autonomous Navigationin Dynamic Environments . Springer, 2007, pp. 3–28.[11] A. Walcott-Bryant, M. Kaess, H. Johannsson, and J. J. Leonard,“Dynamic pose graph slam: Long-term mapping in low dynamic envi-ronments,” in
Intelligent Robots and Systems (IROS), 2012 IEEE/RSJInternational Conference on . IEEE, 2012, pp. 1871–1878.[12] R. Senanayake, L. Ott, S. O’Callaghan, and F. Ramos, “Spatio-temporal hilbert maps for continuous occupancy representation indynamic environments,” in
Neural Information Processing Systems(NIPS) , 2016.[13] R. Senanayake, S. O’Callaghan, and F. Ramos, “Learning highlydynamic environments with stochastic variational inference,” in
IEEEInternational Conference on Robotics and Automation (ICRA) , 2017.[14] R. Senanayake and F. Ramos, “Bayesian hilbert maps for dynamiccontinuous occupancy mapping,” in
Conference on Robot Learning(CoRL) , 2017, pp. 458–471.[15] J. Saarinen, H. Andreasson, and A. Lilienthal, “Independent MarkovChain Occupancy Grid Maps for Representation of Dynamic Envi-ronment,” in
IEEE/RSJ International Conference on Intelligent Robotsand Systems (IROS) , 2012, pp. 3489–3495.[16] D. Meyer-Delius, M. Beinhofer, and W. Burgard, “Occupancy GridModels for Robot Mapping in Changing Environments,” in proc. AAAIConference on Artificial Intelligence (AAAI) , 2012, pp. 2024–2030.[17] Z. Wang, P. Jensfelt, and J. Folkesson, “Modeling spatial-temporaldynamics of human movements for predicting future trajectories,” in
AAAI Conference on Artificial Intelligence (AAAI) , 2015, pp. 42–48.[18] T. Krajnık, P. Fentanes, G. Cielniak, C. Dondrup, and T. Duckett,“Spectral analysis for long-term robotic mapping,” in
IEEE Inter-national Conference on Robotics and Automation (ICRA) , 2014, pp.3706–3711.[19] N. Bore, “Object instance detection and dynamics modeling in a long-term mobile robot context,” Ph.D. dissertation, KTH Royal Instituteof Technology, 2017.[20] S. T. O’Callaghan, S. P. Singh, A. Alempijevic, and F. T. Ramos,“Learning navigational maps by observing human motion patterns,” in
Robotics and automation (ICRA), 2011 IEEE international conferenceon . IEEE, 2011, pp. 4333–4340.[21] Z. Marinho, B. Boots, A. Dragan, A. Byravan, G. J. Gordon, andS. Srinivasa, “Functional gradient motion planning in reproducing ker-nel hilbert spaces,” in
Proceedings of Robotics: Science and Systems(R:SS) , 2016.[22] J. Dong, M. Mukadam, F. Dellaert, and B. Boots, “Motion planningas probabilistic inference using gaussian processes and factor graphs.”in
Robotics: Science and Systems (R:SS) , vol. 12, 2016. [23] M. Norouzii, J. V. Miro, and G. Dissanayake, “Probabilistic stablemotion planning with stability uncertainty for articulated vehicles onchallenging terrains,”
Autonomous Robots , vol. 40, no. 2, pp. 361–381,2016.[24] M. Mukadam, J. Dong, F. Dellaert, and B. Boots, “Simultaneoustrajectory estimation and planning via probabilistic inference,” in
Proceedings of Robotics: Science and Systems (R:SS) , 2017.[25] T. D. Bui, C. Nguyen, and R. E. Turner, “Streaming sparse gaussianprocess approximations,” in
Advances in Neural Information Process-ing Systems , 2017, pp. 3301–3309.[26] R. von Mises, “ ¨Uber die ’ganzzahligkeit’ der atomgewichte undverwandte fragen,”
Phys. Z , vol. 19, no. 490-500, 1918.[27] K. V. Mardia and R. Edwards, “Weighted distributions and rotatingcaps,”
Biometrika , vol. 69, no. 180, pp. 323–330, 1982.[28] M. S. Handcock and J. R. Wallis, “An approach to statistical spatial-temporal modeling of meteorological fields,”
Journal of the AmericanStatistical Association , vol. 89, no. 426, pp. 368–378, 1994.[29] J. L. van Hemmen, “Vector strength after goldberg, brown, and vonmises: biological and mathematical perspectives,”
Biological cybernet-ics , vol. 107, no. 4, pp. 385–396, 2013.[30] I. Gilitschenski, G. Kurz, S. J. Julier, and U. D. Hanebeck, “Unscentedorientation estimation based on the bingham distribution,”
IEEE Trans-actions on Automatic Control , vol. 61, no. 1, pp. 172–177, 2016.[31] G. Farneb¨ack, “Two-frame motion estimation based on polynomialexpansion,” in
Image Analysis . Springer, 2003, pp. 363–370.[32] S. Sivaraman and M. M. Trivedi, “Looking at vehicles on the road:A survey of vision-based vehicle detection, tracking, and behavioranalysis,”
IEEE Transactions on Intelligent Transportation Systems ,vol. 14, no. 4, pp. 1773–1795, 2013.[33] R. H. G¨uting and M. Schneider,
Moving Objects Databases . MorganKaufmann, 2005.[34] I. S. Dhillon and S. Sra, “Modeling data using directional distribu-tions,” Technical Report TR-03-06, Department of Computer Sciences,The University of Texas at Austin., Tech. Rep., 2003.[35] M. Ester, H.-P. Kriegel, J. Sander, X. Xu, et al. , “A density-basedalgorithm for discovering clusters in large spatial databases withnoise.” in
ACM SIGKDD Conference on Knowledge Discovery andData Mining (KDD) , vol. 96, no. 34, 1996, pp. 226–231.[36] C. Bishop,
Pattern Recognition and Machine Learning . Springer,2006.[37] B. Majecka, “Statistical models of pedestrian behaviour in the forum,”
Master’s thesis, School of Informatics, University of Edinburgh , 2009.[38] G. Echeverria, S. Lemaignan, A. Degroote, S. Lacroix, M. Karg,P. Koch, C. Lesire, and S. Stinckwich, “Simulating complex roboticscenarios with morse,” in
Simulation, Modeling, and Programming forAutonomous Robots (SIMPAR) , 2012, pp. 197–208.[39] C. Romero-Gonz´alez, ´A. Villena, D. Gonz´alez-Medina, J. Mart´ınez-G´omez, L. Rodr´ıguez-Ruiz, and I. Garc´ıa-Varea, “Inlida: A 3d lidardataset for people detection and tracking in indoor environments.” in
Proceedings of the 12th International Joint Conference on ComputerVision, Imaging and Computer Graphics Theory and Applications(VISIGRAPP) , 2017, pp. 484–491.[40] D. Ruppert,
Statistics and data analysis for financial engineering .Springer, 2011, vol. 13.[41] GPy, “GPy: A gaussian process framework in python,” http://github.com/SheffieldML/GPy, since 2012.[42] M. A. Carreira-Perpinan, “Mode-finding for mixtures of gaussiandistributions,”
IEEE Transactions on Pattern Analysis and MachineIntelligence , vol. 22, no. 11, pp. 1318–1323, 2000.[43] P. Du, W. A. Kibbe, and S. M. Lin, “Improved peak detection in massspectrum by incorporating continuous wavelet transform-based patternmatching,”
Bioinformatics , vol. 22, no. 17, pp. 2059–2065, 2006.[44] L. McCalman, S. O’Callaghan, and F. Ramos, “Multi-modal estima-tion with kernel embeddings for learning motion models,” in