{'fraction_non_alphanumeric': 0.03492960658958143, 'fraction_numerical': 0.008460994985438817, 'mean_word_length': 4.410909090909091, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 2, 'https://': 3, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 5, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Continuous Occupancy Mapping in Dynamic Environments Using Particles

Dynamic occupancy maps were proposed in recent years to model the obstacles in dynamic environments. Among these maps, the particle-based map offers a solid theoretical basis and the ability to model complex-shaped obstacles. Current particle-based maps describe the occupancy status in discrete grid form and suffer from the grid size problem, namely: large grid size is unfavorable for path planning while small grid size lowers efficiency and causes gaps and inconsistencies. To tackle this problem, this paper generalizes the particle-based map into continuous space and builds an efficient 3D local map. A dual-structure subspace division paradigm, composed of a voxel subspace division and a novel pyramid-like subspace division, is proposed to propagate particles and update the map efficiently with the consideration of occlusions. The occupancy status of an arbitrary point can then be estimated with the cardinality expectation. To reduce the noise in modeling static and dynamic obstacles simultaneously, an initial velocity estimation approach and a mixture model are utilized. Experimental results show that our map can effectively and efficiently model both dynamic obstacles and static obstacles. Compared to the state-of-the-art grid-form particle-based map, our map enables continuous occupancy estimation and substantially improves the performance in different resolutions. We also deployed the map on a quadrotor to demonstrate the bright prospect of using this map in obstacle avoidance tasks of small-scale robotics systems.


I. INTRODUCTION
The particle-based map is originally proposed in [1] for dynamic environments. Particles with position and velocity states are used to approximate both dynamic obstacles and static obstacles on the basis of the sequential Monte Carlo (SMC) filtering. In recent works, [2] introduces the theory of random finite set (RFS) to particles-based maps. The probability hypothesis density (PHD) filter is applied to predict and update the particles and estimate the dynamics of the grids in the map. Later, [3], [4], [5] improve the particles-based maps by considering mixture model, semantic information and high-level occupancy status inference, respectively. Because of the solid theoretical basis and the ability to model complexshaped static and dynamic obstacles simultaneously, particlebased maps draw recent attention in representing dynamic environments. Currently, the input form of the particle-based maps is the ray-casting-generated measurement grid map originated from the first work [1], and thus the map is discretized with grids. This discrete form inhibits the state estimation resolution and brings the grid size problem, namely: large grids lead to a low resolution that is unfavorable for path planning, while small grids increase the computation requirements and cause gaps and inconsistencies when the input point clouds are sparse or noisy [6]. Besides, desktop GPUs are required to run the particle-based maps in real-time, and a more efficient map is required for the applications in small-scale robotic systems.
This work proposes a dual-structure particle-based (DSP) dynamic occupancy map, which is continuous and free from the grid size problem. The input of this map is the raw point cloud rather than the measurement grid map. A novel dual-structure map building paradigm, composed of a voxel subspace division for particle storage and resampling, and a dynamic pyramid-like subspace division for occlusion-aware particle update, is proposed to model the local environment with particles that have continuous states. Under the Gaussian noise assumption, we demonstrate that this updating paradigm is effective and computationally efficient. To reduce the noise in simultaneously modeling static and dynamic obstacles, the importance of newborn particles is addressed by using non-Gaussian initial velocity estimation, and a mixture model that adaptively allocates the number of static and dynamic particles. With a complete process of prediction, update, birth, and resampling of particles, the occupancy status of the local environment can be represented and predicted in the continuous space using onboard CPU devices.
In the experimental tests, we first evaluated the mapping efficiency and dynamic obstacle velocity estimation performance. Then the comparison tests with a state-of-the-art particle-based dynamic occupancy map [5], and a widely used static occupancy map [7] were conducted. Results show that our map has the best occupancy status estimation performance in dynamic environments and competitive performance with [7] in static environments. Furthermore, we verified the DSP map in obstacle avoidance tasks of a mini quadrotor in different environments. To the best of the authors' knowledge, this is the first continuous particle-based occupancy map and the first dynamic occupancy map that can be applied to smallscale robotic systems like quadrotors.
The main contributions of this work include four points: 1) A novel dual-structure particle-based map building paradigm that enables continuous mapping of the occupancy status of dynamic environments. 2) The leverage of initial velocity estimation and an efficient mixture model to reduce noise in modeling static arXiv:2202.06273v1 [cs.RO] 13 Feb 2022 and dynamic obstacles simultaneously.
3) The complete procedures of building a DSP map that can be applied to onboard computing devices of smallscale robotic systems. 4) The released code at https://github.com/g-ch/DSP-map, including an example application in ROS. The remaining content is organized as follows: Section II describes the related work of dynamic occupancy map. Section III presents the formulations of the world model based on random finite sets and an overview of mapping procedures. Section IV expresses the mapping procedures with the dual structure. In Section V, more components for mapping are discussed. The experimental results and the conclusion are presented in Section VI and Section VII, respectively.

II. RELATED WORK A. Discrete Map and Continuous Map
Environment representation is fundamental to obstacle avoidance of robotics systems. One of the most popular representation approaches is occupancy mapping, which originated from [8] and is capable of modeling cluttered environments. Grids (2D or 3D) are computationally efficient forms to realize occupancy mapping. The environment is usually divided into discrete grids, and the occupancy status of each grid is updated with the ray casting algorithm [9], [7], [10], [11]. The size of the grids, however, is difficult to determine. Large grids lead to a low resolution that is unfavorable for path planning. Small grids increase the computation requirements and cause gaps and inconsistences when the input point clouds are sparse or noisy [6]. To avoid the grid size problem and allow arbitrary resolutions, the paradigm of building the map with continuous occupancy probability kernels rather than grids is proposed [6], [12], [13]. Free space and occupied points or segments are firstly generated with the input point clouds and then used to update the parameters in the kernel functions. The occupancy status of an arbitrary position can then be estimated with nearby kernels.

B. Occupancy Maps in Dynamic Environments
The maps mentioned above [8]- [13] are built under the assumption that the environment is static. As the robotic systems were deployed in dynamic environments, improvements have to be made to instantly represent the occupancy position of dynamic obstacles, such as pedestrians and other robots, and, even further, to predict the future trajectories of dynamic obstacles. An intuitive approach is to leverage independent detection and tracking of moving objects (DATMO) [14], [15], [16] to model the dynamic obstacles and utilize static occupancy maps still to represent the other objects. A prerequisite of DATMO is that the detection model and shape model of the dynamic obstacles are well-trained [3], which conflicts with the unknown environment characters in many tasks. In addition, difficulties in data association [3] and the trail noise caused by obstacles movements in the static map [15] are intractable. Therefore, improving the map itself directly by considering the dynamic obstacle assumption is required, and the dynamic occupancy map [17], [18] emerges accordingly.
Early dynamic occupancy maps treat the dynamic obstacles, such as pedestrians and robots, as spurious data in the map, and detect and remove the data from the map to build a robust static map [17], [18], [19], [20]. Starting from the latest decade [1], research works considering modeling the dynamics, mostly velocities, of the obstacles in the map have been carried out to improve the obstacle avoidance performance in dynamic environments. The methods in these works are various. Some apply the dynamic obstacle assumption to the existing structures of static occupancy maps. For example, [21] adopts optical-flow-based motion maps to estimate the velocity of grids and improves the Gaussian process occupancy map [12] to adapt to dynamic environments. [22] improves [21] by learning dynamic areas with stochastic variational inference. In [23], point clouds from lidar are clustered and filtered to estimate the velocities of dynamic obstacles. The estimation is applied to generate non-stationary kernels in the Hilbert space to build the dynamic Hilbert map. With the popularity of deep learning methods, some recent works [24], [25] adopt recurrent neural networks to predict the velocity of each grid in a grid map.

C. Particle-based Dynamic Occupancy Maps
Compared to the above mentioned methods [21], [23], [22], [24], [25], particle-based methods are originally designed for dynamic environments. In particle-based methods, an obstacle is regarded as a set of point objects. And the particles with velocities are used to model the point objects. The theoretical foundation of the particle-based methods originates from the SMC filter [1], [26]. Researchers in [2] introduce the RFS theory to particle-based maps and derive map building procedures with the PHD filter and the Bernoulli filter. Since the environment is supposed to be unknown, all the point objects are modeled with particles that have velocities and weights [1], [2], [26]. In the particle birth procedure, the velocities of the particles are randomly generated and the weights are allocated with a small value. During the filtering process, the weights are updated with observations, which are ray-castinggenerated measurement grid maps [1]. Then the particles with large weights can statistically represent the point objects.
In a cluttered environment with both dynamic obstacles and static obstacles, multiple point objects, dynamic or static, need to be modeled and non-negligible noise can occur. To reduce the noise, two approaches are usually adopted. The first approach is to use a mixture model [27], [28], [3], [5], which includes a separate static model and a dynamic model, to update the states of static and dynamic point objects independently. The mixture model works as dual PHD filters [27] or the grid-level inference [28], [3], [5]. Another approach is to apply additional information to reduce the noise in the updating procedure. For instance, [4] adds an extra semantic grid channel in the input to generate particles with semantic labels and update with the semantic association.
The above particle-based maps adopt measurement grids as the input and express the environment with discretized girds. This discretized expression form suffers from the grid size problem mentioned in Section I. The grid size also limits the state estimation resolution of the obstacles. Therefore, a continuous form is required. In addition, since a large number of particles are used, current particle-based maps rely on Desktop GPU devices for computation. Improvement in efficiency is necessary to deploy the particle-based map on small-scale robotic systems.

III. WORLD MODEL AND SYSTEM OVERVIEW
The DSP map is an egocentric map built on multi-object tracking at the point object level in a continuous neighborhood space. Let S denote this neighborhood map space. S is a real space that has a cuboid boundary with size (L x , L y , L z ). At the center of this cuboid is the robot. The size can be set according to the range of the utilized sensors or the requirements from the local path planner. We consider the obstacles in S as point objects similar to [2]. Fig. 1(a) reveals the relation between obstacles and point objects. One obstacle can correspond to multiple point objects. The number of the corresponding point objects equals the number of points in the point cloud that is used to measure the obstacle. Since the obstacles are unknown, the number of the point objects and their states are random but finite. Therefore, these point objects can be modeled as a random finite set (RFS) [2]. At a certain time or a discrete iteration k, the RFS of the point objects is represented as where N k is the number of point objects at k, and x with index from 1 to N k is the state vector of a point object. The state vector of a point object is given by the 3D position and velocity, namely where the subscripts {x, y, z} are used to represent the axes in Cartesian coordinate. In our map, the states of the point objects are to be estimated with particles. The state of the obstacles can be approximated by the point objects. Since it is difficult and time-consuming to determine which point objects belong to the same obstacle in real applications, the assumption that all the point objects move independently is made. The same assumption is used in the previous works on dynamic occupancy maps, such as [1], [2], and [5].
Dividing the map space S into different subspaces by the position dimensions, X k can be accordingly divided into subsets which are also RFSs. In this work, the subspaces are divided with two structures, which form our dual structure, to realize efficient occupancy status estimation with the sequential Monte Carlo (SMC) PHD filter in the continuous space. The basic steps of the SMC-PHD filters is shown in the Part 3 in Fig. 2.
The first subspace division structure is the cubic voxel subspaces in the cartesian coordinate ( Fig. 1(b)). The voxels can fill up S but have no overlaps with each other. Assume the resolution of the voxel is l. Then the number of the voxels is . Let S (Vi) denote the i th voxel subspace. The point objects in S (Vi) can also be formed as an RFS X (Vi) k . Then X k can then be described as the union of these sub RFSs, which is Since the voxels have no overlaps, any two RFSs in X (V1) k , ..., X V (Nv ) k don't share a point object and thus are independent.
In the SMC-PHD filter, the voxel subspaces are used to resample the particles in S in a uniform manner, which is described in Section IV-E. In addition, these voxel subspaces are used to index and store the particles for efficiency purposes, which is described in Section V-E. However, the voxel subspaces cannot continuously express the occluded space that is significant for updating the map. Thus another division structure that can represent occluded space is required.
Inspired by the perspective projection model for sensors, we also divide S into pyramid-like subspaces in the spherical coordinate ( Fig. 1(d)). These subspaces are divided dynamically and uniformly in the sensor frame. The real shape of a pyramid-like subspace is composed of four triangular faces and one face on the map boundary face. For simplification, we loosely call the subspace as pyramid subspace in the following.
In the spherical coordinate, the azimuth angle range is [0, 2π] and the zenith angle range is [0, π]. Suppose the angle interval of the pyramid division, namely the vertex angle of the pyramid subspace, is θ. θ > 0, and by θ the angle π is divisible. The number of these subspaces is N a = 2π·π θ 2 . Let S (Ai) denote the i th pyramid subspace, and X (Ai) k denote the RFS composed of point objects in S (Ai) . Then X k satisfies The measurement Z k is the point cloud from sensors, such as stereo cameras or Lidars, and can also form an RFS. In analogy to the point objects, the measurement RFS at time k is written as where M k represents the number of the measurement points, and the measurement point z, with index from 1 to M k , consists of the 3D position of each point in the point cloud, which is Let S f ⊂ S, with superscript f , denote the FOV space, which is the space in current sensor FOV. The space in green in Fig. 3. (a) and (b) shows S f , which can be regarded as the union of some visible pyramid subspaces S (A f i ) in the FOV. When the vertex angle θ of the pyramid subspace S (Ai) equals the angle resolution of the sensor, there would be either one or no measurement point in a S (Ai) . If there is a measurement point z in S (Ai) , the subspace behind the measurement point is occluded (painted in gray in Fig. 3.), while the subspace before the measurement point is the visible pyramid subspace S (A f i ) and is possibly free. S (A f i ) ⊂ S (Ai) and the length of   Since the FOV usually cannot cover the whole neighborhood , and Z k can be divides into subsets with these visible pyramid subspaces, which is With the measurement Z k , the states of the point objects in X k are updated by using the SMC-PHD filter. The hollow circles with velocity arrows in Subfigures (b), (c), and (d) in Fig. 1 show the particles used in the SMC-PHD filter. The basic element in our map is the particle.
An overview of the procedures to build our DSP map can be found in Fig. 2. The core procedures are prediction, update, particle birth, and resampling, which are the main procedures of the SMC-PHD filter. Objects in X (Ai) k and measurement , divided by pyramid subspaces, are used in the update procedure, and X (Vi) k divided by the voxel subspaces is used in the resampling procedure. Two adjacent point cloud frames are used to estimate an initial velocity for z (j) , which is then utilized in the particle birth procedure. The filtered result is particles with position and velocity states. Then the output of the map can be calculated with the particles into two forms designed for motion planning. The first form is the current occupancy status, and the second form is the prediction of future occupancy status. Detailed methods for mapping are presented in Section IV and Section V.

IV. MAPPING WITH DUAL STRUCTURE
In this section, the method to build our map with the dualstructure space divisions is presented. In the following, we first introduce the general PHD filter and two properties used in our map. Then we present the detailed prediction, update, particle birth, and resampling approaches with dual structure in our map. The prediction and the particle birth are conducted in space S. In the update procedure, the pyramid subspaces S (Ai) are utilized to update the point objects' PHD efficiently. The voxel subspaces S (Vi) are adopted in the resampling procedure.

A. PHD filter
Probability hypothesis density (PHD) [29] is raised to describe the multi-object density of an RFS. The PHD is the first moment of an RFS and is defined as: is the expectation and δ(·) is the Dirac function. Two important properties of PHD are used in this work. The first property is that the integral of PHD is the expectation of the cardinality of X [29], [30], which can be expressed as where |X| is the cardinality of X and reflects the number of objects in X. This property is used to estimate occupancy status later. Another property is that if X (1) , X (2) , ..., X (N ) are independent RFSs, and X (1) ∪ X (2) ∪ ... ∪ X (N ) = X, such as the sub RFSs in Equations (3) and (4), then [31] The sub RFSs, such as X (Ai) and X (Vi) , in different spaces, satisfy this property. The PHD filter [29] is an efficient filter that propagates the PHD of X k in the prediction and the update procedure, and can be used to handle the multiple point objects tracking problem in our map. In a typical PHD filter, the prior object set X k|k−1 at time k can be treated as the union of two independent subsets, which is X k|k−1 = S k|k−1 ∪ B k|k−1 , where S k|k−1 represents the persistent objects from X k−1 and B k|k−1 is the newly born objects. Note S k|k−1 and B k|k−1 are distinguished by birth time rather than space and thus they are both in map space S but don't share any element. S k|k−1 is usually modeled with Multi-Bernoulli mixture (MBM), and from X k−1 to S k|k−1 , the objects have a probability of P s to survive. Meanwhile, B k|k−1 is modeled as a Poisson point process (PPP) [29] with intensity γ k|k−1 (x k ). Similarly, the measurement set Z k in the FOV at time k can be expressed where O k is the detected objects set and C k is the set of clutters. From X k to Z k , the objects have a probability of P d to be detected. The clutters C k are modeled as a PPP with intensity κ k (z k ).
Let D S k|k−1 (x k ) and D B k|k−1 (x k ) denote the PHD of S k|k−1 and B k|k−1 , respectively. Considering the property (10) and the MBM and PPP models, the general PHD filter [31] can be described as: where Equations (11) and (12) show the prediction step, and Equations (13) and (14) present the update step. π k|k−1 (·) is the state transition density of a single object and g k (·) is the single object measurement likelihood.

B. Prediction
The motion model of a single point object is defined by the constant velocity (CV) model in our work, then an arbitrary object x k that survived from k − 1 is predicted by: where E is the identity matrix and Σ is the noise. Here the noise is supposed to obey a Gaussian distribution with a covariance Q. Then the state transition density turns to To propagate the PHD in an efficient way, we use particles to represent the PHD, which is also known as the SMC-PHD filtering [32], [30]. With the particles, the posterior PHD at time k − 1 is approximated by where L k−1 is the number of particles at time k − 1, w (i) k−1 is the weight of particle with index (i), andx (i) k−1 denotes the state vector of particle (i). We distinguish the state of a point object and the state of a particle with the tilde notation.
Referring to [32], with Equations (17), (11) and (12), the PHD of the prior set X k|k−1 at time k, can be derived as: Since we use a Gaussian distribution to estimate the transition density, π k|k−1 (x k |x (i) k−1 ) can be readily sampled by particles, and D S k|k−1 (x k ) can be approximated by where w The birth of new particles will be further discussed in Subsection IV-D.

C. Update
The update step utilizes the measurement Z k to get the posterior states of the objects X k . To propagate the PHD sequentially, the posterior PHD must be formed into the summation of particles, namely the form to substitute k − 1 with k in Equation (17). We express this form repetitively to illustrate the notations during propagation better: Note that the field of view (FOV) of the sensor is usually limited. Z k can only be in S f , defined in Section III. The objects that do not belong to S f are in unknown area and should not be updated, otherwise their existence probability will significantly be falsely reduced. Thus the notations in Equation (22) should contain a superscript f , such as . Let superscriptf represents the definitions in S \ S f . Then by using the property in (10), the PHD of the all objects in S should be estimated as: For a point object x that lies in a pyramid subspace belonging to the FOV space, such as the subspace outlined with black dashes, we define the activation space of x as the union of this pyramid subspace and its adjacent n pyramid subspaces. n = 2 in this case. Subplot (c) shows a cutaway view of the activation space. Point cloud measurement z k , which locates out of the activation space, is considered to update point object x, whose distance to map center is r. where The following truncates and concerns only the particles' update inside of the FOV, and for notation simplification, the superscript f is not added. (13) and (14) with the representation in the last row of (21). The weight of the posterior particles in S f can be expressed as [32]: where L f k is the number of particles in S f . The particle states remain unchanged in the update.
It should be noted that C k (z k ) is only controlled by z k , thus for every w (i) k , C k (z k ) can be shared for the same z k . Thus the calculation operation of C k (z k ) should be performed L f k · M k times, where M k is the number of measurement points in Z k . Two multiplication operations and one PDF calculation are performed each time. In addition, considering the sum in Equation (22) and (24), another L f k · M k times of multiplication, division and PDF calculation operations should be performed. The algorithmic complexity is O(L f k M k ). In an unknown environment, there could be many obstacles and over a million particles can be required to approximate the states of the point objects. Thus L f k · M k can be very large, and the efficiency of the map is not adequate. The following considers using the pyramid subspaces to reduce the complexity.
Considering the measurement noise of the commonly used point cloud sensors, such as depth camera and Lidar, the single object measurement likelihood g k (·) can be assumed as a Gaussian distribution, which is ] · x k since the measurement is only position, as is described in Equation (6).
Unlike the prediction covariance Q, the measurement covariance R is usually not constant but related to the distance of the obstacle in regular sensor models. Firstly, suppose that the measurement error of a point object x k ∈ S f is independent on each axis, and the standard deviation on each axis is equally Suppose the robot is in a sphere model with radius r min . The space inside of the sphere is not considered in the map. Let r max = L 2 x + L 2 y + L 2 z . Then r k ∈ [r min , r max ]. Considering the sensor has a limited FOV with vertical view angle θ v , α k ∈ π−θv 2 , π+θv 2 , θ v < π. The covariance is R(x k ) = ρ 2 (r k )E 3×3 and g k (z k |x k ) can be rewritten as where z k,{x,y,z} and p k,{x,y,z} are the single-axis position of a measurement and an object, respectively, at time k, which are described in (6) and (2). Define an activation space S x k . S x k is the adjacent space of x k , and is composed of the union of the pyramid subspace where x k is and the adjacent n pyramid subspaces in S f . For example, Fig. 3. (b) and (c) show the activation space of x k with n = 2. The number of pyramid subspaces in S x k is (2n + 1) 2 . Let ∆d(z k , x k ) denote the Euclidean position distance between an observation point z k and the point object x k . Consider the 3d Gaussian probability density, g k (z k |x k ) can be further written as: If z k / ∈ S x k , the absolute azimuth angle and the zenith angle difference between x k and z k is not less than nθ. Let θ = nθ. It can be derived with geometric relationships that the lower bound of ∆d(z k , x k ) is f (r k , θ , α k ) = r k sin θ sin α k . Thus for ∀z k / ∈ S x k , the maximum density is When f 2 (r k , θ , α k ) increases, g k,max (z k |x k ) decreases monotonically. Suppose is a small constant. If g k (z k |x k ) < , the approximation g k (z k |x k ) = 0 is taken to improve the update efficiency. Let g k,max (z k |x k ) = . Equation (29) can be reformed as Since r k ∈ [r min , r max ] and α k ∈ π−θv 2 , π+θv 2 are in close intervals, θ must have a maximum value θ max . For example, Case 1: when ρ(r k ) equals a constant value σ, θ max is Case 2: when ρ(r k ) = σ r k , which means the measurement variance grows linearly with r k , then Therefore, given a threshold , θ max can be calculated and the parameter n for activation space is n = θ max θ . Then ∀z k / ∈ S x k , g k (z k |x k ) ≤ ≈ 0. If the measurement variances on each axis are not identical, the upper envelope of the variances can be taken as ρ(r k ), and the above inference still holds. If the measurement errors on each axis are not independent, the former Equations cannot hold but the derived result can be used as a approximation to determine n. In the rest context, we suppose the measurement errors on each axis are independent.
Let Z Sx (i) k k denote measurement point set in the activation space of particlex (i) k , and L S z k k denote the number of particles whose activation space includes z k . Equations (24) and (25) can be expressed as: The complexity of the update step in (33) and (34) is then about (2n+1) 2 N f times to Equations (24) and (25), where N f = θ h θv θ 2 , described in Section III, is the number of pyramid subspaces in

D. Particle Birth
In the PHD filter, the newborn objects in the prediction step are modeled as a PPP with intensity γ k|k−1 (x k ). Theoretically, the newborn objects can appear anywhere in S with arbitrary velocities, and thus a large number of newborn particles with random states should be added to represent the PHD of the newborn objects. However, this added redundancy in both the prediction step and the update step. A better approach to add the particles is to generate them with the observation Z k , which is proposed in [30]. Using this approach, the PHD filter described in Equations (11) to (14) would slightly change. We don't present the changed equations here but directly express the particles' operation changes.
The newborn particles are generated from Z k and thus are supposed to be inside of S f . Given a sampling approach x b,k ∼ q(·, z k ), we can generate particles with a number of β for each measurement point z k . Then there are M k β newborn particles. The position of each newborn particle is sampled from the Gaussian noise model in Eq. (26). Normally, the velocity of the newborn particle is randomly sampled in a feasible velocity range. However, this random sampling leads to heavy noise, and the convergence speed is slow. Thus we sample the velocity of each newborn particle through an initial velocity estimation method, which is described in Section V-A and V-B. The weight of these particles are set to be k|k−1 is a parameter corresponding to the expected number of newborn objects.
In the update step, the weight of the newborn particle is calculated separately [30]. The weight update Equations (33) and (34) are reformed to represent the survived particles and the newborn particles separately:

E. Resampling
The resampling step is to constrain the number of particles and prevent degeneration. After resampling, the cardinality expectation and the posterior PHD of X k should not change. With the posterior PHD D X k (x k ) in the form of (22), the cardinality expectation of X k is estimated by (9) For a single voxel subspace V j , the cardinality expectation E[|X (Vj ) k |], can also be estimated with the weights of the particles inside, which is represents the number of particles in voxel V j at time k, and w (Vj ),(i) k denotes the weight of the i th particle in voxel V j . The effect of particles out of V j is truncated. The same truncation is used in [2].
Then the resampling is conducted by rejection sampling approach in the voxel subspaces rather than the whole map to get relatively uniformly distributed particles. Let L V k,max denote the maximum number of particles in a voxel. Then the number of particles after resampling iŝ The weight of the particles after resampling is identicallŷ

V. EXTENSIONS IN MAPPING
This section proposes some important extension modules. Firstly, the initial velocity estimation module for newborn particles and a mixture model composed of a static model and a constant velocity model are proposed to reduce the noise in mapping. Then the occupancy status estimation and future status prediction modules, which generate the output designed for motion planning, are expressed. Finally, several useful extra extensions are discussed.

A. Initial Velocity Estimation
The particle-based maps model the obstacles as point objects. This model is very friendly with particle-based tracking but works only at the sub-object level, which will cause non-negligible noise when the obstacle has a relatively large volume. Specifically, the noise is caused by the false update of the particles. Fig. 4 (a) illustrates the false update. Multiple noise particles, such as Particle B in the figure, with variant velocities, may occur during the filtering process, which further leads to a very large variance in estimating the velocities of the obstacles, and the convergence is hard to achieve. Therefore, an object-level estimation can be valuable to reduce the noise. We add the object-level estimation by considering initial velocities for the newborn particles, which are randomly generated velocities in other works. The procedures to acquire the initial velocities from two adjacent point clouds is shown in Fig. 4 (b). The point cloud that obviously belongs to static obstacles, like the ground, is segmented by considering the height dimension and assigned with the zero velocity. The rest point cloud is clustered, and the result clusters are matched with the clusters extracted from the last frame. Then the velocity of each cluster can be estimated by differentiating the  Fig. 4. The particle false update problem (a) and the initial velocity estimation procedures (b). In subfigure (a), a dynamic obstacle (a mobile robot) is moving from left to right, from time t = k − 1 to time t = k. Take Point 1 near the left edge of the obstacle and Point 2 near the right edge of the obstacle as two point objects to be measured. At t = k − 1, suppose Particle A and Particle B are generated in the particle birth procedure (Section IV-D) with the point cloud measurement at Point 2. Since the point cloud measurement doesn't provide velocity, Particle A and Particle B are randomly given a velocity to the right and a velocity to the left, respectively. At t = k, the obstacle moves to a new position (with blue outlines), and the positions of Particle A and Particle B move accordingly. We have new measurement at Point 1 and Point 2. Since Particle A is close to Point 2 and Particle B is close to Point 1, the two particles will both be updated to have a high weight. However, Particle B has a velocity towards the opposite direction and obviously belongs to noise. Thus A is rightly updated while B is falsely updated. Note the particle false update problem also exists in modeling large-volume static obstacles and is more frequent when multiple dynamic and static obstacles exist, in which case particles generated from one obstacle may be falsely updated with the measurement from another obstacle. position of the matched clusters' center. We use the Euclidean cluster extraction based on k-d tree [33] for clustering and the Kuhn-Munkras (KM) algorithm for matching. In the matching process, the position of the cluster center and the number of points in the cluster are used as features. If a cluster at t = k−1 cannot be matched, this cluster is regarded as a new obstacle, and no velocity estimation result is assigned.
The velocity estimated by position differentiating between two adjacent inputs is quite noisy because of three main reasons. The first reason is that the position error of the point cloud measurement is amplified and propagated to the velocity estimation by differentiating. The second reason is that the position of a cluster center varies when using point clouds observed from different angles, and the third is that the clustering and matching result might contain many errors in complex environments. We have assumed that the measurement noise of the point cloud is Gaussian noise. Thus the noise caused by the first reason is still Gaussian noise. However, the noise caused by the latter two reasons can be very random. Therefore, the estimated velocity cannot be regarded as the velocity measurement and utilized in the update procedure. We thus adopt this estimated velocity as a reference of initial particle velocities in the particle birth procedure. Details are presented in Section V-B. The comparison result of using newborn particles with randomly generated velocities and using newborn particles considering initial velocity estimation is presented in Section VI-B.

B. Mixture Model
To further reduce the noise caused by the false update and model static objects better, we adopt a mixture model. The first model is the CV model, where the zero velocity situation is excluded, and the second model is a static model. Equation (15) can then be rewritten as: where λ 1 and λ 2 are weight coefficients and λ 1 +λ 2 = 1. Since the environment is unknown, the value of λ 1 and λ 2 should not be fixed but should be updated with the filtering process. Σ is the noise matrix, in which the velocity noise is zero. Under the assumption that the position noise in Σ is also Gaussian noise, this mixture model turns to a Gaussian mixture model (GMM). The GMM is still a sampling-friendly model to be used in the SMC-PHD filter. However, it is hard to derive λ 1 and λ 2 of each object, from the PHD used in the propagation. Therefore, we assume that the objects in one voxel subspace, which is a small subspace, have the same weight coefficients. Then λ 1 and λ 2 can be estimated by the ratio between the cardinality of dynamic objects and the cardinality of static objects in the voxel subspace. In a voxel subspace, the dynamic objects and the static objects can be defined as two independent RFSs, which is A particlex (i) might correspond to a dynamic object x or the static object x s , depending on the transition density π k|k−1 (x k |x (i) k−1 ). For simplicity, the absolute velocity, namely V (x (i) ) is used as the feature to determine the correspondence, and the Dempster Shafer theory (DST) is adopted to approximate |X Suppose the weight summation of particles that satisfy , and the weight summation of particles that satisfy V ( d , whereV is a threshold suggesting that particles with a velocity larger thanV correspond to x rather than x s . The particles with 0 < V (x (i) ) <V , however, can correspond to x or x s . Suppose the weight summation of these particles is W which describes the basic belief . Then the belief and the plausibility are According to DST, the probability is between the belief and the plausibility. We simply take middle value as the probability estimation, which is pr(·) = bel(·)+pl(·)

2
. The cardinalities of dynamic objects and static objects in a voxel are approximated by |X Then the coefficients λ 1 and λ 2 in Equation (42) can be estimated with the ratio of the cardinalities: In the particle birth procedure, the velocities of the newborn particles are assigned based on λ 1 and λ 2 in the corresponding voxel subspace, and the initial velocity estimation results in Section V-A. If a measurement point is labeled with the ground in the initial velocity estimation procedure, the velocities of the particles generated from this point are all zero. Otherwise, the mixture model is used. The number of dynamic particles generated from a measurement point is λ 1 β, and the number of static particles is λ 2 β, where β is the total particle number for each measurement point. According to the discussion of estimation noise in Section V-A, the velocities of dynamic particles are further composed of two parts: velocities sampled from a Gaussian distribution, whose center is the estimated velocity, and variance is a fixed parameter, and random velocities. Since there is a lot of noise in real-world tests, we set a large variance parameter for the Gaussian distribution, and the particles with random velocities take half of λ 1 β. If a measurement point is not estimated with an initial velocity, the random velocities will be assigned to all dynamic particles. If too few particles exist in the voxel subspace where the measurement point belongs to, e.g., the voxel subspace is observed for the first time, an initial guess of λ 1 = λ 2 = 0.5 is used.

C. Occupancy Status Estimation
With the posterior PHD D X k (x k ) in the form of (22), the number of the point objects in S, namely the cardinality expectation of X, can be estimated by Equation (38). For an arbitrary point p in the map, the occupancy status can be estimated by the cardinality expectation of point objects in a small neighborhood space. The size of the neighborhood can be determined by the density or resolution of the input point cloud. In our case, the point cloud is pre-filtered by a voxel filter with resolution Res, which means there should be either one or no point observed in a voxel with side length l = Res. If the point cloud is not pre-filtered, Res is usually a function of the distance r from the point to the sensor and a changing length l = Res(r) can be used. Then the neighborhood space can be chosen as the voxel subspace S (Vj ) centered by p with side length l = Res. The occupancy probability of p can be estimated with the cardinality expectation of the point objects in this voxel subspace. In a more general situation, l can be determined by the requirements from the path planner or the  Fig. 8, and the map is built with the recorded flight data used in Section VI-C. Some parts of the tree and the ground are missing because they haven't been observed. mission requirements. Then the occupancy probability of p can be calculated by: is the RFS composed of point objects in S (Vj ) at time k, and E[|X (Vj ) k |] is given by Equation (39). Occupancy probability calculated by this equation can be larger than one in real applications because the noise in the motion model and the noise from measurement may lead to non-uniformly distributed point objects. If pr() > 1, pr() = 1 is used. In addition, a threshold can be set for pr() to determine a binary status, namely occupied or free.
The occupancy status of S (Vj ) is also represented with pr(). Considering the predefined voxel subspaces in Section III, a 3D grid map, which is used in most motion planning algorithms currently, can be generated. It should be noted that this 3D grid map doesn't suffer from the grid size problem because the update is fulfilled in the continuous space. Fig. 5 shows an example occupancy estimation result in a scenario with a static obstacle and a dynamic obstacle.

D. Future Occupancy Status Prediction
Predicting the future occupancy status is very useful for motion planning in dynamic environments. In our DSP map, the future occupancy status prediction is fulfilled by predicting the position of the particles according to the motion model in (42), and then using (47) for occupancy status estimation. Fig. 6 presents the future occupancy estimation results of the map shown in Fig. 5. The occupancy status of the static obstacle, the tree, almost stays the same in each plot. The occupied position of the dynamic obstacle, the pedestrian, is predicted to move down with the CV model. The occupied grids are spreading, and their occupancy probabilities are getting lower as the prediction time increases. The reason is the uncertainty in velocity estimation, which is reflected by the variance of particles' velocities. The noise in other parts of the plots is also caused by the estimation uncertainty. However, since the occupancy possibility of the noise is not high, they can be removed by filters. Since the future occupancy status prediction in dynamic environments has inevitable uncertainty, the predicted occupancy probability can be used as the risk in motion planning algorithms.

E. Extra Extensions
Several useful extensions of the DSP map are discussed in this subsection to serve the motion planning better.
1) DSP Static Map: By using only the static model described in Section V-B, the DSP map turns to a static map, named with DSP-Static map. In this case, the number of particles used in this map can be very small since the velocity dimension is not considered, which means the DSP-Static map is lighter. Compared to the voxel map for static environments, the DSP-Static map is continuous and free from the voxel size problem. Small obstacles can also be represented, which will be discussed in the following context in this section. In the experiment section, the DSP-Static map is also tested.
2) Unknown Area: Representing the unknown area can be useful in exploration tasks. For static grid maps, the grids are initialized with a tag "unknown", and the tag is removed when a ray generated from point cloud measurement passes through or hits the grid. In the DSP map, the unknown area can be represented by the update time of the particles. Adding time stamps on the common particles doesn't work because the particles are born only in the area with obstacles, and thus the unknown area and the free area cannot be distinguished. Therefore, in the map initialization procedure or when a new area appears because of the movement of the robot, a small number of static particles, named time particles, which have a zero weight and a time stamp, can be uniformly added to the map. When the measurement point cloud comes, the time particles only update their timestamp to the current time. Then the unknown property of each area can be evaluated by checking the time stamp.
3) Data Structure: The unordered set should naturally be an excellent structure to describe the RFS. However, over a million particles can exist in the map at most, so the efficiency is heavily affected by the memory access speed mainly determined by the cache hit rate. Using an unordered set to store so many particles will lead to a low cache hit rate because of the discontinuous access in memory storage. Therefore, we use a two-dimension array with fixed and enough large size to store the particles in voxel subspaces. Nearby particles, in one voxel or adjacent voxels, can be hit by the cache and accessed quickly. In addition, iteration of all particles is time-consuming. Thus the occupancy status estimation is conducted in the same step with resampling. The future status prediction is conducted in the same step as the prediction in the PHD filter. The initial velocity estimation is only used in the particle birth procedure, so it runs in parallel with the prediction and update procedure in the filter. In an environment with dynamic obstacles that move in a 2d plane, such as pedestrians, the velocity dimension of dynamic particles is reduced to two, and thus fewer particles can be used and the efficiency can be improved.

VI. EXPERIMENTAL RESULTS
The performance of the DSP map is evaluated in three aspects, which are efficiency, velocity estimation accuracy, and mapping performance comparison. Firstly, the efficiency tests are conducted on different platforms to determine if our particle-based map, which usually requires a large number of particles, can be deployed in real-time applications of robotic systems. Then the velocity estimation accuracy test is performed to evaluate the ability to model the velocity of dynamic obstacles from point cloud inputs. Furthermore, the mapping performance comparison tests are carried out to prove the advantage of our map in estimating occupancy status in the environment with both static and dynamic obstacles. Finally, a demo of using this map is presented.

A. Efficiency Tests
Our map was tested on four different kinds of devices with different CPUs. Table I presents the hardware specifications of the devices. Fig. 7 shows the test results with different maximum particle numbers. The tests were conducted with the dataset recorded in the simulation shown in Fig. 7. It can be seen that even though the maximum CPU frequency of Xavier is much lower than that of NUC, the overall efficiency is higher. The reason is that the L2 cache capacity of Xavier is much larger, and thus the cache hit rate is much higher. With a laptop with a powerful CPU, the map can run at a frequency of 10 Hz even if the maximum particle number is up to two million. When the maximum particle number is reduced to 3.3 × 10 5 , the frequency is over 50 Hz with the laptop and 10 Hz with the Up core cardboard. In comparison, current particle-based maps rely on desktop GPUs and cannot be deployed to small computing boards like Up core or NUC.
In the following experiments, we use the laptop in velocity estimation and mapping performance comparison, and the maximum particle number is 1.8 million. In the application demo with a MAV, the map is running onboard with an Up core cardboard, which is lightweight and is suitable for MAVs. The maximum particle number is about 0.4 million.

B. Velocity Estimation
The velocity estimation experiments were conducted with the data collected in an indoor testing field with the Nokov motion capture system. An Intel Realsense d435 camera was fixed at an edge of the testing field to collect the point cloud. Two pedestrians, wearing helmets with markers, walked around in the testing field, and their trajectories estimated by the motion capture system were recorded synchronously with the point cloud. The experiments can be divided into two groups. In the first group, the pedestrians tried to walk with a constant velocity. In the second group, the pedestrians walked randomly and freely. Fig. 8 (a) shows a snapshot of the data collection scenario.
We compared the velocities estimated by four different point-cloud-based methods. The first method differentiates the center position of two matched clusters, and no filter is adopted. The matching is achieved by the KM algorithm. The second is a multi-object tracker realized by the KM algorithm and Kalman Filters (KF) with a CV model. The input of the KF are the center positions of matched clusters. The third method is the DSP map with the suffix "Random", whose newborn particles have random velocities. The fourth method is the DSP map with the suffix "Dynamic", whose newborn particles consider initial velocity estimation. Since our maps don't explicitly segment the objects, the state of a pedestrian was estimated with the particle cluster near the pedestrian's real position. Table II presents the estimation results of the two groups. Consider the process in which a pedestrian walks  from one side of the testing field to another as a tracklet. Over thirty tracklets were collected in each group. Fig. 9 shows the velocity estimation curves of a typical tracklet. Three metrics are used for evaluation. The root mean square error (RMSE) reflects the estimation accuracy evaluated with the mean of the velocity estimation distribution and the ground truth from the motion capture system. The Var. is the mean variance of the different axes on every point. The differentiating method outputs a single value rather than a distribution, and thus a dash is placed in its Var. in Table II. For a particle-based map, large variance means the particles would disperse to a large scale of the area and cause a lot of noise in the map. Mean Bhattacharyya distance (MBD) is to measure the similarity between the estimated and ground-truth velocity distribution. MBD takes both mean value and variance into consideration and is a composite metric. The results in Table II show that DSP-Dynamic has the best performance with all three metrics. The differentiated velocity has a large RMSE, and the error can be huge sometimes, as Fig. 9shows. Using KF can reduce the error, but the Var. is over 30% larger  than that of DSP-Dynamic and the MBD is over 12% larger. Compared to DSP-random, DSP dynamic decreases over 14% on RMSE, over 68% on Var., and over 34% on MBD, showing the importance of the initial velocity estimation.

C. Mapping Performance Comparison
The mapping performance was evaluated with accuracy, recall, and F-1 score in three different simulated worlds shown in Fig. 8 (b), (c), and (d). The pedestrian street world contains both static obstacles and dynamic obstacles. The pedestrian square contains only dynamic obstacles, while the forest world contains only static obstacles. The worlds were built in Gazebo simulation software, and a simulated IRIS drone with a Realsense camera was controlled manually to collect point cloud data. The ground truth occupancy status is calculated with the 3D mesh files of the obstacles. We compared our DSP-dynamic map with a widely used static local occupancy map named Ewok [7], and a state-of-the-art particle-based dynamic occupancy map named K3DOM [5]. K3DOM is the only 3D dynamic occupancy map that has a released code currently. We also compared the DSP-Dynamic map with the DSP-Random map, which uses newborn particles with random velocities, and the DSP-Static map, which uses static newborn particles to represent only static obstacles. K3DOM runs on NVIDIA RTX 2060, and the rest maps run on AMD Ryzen 4800HS. During the evaluation, the occupancy status of the DSP maps is voxelized with the voxel subspaces to have the same comparison form as Ewok and K3DOM. The selected parameters are (L x , L y , L z ) = (9.9, 9.9, 6.3), θ = 3 • and the initial weight for the particles is 0.0001. Three different voxel resolutions, from 0.1 m to 0.3 m, were tested. The quantitative results can be found in Table III. The recall of all maps is small because some obstacles are in the unknown space. Snapshots of different maps with resolution 0.1 m and 0.3 m in different worlds can be found in Fig. 10 and Fig. 11 .
In all three worlds and all three resolutions, the DSP-Dynamic map has the highest F-1 score compared to other maps, which suggests the DSP-Dynamic map has the best  performance considering both accuracy and recall. In the pedestrian square world with only dynamic obstacles, the DSP-Random map outstands on accuracy because the state variance of particles is very large, and thus only a small number of voxels, which are usually accurate, are regarded as occupied voxels. However, this large variance makes the recall small, and many noises appear in the area out of the FOV when static obstacles exist (Row (1) and (3), Column (e) in Fig. 1). Compared with DSP-Random, DSP-Dynamic have a substantially larger recall and fewer noises due to the initial velocity estimation and the mixture model. The accuracy and recall of K3DOM and Ewok are much lower, especially when the resolution is high. In the forest world with only static obstacles, Ewok has the best accuracy overall, and is followed by K3DOM and DSP-Static. The recall of DSP-Static is the largest among these three because it has a fast response speed by using PHD filter rather than raycasting. In comparison, the accuracy of DSP-Dynamic is smaller, but the recall is even larger than DSP-Static because the moving particles can further accelerate response. Thus the F-1 score of DSP-Dynamic is still the highest. In the pedestrian street world with both static and dynamic obstacles, DSP-Dynamic outstands on almost all metrics. The accuracy when resolution is 0.1 m is also comparable with Ewok.
Red ellipses in Fig. 1 show the trail noise caused by the motion of dynamic obstacles. The trail noise is very obvious in static maps like Ewok and DSP-Static. As a dynamic map, K3DOM still has trail noise while our DSP-Dynamic has very little. Red rectangles in the figure outline some areas with predicted pedestrian positions out of the FOV. Only DSP-Random and DSP-Dynamic have correct predictions near the real pedestrian position, and the prediction predicted by DSP-dynamic is better, especially when the resolution is high. Compared to K3DOM, the F-1 score of DSP-Dynamic increases at least 105%, 30%, and 218% in the three worlds, respectively, with different resolutions. . Fig. 12 presents several snapshots of building the DSP-Dynamic map in different scenarios. The localization was realized by Realsense T265 tracking camera and the point cloud was from a Realsense d435 camera. To further demonstrate the effectiveness and efficiency of our map in robotic systems. We deployed the DSP-Dynamic map on a mini quadrotor, named Mantis 1 , with a weight of only 320 grams, and utilized a sampling-based motion planning method [34] to realize obstacle avoidance in the environments with static and dynamic obstacles. The point cloud was collected from a Realsense d435 camera, and everything, including mapping and motion planning, was performed on the CPU of a low-cost Up core computing board. Fig. 13 shows the hardware of the quadrotor and the testing scenarios. The testing demos can be found at https://youtu.be/seF Oy4YbXo. During the tests, the quadrotor flies at a maximum speed of 3m/s. 1 Mantis from BY Intelligence: https://byintelligence.com/ VII. CONCLUSION This paper presents a novel dual-structure particle-based 3D local map, named DSP (dynamic) map, that allows continuous occupancy mapping of dynamic environments. Voxel subspaces and pyramid-like subspaces are adopted to achieve efficient updates in continuous space. The initial velocity estimation and a mixture model are considered to reduce noise. Experiments show that the DSP map can increase the dynamic obstacle velocity estimation performance by over 30% on MBD, compared to other point-cloud-based methods. In occupancy status estimation tests, the DSP map also outstands and increases the F-1 score by at least 30% to 218%, with different resolutions, compared to the state-of-the-art particlebased occupancy map. Furthermore, efficiency tests and a realworld application demo demonstrated the broad prospect of this map in obstacle avoidance tasks of small-scale robotic systems. Future works will consider two main points. The first one is to introduce semantic information to this map to better identify and model different obstacles and further predict their future states with multiple hypothesizes. The second one is to connect this dynamic local map to a global static map to achieve global mapping in dynamic environments.