Stochastic Simulation in Surface Reconstruction and Application to 3D Mapping Jeff Leal, Steve Scheding and † Gamini Dissanayake Australian Centre for Field Robotics (ACFR) Rose Street Building (J04) The University of Sydney {jeff,scheding}@acfr.usyd.edu.au † Department of Mechanical and Mechatronic Engineering University of Technology, Sydney
[email protected] Abstract Three dimensional terrain maps are useful representations of environments for various robotic applications. Unfortunately, sensor data (from which such maps are built) is uncertain and contains errors which are usually not accounted for in existing terrain building algorithms. In real-time applications, it is necessary to quantify these uncertainties to allow map construction decisions to be made online. This paper addresses this issue by providing a representation that explicitly accounts for sensing uncertainty. This is achieved through the use of stochastic simulation techniques. The result is in an algorithm for online 3D multi-resolution surface reconstruction of unknown, and unstructured environments. Results of the surface reconstruction algorithm in a real environment are presented.
1 Introduction Surface reconstruction consists of building a surface from acquired sensor data. Terrain surfaces of unstructured environments are an important form of robotic ‘perception’ in many applications, allowing autonomous decision making by the robotic system. These applications are manifold; most commonly the generated surface (map) is used for navigation, localization, path planning, etc. [1, 2]. Another application of surface reconstruction is to generate a meshed model in order to perform differential computation on localized nodes of that object (e.g. Finite Element Analysis, Airfoil Flow, etc.) [3]; further employment of such techniques are for visualization purposes [4]. There is a large amount of literature on the subject of mesh generation, and surveys on various surface reconstruction and meshing techniques can be found in [5, 7, 8].
Sensors usually provide imperfect and incomplete information in their measurements. The use of sensor data to reconstruct the maps introduces some important issues. The main issue consists of the fact that decisions (e.g. tessellation, decimation, localisation, navigation, etc.) are being made with uncertain sensor data. Additionally, most imaging sensors provide enormous quantities of information, which makes the handling and fusing of this information into a numerical representation difficult. There are many potential computational bottlenecks (both processing and memory requirements). These issues are not usually addressed in existing algorithms which frequently accept the generated surface as a ‘true’ model of the surface. This makes performing decisions a difficult or even unfeasable task. Unfortunately, it is not possible to identify and quantify the exact sources of all these errors, which consequently results in the need to be able to perform decisions under uncertainty. The approach presented here introduces a stochastic information representation, which summarises all available sensor data. This results in a representation that quantifies the bounds of the uncertainty in a surface region. Therefore, decisions can be performed using the most current available knowledge. The algorithm also enables the fusing of data from multiple viewpoints and different types of ranging sensors (e.g. Laser, Radar, Stereo Vision, etc.). The following section describes the methodology used in this approach. Section 3 describes the representation of sensor information. Section 4 describes the surface representation. Finally, some experimental results using this approach are presented, followed by conclusions.
2 Methodology A surface is fitted to data obtained from multiple viewpoints and from various sensors. In order to register data correctly it is necessary to know with some degree of cer-
tainty the sensor locations and orientations. As will be seen in Sections 3 and 5, uncertainty in sensor location can be dealt with easily. The sensor uncertainty can be modeled with some degree of accuracy with the information obtained from the manufacturers specifications or through empirical testing of the sensor. Although bounds can be chosen on these sources of uncertainties, there still exists the issue of representing these uncertainties in a manner that can be useful for making decisions about the true surface location. For this, a particle-based representation is used to encapsulate the uncertainties from these sources. Particles are spawned from a likelihood distribution that represents the uncertainty model (e.g. sensor observation uncertainty model, sensor location uncertainty model, etc.). This information is then maintained in a database and decisions are performed on the knowledge contained in that database. A Delaunay Triangulation (DT) is performed on the particle representation using a Weighted Principal Component Analysis method (WPCA)[9, 10]. The two representations, the sensor information summary, and the DT are distinct and separate. However the geometric model used (in this case a DT) is constructed from the particle representation. Figure 1 illustrates the general steps of the proposed algorithm for online multiresolution map-building. Sensor 1
...
Observation Uncertainty Model
Sensor N
Observation Uncertainty Model
Inference Technique (decision theory) Mission-specific utilities
Generate Samples
Fuse into Particle Representation
Local Information Management (Resampling SIR)
Summary of Information
Inference Technique (decision theory) Mission-specific utilities
Action 11
Action 21
Action 22
. . . Geometric Model Inference Technique (decision theory) Mission-specific utilities
Action N
Figure 1: Flowchart illustrating the main steps of the proposed algorithm.
The particle representation enable straightforward fusing of measurements from other sensors and other viewpoints into the information summary. The following section illustrates this concept in more detail.
3 Information Representation In an unknown environment, the only source of information are sensor measurements. The focus here is predominately with ranging sensors. Sensor data contain errors and uncertainty. In order to maintain a consistent repre-
sentation of the global information from all sensor data it is necessary to account for this uncertainty in the the representation. The particle representation is used as an approximation to sensor uncertainty distributions. The use of particles introduces some advantages over other representations (e.g. state space). In a state space representation it is necessary to define a linearised process and observation model. Approximation errors are introduced when performing these linearisations. Means of reducing these errors are presented in [11]. With the particle approach presented in this paper there is no need for linearisation, consequently reducing approximation errors for sufficiently large number of particles. Additionally, this method does not require a predefined array or occupancy grid where states are determined by locations in that array, thus increasing the efficiency by directing computation to specific nodes rather than to every location in the predefined array. Finally, it is impossible to define a process model, since the interest lies in obtaining surfaces of unknown environments. Consequently avoiding the introduction of more sources of errors. From this particle representation, consistent information about the surface can be extracted, such as the uncertainty in the surface location given all previous observations, and the amount of information in a particular region of the surface. Additionally, decision mechanisms can be formulated from this representation. These issues are developed in the following subsections. 3.1 Information Uncertainty In order to distinguish between particles that represent surface locations with lower uncertainty and surface locations with higher uncertainty, the notion of likelihood is introduced. The probabilistic information contained in an event, zk about the variable m is described by a distribution p(zk | m), known as the likelihood function. Sampling particles from this likelihood function allows to extract an approximation of the uncertainty distribution of a particular observation. An example of a particle distribution for one rangebearing observation, (robs , θobs ) is presented in Figure 2. The figure contains 1000 particles spawned from the joint distribution in range, N (robs , σr ) and in bearing, N (θobs , σθ ). Each particle is then weighted with the likelihood pi (zk | m):
pi (zk = (robs , θobs ) | m) =
e
−
(r−robs )2 2 2σr
√
2πσr
e
−
(θ−θobs )2 2σ2 θ
. √ 2πσθ
(1)
where m ∈ M , the surface location space and zk ∈ Z, the observation space. The result of every observation is then a group of particles that represent possible surface locations and the likelihood of these possibilities. This characterizes not only the local surface but also the observation uncertainties. This methodology can be eas-
proportional to l(x).g(x), where l(x) is a known function. The sampling importance resampling method states that N samples drawn from the discrete distribution g(x), with probability mass,
w(m i) =
Figure 2: Particle-based representation for the likelihood distribution from a range-bearing observation model( the density of particles depict regions of higher likelihood).
ily extended to sensor location uncertainty. As location uncertainty increases(decreases), the uncertainty in measurements increases(decreases). This allows to propagate location uncertainty through the observation model and into observation uncertainty. 3.2 Information Gain At any time, the current set of particles represents the likelihood of the surface passing through any point in space. Each sensor observation and its observation model provide a discrete sample of this distribution. Hence the sum of information provided by each observation’s likelihood function, represents the current best estimate of the entire surface’s likelihood function. Subsection 3.3 focuses on the issue of limiting the amount of global information, while still enabling the fusing of new information. As a result of this information knowledge, it is possible to define inference techniques in order to make decisions (e.g. active sensing, path planning, feature extraction, etc.) is enabled. 3.3 Information Management Due to limitations that might be imposed on the system, such as time and storage space, it may be necessary to reduce the amount of information in the particle representation. For this, resampling techniques are employed. There are various resampling techniques that have different outcomes. In the context of this work, the objective is to reduce the amount of knowledge but still maintaining the quality in that knowledge. One technique that performs this in a stochastic manner is Sampling Importance Resampling (SIR)[6, 12]. The idea is to sample a new set of N particles from the prior p(m | Z k−1 , Mk ) to obtain a set of samples that represent the posterior p(m | Z k , Mk ). Mk denotes a subset of the particles, in a neighboring region, to a point m on the surface distribution and the prior is represented by the particle distribution in that region. Let P denote the number of samples which are distributed from a continuous density function g(x), and that samples are required from the probability density function (pdf)
l(m i) i) Mk l(m
(2)
tends to the required density as N tends to infinity. If g(x) is taken as p(m | Z k−1 , Mk ) and l(x) as p(zk | m, Mk ), then the theorem provides a means of obtaining a new set of N samples, which represent the new posterior distribution. Proof of this result can be found in [12]. Taking N < P , is equivalent to reducing the number of particles that represent the posterior distribution, hence reducing the amount of knowledge.
4 Surface Representation In order to visualize a surface or to perform some simple decisions (e.g., distance to surface, local/global surface resolution, etc.), an approximation to the true surface must be chosen. As mentioned previously a Delaunay Triangulation was employed for the work presented in this paper, although there is no restriction on the model chosen. The reason for this choice is due to the DT properties of well formed triangles and minimized global amount of triangles over a surface[13]. Due to the fact that a summary of previously obtained sensor information is kept, an online multi-resolution surface approximation can now be formulated and fit to the particle set at any time. This property enables the representation of a surface that assists with mission-specific goals. Additionally, due to conflicting representations that may exist for multiple platform and multiple mission objectives (e.g. Navigation, Path Planning, Localization, etc.) different surfaces can be generated from the same underlying source of information, the particle representation.In order to approximate triangles to a set of weighted particles, a Weighted Principal Component Analysis is employed. 4.1 Weighted PCA This technique is used for fitting a triangular primitive to the current surface pdf(the particle representation) in a given region. In this work, the degree of optimality in the fit of the primitive to the true surface is defined as to minimize the mean square error (MMSE) of the pdf. It can be proven that the representation given by PCA is an optimal linear dimension decomposition technique in the least mean-square sense [14]. Due to the fact that the true surface is unknown and that there are uncertainties in the measurements, the MMSE will have uncertainty associated with it. Thus the MMSE is performed by calculating the variability in the particle distribution using
Figure 3: Data obtained from Laser and Radar Sensors.(a) Only Laser sensor data. (b) Only Radar sensor data.
Figure 4: The triangulated reconstructed surface from three viewpoints (VP), and with multiple scanning resolutions. Laser and Radar information are merged into the same representation. Information is kept in the additional scans. The MMSE threshold of the local particle representation over triangles was set to σ 2 = 0.02m2 . Triangulation resulted from (a) VP1 data; (b)VP1 data + VP2 data; (c) VP1 + VP2 + VP3 data.
the weights of the particles. This enables particles with more likelihood of belonging to the true surface to have more emphasis on the outcome of the WPCA. The WPCA algorithm enables the decomposition of any number of particles into N maximum variation directions in N-dimensions (principal components). In 3D, this consists of decomposing local particle distributions into three major directions. In order to perform this, a local covariance matrix is defined as,
Cl =
N
i=1
w(m i ).(m i − m).( ¯ m i − m) ¯ T N i) i=1 w(m
(3)
where w(m i ) are the weights defined by Equation 1 and m i are the locations of the particles in 3D space. The principal components are consequently given by the eigenvalues of Equation 3 and their directions are given by the corresponding eigenvectors. The smallest principal component is the projection on the direction in which the variance of the weighted particles is minimized. This direction corresponds to the normal of the particle set correct only up to orientation. Triangles are then updated to reflect the changes in the particle distribution by adjusting the normals of the triangles with the normals obtained by the WPCA minimum component. A similar approach to derive surface normals can be found in [4]. In some under-observed cases the particle normal
has not converged to a reasonable direction. These regions are designated low information sources, hence the local surface is an inadequate fit to the local sensor information representation. 4.2 Surface Management In order to approximate a set of triangles to the particle distribution with higher detail, it may be necessary, at some stage, to increase the number of triangles. This is performed by splitting triangles and re-fitting these triangles to the particle distribution. Additionally, it may be necessary to reduce the number of triangles in order to reduce or eliminate unnecessary detail. This is performed by merging triangles into lesser triangles and refitting them to the particle distribution. The decision for choosing a particular action to be performed (split, merge, update) can now be defined in a decision-theoretic framework. With the definition of utilities, it is possible to achieve surfaces that are more useful to specific applications. This technique was shown to be efficient in 2D map-building [1]. At the current stage of this work, only simple decimation utilities were implemented in 3D.
5 Experimental Results A mm-Wave Radar and a Riegl Rangefinder Laser mounted on a Pan-Tilt device were used to obtain out-
door data. The platform is shown in Figure 5. Differential GPS was used to register sensor location and attitude. This information allowed for data registration. Due to the errors in the convergence of the DGPS measurements, the registration of all three scans is not ideal. The raw data obtained from both sensors can be seen in Figure 3. The intensity of the returns from the devices are also depicted. Since the flora of the testing area was predominantly grass, the intensity of returns were mostly inversely proportional to range. Results from using this algorithm with sensor data obtained from the platform at three different viewpoints are shown in Figure 4. To allow for the uncertainty of the mismatched data, due to registration errors, in the reconstructed surface, the MMSE threshold was set to σ 2 = 0.02m2 . This allows for some disparity in the sensor data.
7(d)(e)(f) show results obtained by applying the algorithm with two different σ 2 as thresholds, 0.02m2 and 0.05m2. Figure 7(e) shows that resampling and updating is predominantly performed when observations are made in revisited regions, this is seen also in (d) where the flat regions relate to small information gain. The regions of high information gain occur mostly at the beginning of a new scan where the sensors are pointed towards unvisited regions. Figure 7(f) finally demonstrates the average error formed between the triangle normals and the WPCA normal. Although, initially the result seems to be the inverse of what is expected, by combining the results seen in (d), i.e., the fact that with σ 2 = 0.05m2 updates are higher than with σ 2 = 0.02m2 , this allows for triangle normals to converge more to the respective particle normals. Tessellation with poorly registered data demonstrates to be an issue, due to the fact that data is being fused incorrectly. This effect can be reduced, by altering the normal error threshold to larger values, with a tradeoff in map resolution.
6 Conclusions and Future Work Figure 5: mm-Wave Radar and Riegl Rangefinder Laser mounted on a Pan-Tilt unit.
Figure 6: The representation of two levels of knowledge about the surface: The particle representation, and the triangulated surface (DT).
Figure 6 depicts the two levels of representation in the knowledge base. One is the geometric model (DT) that is designed to fit in a Least Mean Square (LMS) sense to the other, the particle representation. The utilities defined in these examples are to minimize local errors, forcing a threshold on the global error in the final reconstructed surface. Additionally, it can be seen that regions where little sensor information is available results in a larger triangulation. Figures 7(a)(b) demonstrate that decisions to tessellate the triangulation are more predominant on Laser information, since this sensor data has higher quality, hence higher likelihood. Consequently, the inverse happens with respect to decimation. Figure 7(c) shows the effects of resampling on the resulting amount of different sensor data. The Radar data is reduced in a greater amount compared to the lesser uncertain Laser data. Figures
This paper presented an approach to surface reconstruction that encapsulates the uncertainties in the sensorial data. In addition, a stochastic framework was described to deal with such uncertainties and to maintain a consistent knowledge representation. Experimental results were shown, in a large, unstructured and expandable environment demonstrating the success of this approach in the surface reconstruction problem. Work is currently being done in defining utility functions that perform multiple terrain building operations depending on the application. This will allow for the surface to alter its appearance in order to maximize its use in different situations.
References [1] J. Leal, S. Scheding, and G. Dissanayake, “Probabilistic 2D mapping in unstructured environments,” Proceedings of the Australian Conference on Robotics and Automation, pp. 19–24, August 2000, Melbourne, Australia. [2] S. Thrun, W. Burgard, and D. Fox, “A real-time algorithm for mobile robot mapping with applications to multi-robot and 3d mapping,” IEEE International Conference on Robotics and Automation, pp. 321–328, April 2000, San Feancisco, CA. [3] P.L. Chew, “Guaranteed-quality mesh generation for curved surfaces,” The 9th Annual Computational Geometry, 1993. [4] H. Hoppe, T. DeRose, T. Duchamp, J. McDonald, and W. Stuetzle, “Surface reconstruction from unorga-
Figure 7: (a)(b) Total number of splits and merges at the current observation with σ 2 = 0.02m2 . (c) Number of particles
existent in the sensor information summary discriminated by sensor type with σ 2 = 0.02m2 . (d) Total number of triangles with σ 2 = 0.02m2 and σ 2 = 0.05m2 . (e) Total number of updated and resampled triangles with σ 2 = 0.02m2 and σ 2 = 0.05m2 . (f) Average normal error between the particle distribution and the triangulation (degrees) with σ 2 = 0.02m2 and σ 2 = 0.05m2 .
nized points,” Computer Graphics (SIGGRAPH), vol. 26, no. 2, pp. 71–78, 1992. [5] M. Bern and D. Eppstein, “Mesh generation and optimal triangulation,” Csl-92-1, Xerox PARC, March 1992. [6] P. Fearnheard, “Sequential Monte Carlo Methods in Filter Theory,” PhD Thesis.Merton College, University of Oxford 1998.
[11] S.J. Julier and J.K. Uhlmann, “A consistent, debiased method for converting between polar and cartesian coordinate systems,” The Proceedings of AeroSense, March 1997, Orlando, Florida. [12] A.F.M. Smith and A.E. Gelfand, “Bayesian statistics without tears: A sampling-resampling perspective,” The American Statistician, vol. 46, no. 2, pp. 84–88, May 1992.
[7] P.S. Heckbert and M. Garland, “Survey of surface approximation algorithms,” Tech. Rep. CMU-CS-95-194, CS Department.,Carnegie Mellon University, 1995.
[13] L. Guibas and S. Jorge, “Primitives for the manipulation of general subdivisions and the computation of voronoi diagrams in filter theory,” ACM Transaction on Graphics, vol. 4, no. 2, pp. 74–123, April 1985.
[8] S.J. Owen, “A survey of unstructured mesh generation technology,” 7th Proceedings of the International Meshing Roundtable, pp. 239–267, 1998.
[14] I.T. Jollife, Principal Component Analysis, Springer-Verlag, 1986.
[9] S. Vijayakumar, Computational Theory of Incremental and Active Learning for Optimal Generalization, 95d38108, Tokyo Institute of Technology, 1998. [10] W.F. Massy, “Principal component regression in exploratory statistical research,” Journal of the American Statistical Association, vol. 60, pp. 234–246, 1965.
