Estimating Motion From Sparse Range Data Without Correspondence

Abstract

Estimating observer motion from time-varying range &ita and fusing this data into a coherent map of the environment are two impor- tant problems in robot navigation. Current methods first detenmine a correspondence between range measurements acquired from different viewpoints, and then compute a motion estimate from this correspon- dence. In this paper, we present an alternative technique which does not assume that any such correspondence exists. Instcad, a smooth surface assumption is used, i.e., the sensed points are imedl to lie on some piecewise smooth surface. A motion estimate is obtained by finding the geometric transformation which makes it most likely (in a Bayesian sense) that the points came from the same surface. We derive an energy equation which measures the distance bctween the new data pints and the dense interpolated depth map wllich is being incrementally rehed. The shape of the energy equation in the neighborhood of the optimal motion estimate is used to com- pute the uncertainty in the estimate. The resulting modon estimation algorithm can be used in conjunction with other motion stirnation systems, and provides a flexible and robust method for computing motion ffom sparse range data. sets of points so that they may be integrated into an updated surface estimate. Our metliad is based on a smooth surface usswnption, i.e., the points which are sensed with the range finder (from two or more viewpoints) are assumed to lie on a piecewise smooth surface:. Our algorithm finds the motion which makes it most likely that these sets of points are from the same piecewise smooth scene. In practice, the logarithm of the likelihood measure itums out to be closely related to the weighted sum of squan:s distance between the new data points and the current surface estimate. This method thus fits well with incremental sensing strategies, where dense depth estimates ~IC obtained by integrating measurements taken from a moving camera (Matthies88). The method presented in this paper shows how to mea- sure the the likelihood of a particular collection of transformed points being properly "regisxered", and how to find a locally optimal motion estimate using gradient descent. This paper does not, however, address tlhe issue of how to search the large space of possible transformations for the "best" motion. Our method is thus meant to b, used in conjunction with some other motion estimation system - such as an inertial navi- gation system - which is used to start the gradient descent algorithm in the vicinity of the solution. Our method will also work if the range of possible motions is small, which can be ensured by sampling the data sufficiently rapidly (as is the case in real-time robot control). The motion estimation algorithm we develop can be ap- plied to both mobile robot navigation and robot manipulation. As part of a mobile robot syslem, the algorithm is used to refine or improve motion estimates obtained from other sources such as inemal navigation, dead retkoning or landmark recognition. The algorithm also builds up and maintains a dense depth map of the environment which can, be used for integration with other sensors. This map can either be a retinotopic (image-based) depth map or a terrain-based elevation map (Olin88). Our al- gorithm is particularly well suited for terrain maps since it can handle data points that are irn:gularly spaced (from perspective de-projection), incaporate prior knowledge from cartographic data, and fuse data with only limited mas of overlap. In robot manipulation, our algorithm can be used to determine object or observer motion from sparse tactile data. The general approach uised in this paper is to incremen- tally build up a dense depth map by interpolating and integrat- ing sparse range data, and to match new points to this surface to perform the motion estimation. We thus start by reviewing