I Introduction
Constructing a geometric representation of the environment is a fundamental problem in robotics because it is essential for localization, reliable navigation, as well as occlusion and collision checking in tasks such as object recognition and manipulation. Of course, geometric mapping has been studied extensively and there exist many accurate and efficient methods [1, 2, 3, 4, 5, 6, 7]. While accuracy and efficiency are important requirements, in this paper we focus on an additional requirement that many existing methods do not address. In particular, we focus on maintaining information about covariance of different elements in the map representation. For example, occupancy grid mapping [1, 8, Ch.9]
, one of the early, very successful approaches to geometric mapping, represents the environment as a regular grid in which each cell is a Bernoulli random variable that captures the probability of the cell being occupied or free. Occupancy grid mapping relies on the assumption that the occupancy states of different grid cells are statistically independent and sensor data is correlated only with directly observed cells. We claim that maintaining a model of occupancy dependence among the cells may improve the accuracy of the resulting maps. However, our main motivation for considering such a model with covariance information is that the correlation between known and unknown areas in the map enables planning informative robot trajectories for autonomous mapping
[9, 10, 11]. More precisely, covariance statistics can be used to quantify the current map uncertainty (e.g., the via Shannon’s entropy [12, 13, 14]) and to infer the probable occupancy of unobserved locations based on the occupancy of already observed cells.The above has been recognized by several recent works [15, 16, 17, 18, 19, 20, 21, 22, 23] that propose the use of Gaussian Processes (GPs) [24] for occupancy mapping. In addition to capturing covariance information via a kernel function, these models provide resolutionfree mapping in the form of a continuous occupancy function. The challenge for GP methods is to deal efficiently with the hybrid nature of the problem – discrete measurements (occupied or free) and continuous occupancy function. O’Callaghan et al. [15, 16, 17] proposed a probabilistic leastsquares classification method in which a probit^{1}^{1}1The probit functions is defined as the culmulative distribution function of a standard normal, i.e. , where is the standard normal pdf. function is used to convert a latent function estimated via GP regression to binary occupancy predictions. A more accurate but computationally demanding mapping method is to use GP classification [24, Ch.3.3], in which the latent function posterior is updated from the discrete occupancy measurements. Since the measurement likelihood in this case is not Gaussian, the posterior distribution cannot be analytically determined and has to be approximated with iterative methods. The major drawback of GP regression and classification is that their computational complexity is cubic in the number of measurements. In an online mapping setting where the number of measurements grows with time, applicability of these methods is limited.
Many techniques [25, 26, 27] focus on improving the computational complexity of GPs in general, relying on sparse kernels and matrix factorization to enable efficient inversion of the covariance matrix (resulting from evaluating the kernel at the training examples). Specific to GP mapping, Kim and Kim [20, 21, 22] proposed the use of a sparse kernel and Bayesian Committee Machines to perform small GP regressions with subsets of training data before fusing them into the full map. Similarly, Wang and Englot [23] partition their sensor data amongst several GP regressions and then use Baysian Committee Machines to first combine the sensorlevel regressions and then again to fuse the resulting regression with the map. The speed of this method is further improved with a new datastructure, the testdata octree, which improves access time and only considers correlations between grid cells within a specified range. Despite its advantages, this method does not capture the discrete nature of the measurements in its model and ignores some cross correlations in the grid.
Our key insight, compared to the existing work which uses GPs for metric mapping, is that the values of the latent function at a discrete set of points can be recovered exactly by a Kalman filter
[28]. More precisely, the state of the Kalman filter can capture the continuous values of the latent occupancy function evaluated at each cell of a discrete map, while a probit observation model can convert these latent values into boolean measurements indicating the occupancy of the corresponding cell. The significance of this observation is that a Kalman filter can avoid the cubic scaling with the number of data points (observations of free/occupied space), while providing essentially the same estimates of the mean and covariance of the map. Our second idea is that, although the use of a probit measurement model makes the posterior distribution of the filter nonGaussian, the posterior can be approximated via a Gaussian based on a single forward pass through the measurement data. We prove that this method results in an efficient approximation of GP classification [24, Ch.3.6]. We name the proposed approach an Occupancy Grid Filter (OGF) to emphasize that these two insights were inspired by filtering techniques. Our work makes the following contributions:
[nosep]

We prove that the OGF is an accurate approximation of GP classification on a grid and show that in practice the classification error is negligible.

The computational complexity of the OGF is constant in time while GP methods scale cubically with time as the number of measurements grows. This property makes the proposed occupancy grid filter suitable for online applications.
Ii Problem Formulation
Consider a robot whose task is to build a geometric map of a static environment. Let be a bounded connected set representing the environment and let be a function such that , if is unoccupied, and , otherwise. We restrict attention to occupancy grid mapping [8, Ch.9]. More precisely, we consider a tessellation of with an associated lattice and estimate the map occupancy only at the locations . Suppose that the robot collects a sequence of occupancy measurements , e.g., via a laser scanner or a depth camera, along a known trajectory . Each occupancy measurement is a matrix containing the 3D positions of the cells observed by the sensor and their occupancy , i.e., , , , and is the sensor’s field of view.
Problem.
Given occupancy measurements , construct an estimate of the true environment occupancy .
Note that one more state, , is added in order to allow the case that the occupancy of a cell is unknown.
Iii Gaussian Process Classification
In this section, we give a brief overview of GPs and how they are applied to the occupancy mapping problem. As defined in [24]
, a GP is a collection of random variables, any finite number of which have a joint Gaussian distribution, represented as, incorporate
where is the mean of , and is a kernel function used to construct the covariance matrix.
In a binary classification problem, cannot directly model the class labels because of their discontinuous nature. Instead, is treated as a latent function with GP prior, which is then squashed through the probit function to model the likelihood of a measurement, i.e. or . The following two steps are required in a typical application of binary GP classification to mapping [16].
Iiia Training
In the training step, the posterior
(1) 
is used to estimate the latent values, , of all the captured samples. Note the slight abuse of notation, in (1) should be more accurately represented as instead of , i.e. the training samples can be taken at any points . In (1), is the prior of the latent values of all training samples, where is a covariance matrix constructed using a predefined kernel, is a normalization factor independent of , and is the joint likelihood function which can be represented as,
(2) 
because the measurements are independent given the latent values. The posterior pdf in (1) cannot be obtained in closed form since
is not of normal distribution. There exist different methods to approximate the posterior. The Laplace approximation
[24, Ch.3] finds the mode of and the local negative inverse Hessian to construct a normal distribution that approximates . Expectation Propagation (EP) [24, Ch.3] replaces each term in (2) with a (unnormalized) normal distribution, and tries to approximate the true posterior with product of normals in an iterative manner. More details on EP can be found in Sec. V.IiiB Prediction
With the estimated latent values of the training samples, , the latent values, , of a query position in can be estimated as,
where represents the correlation between and , which is constructed using the kernel function with query positions and the positions of the training samples. By squashing through the probit function, we are able to recover the occupancy of the grid,
(3) 
where and are the thresholds for determining occupied and free cells respectively.
Iv Occupancy Grid Filter
In this section, we propose a new solution to the mapping problem which we call the Occupancy Grid Filter. This method takes into account the spatial correlation and discontinuous nature of occupancy and, at the same time, has lower computational cost than GP classification methods. The OGF arises from two key ideas. First, instead of creating a continuous map which is then evaluated at query points, we preselect the query points and only maintain latent function values at those points. Second, we note that the training step of GP classification described in Sec. III is a batch optimization which must be reevaluated when new measurements become available. We replace this with an incremental, Kalmanfilterlike approach which greatly reduces the computational complexity.
By defining a grid and abstracting each cell with its center position , we only need keep track of the latent values of the query positions , where can be thought as the state of the map. Following the idea of GP classification, the prior of the state is , where and is constructed by applying a kernel function on , i.e. . Note by initializing the prior covariance with the kernel we are capturing the same correlation which would be captured by the associated GP. With the assumption that the measurement is within the predefined tessellation , the binary measurement model can be represented as
with the Bernoulli distribution,
Assume that the distribution of the map follows at time . With a measurement for the cell at time , the posterior distribution of the map can be computed based on Bayes’ rule, i.e.,
Since the likelihood function is a cumulative normal function, the posterior of at is no longer normal. To ensure the map follows a normal pdf, the posterior is estimated by minimizing the following KullbackLeibler (KL) divergence^{2}^{2}2The KL operator is defined as . Note that the KL operator is not associative. The order in (6) is chosen so that (6) can be solved analytically.
(4) 
The same objective function is used in [29] to enable the contextaware filter to incorporate binary measurements. We adapt the formulation of the discrete measurement update from the contextaware filter to solve (4) for the mapping problem. The mean and covariance estimation for the map at is
(5) 
where
is a selection vector containing all
s but a single corresponding to the cell, is the normalization factor,The occupancy of each cell can then be recovered through (3). Unlike GP classification, the occupancy grid filter merges both training and prediction steps, and incrementally updates the map only keeping track of the latent values on the grid.
V Theoretical Properties of OGF
A mentioned in Sec. III, GP classification cannot be solved in closed form. In this section we briefly describe one of the conventional iterative solutions, EP, in the context of the mapping problem. We then show that the occupancy grid filter introduced in Sec. IV is a streamwise approximation of EP and can solve the occupancy mapping problem with computation complexity linear in the number of measurements at each time step and quadratic in the size of the map. The idea of EP is to approximate the posterior distribution of , , by
where is a normalization factor and
is an unnormalized normal pdf. For convenience, we follow the naming convention in [24, Ch.3] calling , , and site parameters. Conceptually, EP approximates the original posterior distribution with a product of normals, which itself is also a normal pdf. The site parameters are estimated iteratively, and the final approximation, , can be easily recovered by combining each term after convergence. At each iteration, the cavity distribution of , defined as
is found with and called cavity parameters. Then the site parameters for are updated by minimizing the KL divergence between and , i.e.
(6) 
Since is in the exponential family, (6
) can be solved by matching the first two moments of the distributions. As given in
[24, Ch.3], the solution to (6) is,(7) 
and,
(8) 
In (7), and are the first and second moments of the distribution . A complete description of EP can be found in Algorithm 1.
Va Accuracy
Theorem 1.
Given a prior of a random variable with and a measurement with Bernoulli likelihood . If the posterior of , i.e. (the normalization factor is ignored here and onwards in this theorem), is to be approximated by another normal pdf , the following two approaches will give the same results.
(9) 
and,
(10)  
i.e.
Proof.
Recall that (9) is in the same form of (4), which is the objective function of the occupancy grid filter at the measurement update step. Therefore, the solutions for and follows the form of (5).
One critical observation for (10) is that the approximated distribution can be thought of as the product of a Gaussian prior and a measurement with a normal likelihood. The measurement model is this case is linear and can be represented as,
where is defined similarly as in (5). Then the resulting posterior distribution can be readily computed by following the measurement update step of a Kalman filter,
(11) 
Using the results for and in (7) and (8),
i.e. the two optimization methods result in the same approximation for posterior of . The details for computing and can be found in Appendix A. ∎
Corollary 1.1.
The proposed method is a streamwise approximation of Expectation Propagation to solve Gaussian process occupancy mapping, where “streamwise” means processing the measurements sequentially without iterations.
Proof.
We prove this corollary by applying Mathematical Induction. The initial prior for OGF is where and is constructed by the kernels as in Gaussian processes. The initial approximation provided EP is also , since for all given the initial settings in Algorithm 1. Assume after processing the measurement at time step , both OGF and EP have an approximation for the posterior of with . With the measurement at time , the OGF and EP generates new approximations for the posterior with and by solving (6) and (4) respectively. Note the correspondence between (6) and (10), as well as (4) and (9). A direct application of Theorem 1 shows that . Therefore, OGF is a streamwise approximation of the EP algorithm. ∎
Conceptually, EP can be considered as a smoothing method requiring multiple iterations to converge. The proposed OGF is its corresponding filter approach which process each measurement only once. Assume both EP^{3}^{3}3As has been mentioned in [24], EP is not guaranteed to converge. However, EP shows good convergence property in practical application. and OGF converge in the sense that with new measurements, the estimated mean and uncertainty no longer changes. Based on (5), (7), and (8), this happens if and only if for every cell in the map,
, which implies all cells are well classified. In practice,
may converge to different values for EP and OGF because the gradient of the normal pdf approaches out of the three deviations. However, the difference should not affect the final classification result since is almost at its extreme values when . This behavior will be more clear with the experiment results in the 2D simulation environment in Sec. VIA.VB Complexity
Representing the map size as , we consider the computational complexity at time step with the assumption that a constant number of measurements, , is taken at each time step. Then we have the following remark about the computation complexity.
Remark 1.
For a single time step, the computation complexities for Laplace approximation, EP, and OGF are , , and respectively.
The cubic complexity of Laplace approximation comes from the inversion of the Hessian in Newton’s method, and is the average number of iterations for Newton’s method to converge. For EP, the cubic complexity results from the iterative update where the update for the site parameters of each has complexity . For OGF, no prediction step is required. The computation complexity only results from the measurement update. A direct observation is that, unlike conventional methods, the computation complexity of our approach does not scale with time. Also, in practical applications, the total number of measurements over time is usually in the same order with the dimension of the map. In this case, OGF has much lower computation complexity compared to Laplace approximation and EP.
Vi Experimental Results
Two kinds of experimental results are shown in this section. In Section VIA, we compare the accuracy of OGF with EP and the occupancy grid mapping algorithm [1] in a 2D simulated environment. In Section VIB, we compare the maps built using OGF and Octomap [2] using 3D point cloud measurements from a Velodyne. In both simulated and real world experiments, we used the standard normal pdf as the kernel function, i.e.
Via 2D Simulation Environment
We compare the proposed OGF with EP and occupancy grid mapping algorithm in a 2D simulated environment with known ground truth. Noisefree samples were taken randomly from the map without repetition. The 2D ground truth map and an example of random samples are shown in Figure 2.
with the standard deviation of the normal pdf kernel set to
grid unit.Figure 3 shows the map estimated by both EP and OGF, as well as the difference between the two maps, with the measurements shown in Figure 1(b). As can be observed directly in Figure 2(c), the differences were mainly concentrated at the wellclassified regions where the absolute latent values are relatively large. This echoes our analysis in Section V.
We used (12) to convert the difference of the latent values into a scalar metric, which is the difference as a percentage of the map estimated by EP.
(12) 
Figure 2(d) shows with respect to the number of measurements. Based on the plot, the difference was generally less than . It can also be observed that increased with the number of measurements, which was due to the fact that more cells in the map should be wellclassified as the measurement increases. As shown in the later examples, this minor error in latent values does not affect the classification results.
Figure 3(a) shows the accuracy of the map generated by the occupancy grid mapping algorithm, EP, and OGF with the accuracy defined as,
(13) 
where is the map size, while and are defined in Sec. II and III. It should be noted that occupancy grid mapping was trivial in this case; since the measurements were noisefree, the map generated by the occupancy grid algorithm was the same as marking sample positions as whatever the measurements were.
As can be seen from Figure 3(a), the map generated using the proposed method has the same accuracy as EP for various numbers of measurements. This supports our previous claim that the minor difference in latent value estimation does not affect the classification. It is also indicated in Figure 3(a) that both EP and OGF have lower accuracy than the occupancy grid mapping when there are few samples, but outperform it once the samples get reasonably dense. The reason for this is that sparse measurements will not alter the latent value of a cell far from its prior mean so the occupancy status of most of the cells remain unknown. However, when the density of samples increases the OGF is able to infer the occupancy of the cells in the neighborhood of the measured samples. The required density depends on the type and the parameters of the kernel in use. Figure 4 shows the map generated by the three algorithms with and samples respectively.
ViB Real World Data
In order to evaluate the effectiveness of OGF with real world data we implemented the proposed algorithm using C++ with ROS^{4}^{4}4www.ros.org. A Velodyne Puck (VLP16)^{5}^{5}5velodynelidar.com/vlp16.html was used to provide 3D point cloud measurements. The lidarbased odometry method (LOAM) [30] was used to estimate the position and orientation of the sensor.
To enable the algorithm to deal with the dense point clouds provided by Velodyne, we applied the following downsampling method that reduced the number of samples taken at each measurement update step, but at the same time kept all the samples uniformly distributed across the 3D space. For each lidar ray, we used the ray tracing algorithm introduced in
[31] to identify which cells that ray has travelled through. A measurement was only taken in a cell if no measurement had been taken within this cell before. As has been mentioned in [2], a cell measured as free by one lidar ray may be reported as occupied in others due to the discretization of the 3D space. This effect is especially obvious when the map resolution is coarse. To overcome this problem, we prioritized occupied measurements of a cell over free measurements of the same cell, i.e a cell previously measured as free could be remeasured as occupied, but the opposite was not allowed.Figure 5 shows a comparison between the map generated by Octomap and OGF in both outdoor and indoor environments. In the experiment with real world data, OGF demonstrated its capability of generating a denser map than Octomap by filling the gaps, which is especially obvious comparing the reconstructed ground.
Vii Conclusion
This paper presented an incremental approach for solving the occupancy estimation problem. We proved that the proposed Occupancy Grid Filter is a streamwise approximation of EP, one of the conventional methods to solve GP classification, with constant computation complexity in time. With the 2D simulation environment, we demonstrated that OGF generates almost the same classification results as EP, albeit with minor differences in the latent values only for the wellclassified cells. Also, the accuracy outperforms the occupancy grid mapping algorithm with reasonable number of measurements. We also evaluated the performance of OGF on real world data, and compared the results with Octomap. It was demonstrated that, because of the more accurate representation of the uncertainty of the map, OGF is able to fill the gaps and produce a map that is reasonably denser.
As the computation complexity of our method is quadratic in the map size, for the future work, we would like to explore the possibility of developing a map representation with an adaptive resolution that is compatible with the current framework. Furthermore, we would like to understand if the covariance information provided by OGF, which drops the independence assumption between the cells in the grid, can be a better guidance for autonomous exploration and mapping compared to the classical occupancy grid map.
Appendix A Complementary proof for Theorem 1
In this section, we complete the intermediate steps of (11), which is the measurement update step of a Kalman filter with prior and a linear measurement model . Define,
Then change of mean is,
Using the results from (7) and (8), can be computed as in (14). Note that and are used in (14) to represent the cavity parameters instead of and .
(14)  
Similarly, the change of covariance is,
The change of mean and covariance matches the measurement update step in the occupancy grid filter in (5).
References
 [1] A. Elfes, “Using occupancy grids for mobile robot perception and navigation,” Computer, vol. 22, no. 6, pp. 46–57, 1989.
 [2] A. Hornung, K. Wurm, M. Bennewitz, C. Stachniss, and W. Burgard, “OctoMap: an efficient probabilistic 3D mapping framework based on octrees,” Autonomous Robots, vol. 34, no. 3, pp. 189–206, 2013.
 [3] S. Izadi, D. Kim, O. Hilliges, D. Molyneaux, R. Newcombe, P. Kohli, J. Shotton, S. Hodges, D. Freeman, A. Davison, and A. Fitzgibbon, “KinectFusion: Realtime 3D Reconstruction and Interaction Using a Moving Depth Camera,” in ACM Sym. on User Interface Software and Technology (UIST), 2011, pp. 559–568.
 [4] T. Whelan, R. SalasMoreno, B. Glocker, A. Davison, and S. Leutenegger, “ElasticFusion: RealTime Dense SLAM and Light Source Estimation,” The International Journal of Robotics Research, vol. 35, no. 14, pp. 1697–1716, 2016.
 [5] P. Henry, M. Krainin, E. Herbst, X. Ren, and D. Fox, “RGBD mapping: Using Kinectstyle depth cameras for dense 3D modeling of indoor environments,” The International Journal of Robotics Research (IJRR), vol. 31, no. 5, pp. 647–663, 2012.
 [6] R. Newcombe, “Dense Visual SLAM,” Ph.D. dissertation, Imperial College London, 2012.

[7]
J. Engel, T. Schöps, and D. Cremers, “LSDSLAM: Largescale direct
monocular SLAM,” in
European Conference on Computer Vision (ECCV)
, 2014.  [8] S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics. MIT Press Cambridge, 2005.
 [9] M. Ghaffari Jadidi, J. Valls Miro, R. Valencia, and J. AndradeCetto, “Exploration on continuous Gaussian process frontier maps,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2014, pp. 6077–6082.
 [10] M. Ghaffari Jadidi, J. Valls Miro, and G. Dissanayake, “Gaussian Process Autonomous Mapping and Exploration for Range Sensing Mobile Robots,” ArXiv: 1605.00335, 2016.
 [11] B. Charrow, G. Kahn, S. Patil, S. Liu, K. Goldberg, P. Abbeel, N. Michael, and V. Kumar, “InformationTheoretic Planning with Trajectory Optimization for Dense 3D Mapping,” in Robotics: Science and Systems, 2015.
 [12] A. Krause, “Optimizing Sensing,” Ph.D. dissertation, Carnegie Mellon University, December 2008.
 [13] N. Atanasov, “Active Information Acquisition with Mobile Robots,” Ph.D. dissertation, University of Pennsylvania, 2015.
 [14] B. Charrow, “InformationTheoretic Active Perception for MultiRobot Teams,” Ph.D. dissertation, University of Pennsylvania, 2015.
 [15] S. O’Callaghan, F. Ramos, and H. DurrantWhyte, “Contextual occupancy maps using gaussian processes,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2009, pp. 1054–1060.
 [16] S. O’Callaghan and F. Ramos, “Gaussian process occupancy maps,” The International Journal of Robotics Research, vol. 31, no. 1, pp. 42–62, 2012.
 [17] ——, “Continuous Occupancy Mapping with Integral Kernels,” 2011.

[18]
F. Ramos and L. Ott, “Hilbert maps: scalable continuous occupancy mapping with stochastic gradient descent,” in
Robotics: Science and Systems, 2015.  [19] C. Vido and F. Ramos, “From grids to continuous occupancy maps through area kernels,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2016, pp. 1043–1048.
 [20] S. Kim and J. Kim, “Recursive Bayesian Updates for Occupancy Mapping and Surface Reconstruction,” in Australasian Conference on Robotics and Automation (ACRA), 2014.
 [21] ——, “Occupancy Mapping and Surface Reconstruction Using Local Gaussian Processes With Kinect Sensors,” IEEE Transactions on Cybernetics, vol. 43, no. 5, pp. 1335–1346, 2013.
 [22] ——, GPmap: A Unified Framework for Robotic Mapping Based on Sparse Gaussian Processes. Springer International Publishing, 2015, pp. 319–332.
 [23] J. Wang and B. Englot, “Fast, accurate gaussian process occupancy maps via testdata octrees and nested bayesian fusion,” in IEEE Int. Conf. on Robotics and Automation (ICRA), 2016, pp. 1003–1010.

[24]
C. E. Rasmussen and C. K. I. Williams,
Gaussian Processes for Machine Learning
. the MIT Press, 2006.  [25] S. Ambikasaran, D. ForemanMackey, L. Greengard, D. Hogg, and M. O’Neil, “Fast Direct Methods for Gaussian Processes,” ArXiv:1403.6015, 2014.
 [26] C. E. R. Joaquin QuiñoneroCandela, “A Unifying View of Sparse Approximate Gaussian Process Regression,” Journal of Machine Learning Research (JMLR), vol. 6, no. Dec, pp. 1939–1959, 2005.
 [27] S. Anderson, T. Barfoot, C. H. Tong, and S. Särkkä, “Batch nonlinear continuoustime trajectory estimation as exactly sparse gaussian process regression,” Autonomous Robots, vol. 39, no. 3, pp. 221–238, 2015.
 [28] S. Reece and S. Roberts, “An Introduction to Gaussian Processes for the Kalman Filter Expert,” in Conference on Information Fusion, 2010.
 [29] R. Ivanov, N. Atanasov, M. Pajic, G. Pappas, and I. Lee, “Robust estimation using contextaware filtering,” in Allerton Conference on Communication, Control, and Computing, Sept 2015, pp. 590–597.
 [30] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in realtime.” in Robotics: Science and Systems, vol. 2, 2014.
 [31] J. Amanatides, A. Woo et al., “A fast voxel traversal algorithm for ray tracing,” in Eurographics, vol. 87, no. 3, 1987, pp. 3–10.
Comments
There are no comments yet.