Generative Cloud of Points SLAM (G-SLAM)

: Mobile robots need an environmental perception ability in order to interact with the 1 surrounding environment. In this paper we present the G-SLAM method, where the map is consisted 2 of a cloud of scattered points in the continuous space and each point is accompanied by an obstacle 3 existence probability. On the other hand the robot’s pose (and trajectory) is estimated by an particle 4 ﬁlters while the cloud of points is estimated by an adaptive recursive algorithm which is presented. 5 Main feature of the presented method is the adaptive repositioning of the scattered points and their 6 convergence around obstacles. In addition to our previous publications, In this paper a mathematical 7 derivation of the method is presented and real case scenarios are discussed, presented and compared 8 with other methods.


Introduction
In this paper it is presented the G-SLAM [1] method, a Simultaneous Localization and Mapping (SLAM) method, where the map is cloud of points in the continuous space followed by a probability which describes the existence of an obstacle in the subsequent point in space.In addition to our previous publication [1], in this paper it is presented the mathematical formulation and derivation of the problem and the experiments and the results are enhanced with more SLAM methods.
Here we use a probabilistic approach which is based on particle filters.Particle filters are used for the robot's pose estimation.Usually an occupancy grid map with high resolution grid defined over the space but in this paper a cloud of scattered points in the continuous space is used instead.The map update procedure uses a recursive algorithm and produces the map's probability distribution .G-SLAM method generates new hypothetical points of features in space which are subsequently tested whether they correspond to real obstacles or not.
The G-SLAM's main advantage is the adaptive repositioning of the scattered map's points that "drives" them in a convergence around the obstacles.The final map resulted from the G-SLAM method exhibits high density of weighted points around the obstacle and a subsequent high sparsity in the free space.The resulted weighted points represent the probability distribution of the obstacles in the continuous space.This method fits best on problems where detailed maps are necessary but with low computational resources.
In section 2 we discuss the probabilistic analysis of the combined SLAM problem in terms of recursively computed probability distributions which estimates the probabilistic map and the robot's trajectory along with the G-SLAM method are presented.In section 3.1 it is shown the model of the robot used which consist of the robot's kinematic model and the laser sensor's measurement model is described.Section 3.3 exhibits the experimental results from real case scenario in a variety of methods including G-SLAM.

Notations
• s t : is a time series of the robot's pose, while s t is only the pose at a time instance.
• Θ: represents the map and is a set of points θ k in space.
• z t : is a time series of the measurements, while z t represents only the measure at time t.
• u t is the time series of the robot's control inputs, while u t refers only at time t control input.
• d z (.): represents the probability density function of the sensor's measure noise.
• d f (.): represents the probability density function of the transition's model noise.

SLAM Posterior
While most SLAM methods are trying to estimate the robot's pose s t and the map Θ at timestamp t, in this paper our goal is to estimate the whole time series s t and the map Θ using the observation time series z t and the control time series u t .In probabilistic terms this posterior is expressed as: Using the definition of the conditional probability, the posterior in equation 1 can be expressed as: The two factors of the equation 2 correspond to the robot's trajectory posterior and the map's posterior respectively.The calculation of these two factors is discussed bellow.

Pose prediction
The left posterior of the equation 2 refers to the estimation of the pose time series given the map and the time series of the observation and the controls.
The calculation of this posterior is done using the technique of particle filtering.The proposal distribution for the particles will be the posterior p(s t |z t−1 , u t ), thus the drawing process for each particle i evolves only the previous state s i t−1 and the current control input u t .
The proposal distribution is generated from the posterior Prob(s t |s t−1 , u t ) using the robot's kinematic model f and of course a random sample of the control's input noise i t .
This procedure creates a cloud of N particles, all representing a possible pose of the robot.It is noteworthy that each particle carries out its own estimation of the map which is independent from the other particles' maps.The estimation of the pose timeseries is not done yet since particle filter's importance factors have to be calculated.The calculation of the importance factors is discussed below in the section 2.5.

Map Update
The rightmost factor of the equation 2 refers to the estimation of the map given the time series of the observation and the controls.The map consists of a set of scattered points in space θ and each one is associated with a probability that the point θ is an obstacle.The distribution of this probabilistic map can be represented by the following posterior.
The equation 5 gives the probability that the feature θ k is an obstacle given the observations z t and the controls u t .By the definition of the conditional probability, the posterior 5 is expressed as: Using the law of total probability for all θ j the denominator becomes The posteriors of the numerator and the denominator have the same form In the rightmost posterior we note that θ k is independent of the control input u t and the pose s t due to the absence of the measurement z t .Thus the above expression becomes Equations ( 7) and ( 8) imply the recursion: Since z t is independent from previous observations, control inputs and previous robot's positions the equation ( 9) is simplified as: In order to compute the recursion (10) we need to calculate the quantity q k t = Prob(z t |s t , θ k ).In case that the probability distribution function of the measurement noise is given by function d z (z), then this quantity can be calculated by: Combining equations ( 10) and ( 11) the probability of every point k is calculated using equation (12).

Importance factor calculation
The distribution as proposed in equation ( 3) is only the proposal distribution.Using the simulation technique of particle filtering the target distribution is calculated as: Through the target distribution the best estimation for the robot's pose s t is calculated.
Using the Bayes Theorem the equation ( 13) is simplified as: So the importance factor is proportional to the posterior p(z t |s t,i , u t , z t−1 ) which is already calculated in the map update section 2.4 as the denominator of equations ( 6) or (10).
Since the set of particles S t = {s t 1 , ..., s t M } is finite, the "cloud" of particles is growing as the time increases, which can lead to the degeneracy of the algorithm.Thus a resampling technique is necessary.
In this paper and on the experiments that took place, the technique of Residual Systematic Resampling (RSR) is used [14].

The G-SLAM Method
The proposed method, G-SLAM is based on a technique that generates stochastically new scattered points that are added into the map.This stochastic generation is based on the current particle and the current observation.Then the update procedure updates the map by updating each point's probability, while afterwards the "meaningless" features are removed from the map set.The sensor's noise is responsible for the stochastic nature of this procedure.This addition of scattered point into the map set is achieved using a drawing procedure which is described bellow.Afterwards the extended map is updated and the updated points with low probabilities are removed.The small probability in a map point, states that this point in space is unlikely to be an obstacle.Algorithms of this type converges as are discussed in [15].In the context of this paper, map update procedure converges to high probabilities in map points near obstacles.Removing all low probability map points, the parts of space which are free of obstacles are also free of map points while on the other hand the parts of space with obstacles gathers all the scattered points around them.
The G-SLAM method can be described abstractly in six steps: 1. Draw pose s i t for every particle i using the subsequent pose s i t−1 and the control u t The existing map can be easily updated using equation (12).For every map point θ i it is calculated the probability of the measure z t to correspond to this point in map using the equation ( 11) and then it is multiplied to the previous map's point probability p i t−1 .It is noteworthy that the denominator of the equation ( 12) is just a normalization factor.

Adding new map points
The stochastic addition of new points into the map is achieved based on the observation z t and the current pose of each particle.For every observation z t a drawing procedure takes place in order to generate a set of M new map points that represents the sensor's measurement probability distribution.
where R t is the covariance matrix of the sensor's noise.
Every element ẑm t is given a probability: where d z (.) represents the probability density function of the measurement's noise.
These elements ẑm t are unlikely to correspond to a map point θ ∈ Θ.Thus in order to update the map it is necessary to create new map points θm = g(s t , ẑm t ) in the map Θ.But also, in order to proceed with the update, it is necessary to calculate each point's probability pm t−1 .In the G-SLAM method the calculation of the probability pm t−1 is done numerically using interpolation methods.The pre-updated map contains points and their subsequent probabilities at time t − 1.These points in space are interpolated with θm in order to estimate the subsequent pm t−1 probability.Afterwards and using the equation ( 12) the new map points are updated and added to the map set Θ.This procedure augments the probabilistic map with M new points and their probabilities.

Removing meaningless map points
As already discussed, update procedure returns an augmented map with more map points than previously had.Some of those points might have near zero probability meaning that the probability of an obstacle existence in this point in space is highly unlikely.
In G-SLAM a map point removal procedure takes place after map update procedure in order to remove all the meaningless map points.So all points θ i which their probabilities p i t are less than a predefined thresshold p i t < p thr are removed from the map set.Using this technique it is prevented the overpopulation of the set Θ and the features θ tends to gather near obstacles.

Agorithm
A pseudo code of the G-SLAM algorithm is given below.

Experiments & Results
To confirm our method's integrity we use the dataset performed by Nieto, Nebot et al. from the University of Sydney [16,17].The experiments were performed with a car performing a full loop (fig. 2).A four-wheel rear-drive car was used, which was equipped with a horizontal scanning laser sensor with 180 degrees field of view and 80 meters of maximum observing radius.The control of the car consists of the linear velocity of the rear left wheel and the heading angle of the front wheels.GPS measurements comes with the dataset in order to validate the car's position.

System description
This dataset is a two dimensional planar dataset and the map is considered as a set of map point Θ = [θ 1 , θ 2 , ...., θ N ] each one corresponding to a point in space (fig.1).The probability that a point θ k is an obstacle is denoted by p k .The set of features along with their probabilities is a probabilistic map of the space.The robot's path is represented by a time-series of it's pose s t = [s 1 , s 2 , ...s t ].In a planar problem each feature θ k is a vector with entries (x, y) coordinates of the point.Robot's pose s t is also a vector with entries (x, y, ϕ) at time t where ϕ represents the angle of the robot's orientation corresponding to a global axis system.
The measurement timeseries z t = [z 1 , z 2 , ...z t ] consist of distance bearing measures (d, ϑ) acquired from the laser sensor and corresponds to the sensor's coordinate system.The distance-bearing laser sensor's feedback consists of a 361 distance measurements with half a degree angular distance between them.

Model
The car used for this experiments was a rear drive car-like four-wheel.The kinematic model of a vehicle like this is described by equation (19).
where v c is the robot's linear velocity at time t, ω is the steering angle at time t, L is the distance between the car's axles and a, b are the coordinates of the laser sensor according to the car's coordinate system.The velocity v c is a function of the rear wheel's linear velocity and depends on the steering angle.
where H is the distance between the center point of the rear axle and the rear wheel.
The measurement model of a distance-bearing sensor is given by the nonlinear equations: It is assumed that the distance-bearing sensor's measurements and the control measurements are noisy with noise functions of a known probability distributions.

Results
The vehicle's kinematic model is described by equation 19 with parameters: L = 2.75, H = 0.74, a = L + 0.5 and b = 0.5.
As it is presented in the figures the algorithm results a probabilistic map that consists of a cloud of points and their probabilities.In figure 2 it is shown a contour graph with the map's probability distribution in contrast with the Grid Occupancy SLAM with almost the same number of map points.
The yellow line represents the best estimation for the car's path.G-SLAM method resulted 3280 map points all of them gathered around obstacles, while Grid occupancy SLAM with 3000 cells resulted a lower resolution map since most of the cells covers a free area.In figure 3 we see a detailed view of the G-SLAM map probability distribution in comparison to the FastSLAM 2.0 map with 8 particles and the Grid SLAM with 3000 cells and a high resolution Grid SLAM with 48000 cells.It is noteworthy that the G-SLAM's map exhibits more detailed characteristics than the FastSLAM's and the low resolution Grid SLAM even if the Grid SLAM's cells are almost the same in number with the G-SLAM's map points.In order to achieve the same resolution with Grid SLAM we need to use around 15 times more dense grid with almost 48000 cells as it is shown in the fourth image on figures 3 and 4. Table 1 shows that the G-SLAM method is slower and inaccurate with small amount of feature particles than FastSLAM, but on the other hand G-SLAM is more accurate and faster when is used with higher number of features M. Also the area which is free of obstacle (blue area) is also free of features θ and all of the features are gathered around the obstacles.The red area represents the highest possibility of the existence of obstacles.
More experiments with a variety in the number of particles N were performed.Also the method tested in different number of additional map points M. Table 2 presents the resulted mean distance error of the car and the mean process time using 2, 8 and 30 particles in the pose estimation procedure and 4, 10 and 20 additionally generated map points for every observation.
Table 2 shows that the G-SLAM algorithm results in a relatively high mean distance error when runs with 2 particles, due to its incapability to be consistent with few particles.In this case the map and the car's path acquire a cumulative error high enough to lead the algorithm into inconsistency.On the other hand the algorithm seems to converge relative fast with respect to the number of particles, since with 8 particle results in the minimum error.

Conclusions
In this paper it is presented the G-SLAM method and it's mathematical derivation.The G-SLAM usesn the simulation technique on both the kinematic and measurement models in order to combine probabilities resulted from recursive forms.It results a detailed probability distribution of the map especially arount the objects while it estimates of the robot's trajectory.
Future work will be the examine the G-SLAM method compatibility in to dynamic environments.
We intent to use the proposed method to a robotic platform and we will investigate the accuracy of the results and the consistency in dynamic environments.

)Figure 1 .Figure 2 .
Figure 1.The robot coordinate system and the position of θ i landmark with respect to the robot coordinate system O R .θ is the angle between the vectors X R and O R θ

PreprintsFigure 3 .
Figure 3.A detailed contour graph of the map's probability distribution in contrast with the FastSLAM 2.0 and Grid based SLAM with low and high resolution.Colors blue to red corresponds to low to high probability.The yellow line represents the car's path estimation.a. G-SLAM with 3280 map points, b.FastSLAM 2.0, c.Grid SLAM with 3000 cells, d.High resolution Grid SLAM with 48000 cells.

Figure 4 .
Figure 4. Surface of the map's probability distribution as resulted from the G-SLAM method

preprints.org) | NOT PEER-REVIEWED | Posted: 1 February 2019 Preprints (www.preprints.org) | NOT PEER-REVIEWED | Posted: 1 February 2019 doi:10.20944/preprints201902.0003.v1
2. Generate and add new map points θ into the particle's i map set Θ i using drawing process based on the observation z t and pose s i 5. Calculate Importance factors for every particle 6. Resample particles if necessary Steps 1,3,5,6 are already discussed in sections 2.3,2.4 and 2.5, while generation and removal of map points are discussed bellow.Preprints (www.

Table 2 .
Comparison results on the Car Park dataset with different number of particles and different number of per step additional points M.