Vector Quantized Variational AutoEncoders (VQ-VAEs) are designed to compress a continuous input to a discrete latent space and reconstruct it with minimal distortion. They operate by maintaining a set of vectors---often referred to as the codebook---and quantizing each encoder output to the nearest vector in the codebook. However, as vector quantization is non-differentiable, the gradient to the encoder flows _around_ the vector quantization layer rather than _through_ it in a straight-through approximation. This approximation may be undesirable as all information from the vector quantization operation is lost. In this work, we propose a way to propagate gradients through the vector quantization layer of VQ-VAEs. We smoothly transform each encoder output into its corresponding codebook vector via a rotation and rescaling linear transformation that is treated as a constant during backpropagation. As a result, the relative magnitude and angle between encoder output and codebook vector becomes encoded into the gradient as it propagates through the vector quantization layer and back to the encoder. Across 11 different VQ-VAE training paradigms, we find this restructuring improves reconstruction metrics, codebook utilization, and quantization error.

Decision trees remain one of the most popular machine learning models today, largely due to their out-of-the-box performance and interpretability. In this work, we present a Bayesian approach to decision tree induction via maximum a posteriori inference of a posterior distribution over trees. We first demonstrate a connection between maximum a posteriori inference of decision trees and AND/OR search. Using this connection, we propose an AND/OR search algorithm, dubbed MAPTree, which is able to recover the maximum a posteriori tree. Lastly, we demonstrate the empirical performance of the maximum a posteriori tree both on synthetic data and in real world settings. On 16 real world datasets, MAPTree either outperforms baselines or demonstrates comparable performance but with much smaller trees. On a synthetic dataset, MAPTree also demonstrates greater robustness to noise and better generalization than existing approaches. Finally, MAPTree recovers the maxiumum a posteriori tree faster than existing sampling approaches and, in contrast with those algorithms, is able to provide a certificate of optimality. The code for our experiments is available at https://github.com/ThrunGroup/maptree.

Large Language Models like ChatGPT demonstrate a remarkable capacity to learn new concepts during inference without any fine-tuning. However, visual models trained to detect new objects during inference have been unable to replicate this ability, and instead either perform poorly or require meta-training and/or fine-tuning on similar objects. In this work, we propose a meta-learning algorithm that emulates Large Language Models by learning new visual concepts during inference without fine-tuning. Our approach leverages a frozen pre-trained feature extractor, and analogous to in-context learning, recasts meta-learning as sequence modeling over datapoints with known labels and a test datapoint with an unknown label. On 8 out of 11 meta-learning benchmarks, our approach---without meta-training or fine-tuning---exceeds or matches the state-of-the-art algorithm, P>M>F, which is meta-trained on these benchmarks.

Maximum Inner Product Search (MIPS) is a ubiquitous task in machine learning applications. Given a query vector and $n$ other vectors in $d$ dimensions, the MIPS problem is to find the atom that has the highest inner product with the query vector. Existing MIPS algorithms scale at least as $O(\sqrt{d})$ with respect to $d$, which becomes computationally prohibitive in high-dimensional settings. In this work, we present BanditMIPS, a novel randomized algorithm that provably improves the state-of-the-art complexity from $O(\sqrt{d})$ to $O(1)$ with respect to $d$. We validate the scaling of BanditMIPS and demonstrate that BanditMIPS outperforms prior state-of-the-art MIPS algorithms in sample complexity, wall-clock time, and precision/speedup tradeoff across a variety of experimental settings. Furthermore, we propose a variant of our algorithm, named BanditMIPS-$\alpha$, which improves upon BanditMIPS by employing non-uniform sampling across coordinates. We also demonstrate the usefulness of BanditMIPS in problems for which MIPS is a subroutine, including Matching Pursuit and Fourier analysis. Finally, we demonstrate that BanditMIPS can be used in conjunction with preprocessing techniques to improve its complexity with respect to $n$. All of our experimental results are reproducible via a 1-line script at github.com/ThrunGroup/BanditMIPS.

Clustering is a fundamental task in data science with wide-ranging applications. In $k$-medoids clustering, cluster centers must be actual datapoints and arbitrary distance metrics may be used; these features allow for greater interpretability of the cluster centers and the clustering of exotic objects in $k$-medoids clustering, respectively. $k$-medoids clustering has recently grown in popularity due to the discovery of more efficient $k$-medoids algorithms. In particular, recent research has proposed BanditPAM, a randomized $k$-medoids algorithm with state-of-the-art complexity and clustering accuracy. In this paper, we present BanditPAM++, which accelerates BanditPAM via two algorithmic improvements, and is $O(k)$ faster than BanditPAM in complexity and substantially faster than BanditPAM in wall-clock runtime. First, we demonstrate that BanditPAM has a special structure that allows the reuse of clustering information $\textit{within}$ each iteration. Second, we demonstrate that BanditPAM has additional structure that permits the reuse of information $\textit{across}$ different iterations. These observations inspire our proposed algorithm, BanditPAM++, which returns the same clustering solutions as BanditPAM but often several times faster. For example, on the CIFAR10 dataset, BanditPAM++ returns the same results as BanditPAM but runs over 10$\times$ faster. Finally, we provide a high-performance C++ implementation of BanditPAM++, callable from Python and R, that may be of interest to practitioners at https://github.com/motiwari/BanditPAM. Auxiliary code to reproduce all of our experiments via a one-line script is available at https://github.com/ThrunGroup/BanditPAM_plusplus_experiments.

Random forests are some of the most widely used machine learning models today, especially in domains that necessitate interpretability. We present an algorithm that accelerates the training of random forests and other popular tree-based learning methods. At the core of our algorithm is a novel node-splitting subroutine, dubbed MABSplit, used to efficiently find split points when constructing decision trees. Our algorithm borrows techniques from the multi-armed bandit literature to judiciously determine how to allocate samples and computational power across candidate split points. We provide theoretical guarantees that MABSplit improves the sample complexity of each node split from linear to logarithmic in the number of data points. In some settings, MABSplit leads to 100x faster training (an 99% reduction in training time) without any decrease in generalization performance. We demonstrate similar speedups when MABSplit is used across a variety of forest-based variants, such as Extremely Random Forests and Random Patches. We also show our algorithm can be used in both classification and regression tasks. Finally, we show that MABSplit outperforms existing methods in generalization performance and feature importance calculations under a fixed computational budget. All of our experimental results are reproducible via a one-line script at https://github.com/ThrunGroup/FastForest.

Clustering is a ubiquitous task in data science. Compared to the commonly used k-means clustering, k-medoids clustering requires the cluster centers to be actual data points and supports arbitrary distance metrics, which permits greater interpretability and the clustering of structured objects. Current state-of-the-art k-medoids clustering algorithms, such as Partitioning Around Medoids (PAM), are iterative and are quadratic in the dataset size n for each iteration, being prohibitively expensive for large datasets. We propose BanditPAM, a randomized algorithm inspired by techniques from multi-armed bandits, that reduces the complexity of each PAM iteration from O(n^2) to O(nlogn) and returns the same results with high probability, under assumptions on the data that often hold in practice. As such, BanditPAM matches state-of-the-art clustering loss while reaching solutions much faster. We empirically validate our results on several large real-world datasets, including a coding exercise submissions dataset from Code.org, the 10x Genomics 68k PBMC single-cell RNA sequencing dataset, and the MNIST handwritten digits dataset. In these experiments, we observe that BanditPAM returns the same results as state-of-the-art PAM-like algorithms up to 4x faster while performing up to 200x fewer distance computations. The improvements demonstrated by BanditPAM enable k-medoids clustering on a wide range of applications, including identifying cell types in large-scale single-cell data and providing scalable feedback for students learning computer science online. We also release highly optimized Python and C++ implementations of our algorithm.

Some robots must repeatedly interact with a fixed set of objects in their environment. To operate correctly, it is helpful for the robot to be able to recognize the object instances that it repeatedly encounters. However, current methods for recognizing object instances require that, during training, many pictures are taken of each object from a large number of viewing angles. This procedure is slow and requires much manual effort before the robot can begin to operate in a new environment. We have developed a novel procedure for training a neural network to recognize a set of objects from just a single training image per object. To obtain robustness to changes in viewpoint, we take advantage of a supplementary dataset in which we observe a separate (non-overlapping) set of objects from multiple viewpoints. After pre-training the network in a novel multi-stage fashion, the network can robustly recognize new object instances given just a single training image of each object. If more images of each object are available, the performance improves. We perform a thorough analysis comparing our novel training procedure to traditional neural network pre-training techniques as well as previous state-of-the-art approaches including keypoint-matching, template-matching, and sparse coding, and we demonstrate that our method significantly outperforms these previous approaches. Our method can thus be used to easily teach a robot to recognize a novel set of object instances from unknown viewpoints.

Reasoning about a scene's thermal signature, in addition to its visual appearance and spatial configuration, would facilitate significant advances in perceptual systems. Applications involving the segmentation and tracking of persons, vehicles, and other heat-emitting objects, for example, could benefit tremendously from even coarsely accurate relative temperatures. With the increasing affordability of commercially available thermal cameras, as well as the imminent introduction of new, mobile form factors, such data will be readily and widely accessible. However, in order for thermal processing to complement existing methods in RGBD, there must be an effective procedure for calibrating RGBD and thermal cameras to create RGBDT (red, green, blue, depth, and thermal) data. In this paper, we present an automatic method for the synchronization and calibration of RGBD and thermal cameras in arbitrary environments. While traditional calibration methods fail in our multimodal setting, we leverage invariant features visible by both camera types. We first synchronize the streams with a simple optimization procedure that aligns their motion statistic time series. We then find the relative poses of the cameras by minimizing an objective that measures the alignment between edge maps from the two streams. In contrast to existing methods that use special calibration targets with key points visible to both cameras, our method requires nothing more than some edges visible to both cameras, such as those arising from humans. We evaluate our method and demonstrate that it consistently converges to the correct transform and that it results in high-quality RGBDT data.

Precision tracking is important for predicting the behavior of other cars in autonomous driving. We present a novel method to combine laser and camera data to achieve accurate velocity estimates of moving vehicles. We combine sparse laser points with a high-resolution camera image to obtain a dense colored point cloud. We use a color-augmented search algorithm to align the dense color point clouds from successive time frames for a moving vehicle, thereby obtaining a precise estimate of the tracked vehicle's velocity. Using this alignment method, we obtain velocity estimates at a much higher accuracy than previous methods. Through pre-filtering, we are able to achieve near real time results. We also present an online method for real-time use with accuracies close to that of the full method. We present a novel approach to quantitatively evaluate our velocity estimates by tracking a parked car in a local reference frame in which it appears to be moving relative to the ego vehicle. We use this evaluation method to automatically quantitatively evaluate our tracking performance on 466 separate tracked vehicles. Our method obtains a mean absolute velocity error of 0.27 m/s and an RMS error of 0.47 m/s on this test set. We can also qualitatively evaluate our method by building color 3D car models from moving vehicles. We have thus demonstrated that our method can be used for precision car tracking with applications to autonomous driving and behavior modeling.

While inexpensive depth sensors are becoming increasingly ubiquitous, field of view and self-occlusion constraints limit the information a single sensor can provide. For many applications one may instead require a network of depth sensors, registered to a common world frame and synchronized in time. Historically such a setup has required a tedious manual calibration procedure, making it infeasible to deploy these networks in the wild, where spatial and temporal drift are common. In this work, we propose an entirely unsupervised procedure for calibrating the relative pose and time offsets of a pair of depth sensors. So doing, we make no use of an explicit calibration target, or any intentional activity on the part of a user. Rather, we use the unstructured motion of objects in the scene to find potential correspondences between the sensor pair. This yields a rough transform which is then refined with an occlusion-aware energy minimization. We compare our results against the standard checkerboard technique, and provide qualitative examples for scenes in which such a technique would be impossible.

Machine perception often requires a large amount of user-annotated data which is time-consuming, difficult, or expensive to collect. Perception systems should be easy to train by regular users, and this is currently far from the case. Our previous work, tracking-based semi-supervised learning [14], helped reduce the labeling burden by using tracking information to harvest new and useful training examples. However, [14] was designed for offline use; it assumed a fixed amount of unlabeled data and did not allow for corrections from users. In many practical robot perception scenarios we A) desire continuous learning over a long period of time, B) have a stream of unlabeled sensor data available rather than a fixed dataset, and C) are willing to periodically provide a small number of new training examples. In light of this, we present group induction, a new mathematical framework that rigorously encodes the intuition of [14] in an alternating optimization problem similar to expectation maximization (EM), but with the assumption that the unlabeled data comes in groups of instances that share the same hidden label. The mathematics suggest several improvements to the original heuristic algorithm, and make clear how to handle user interaction and streams of unlabeled data. We evaluate group induction on a track classification task from natural street scenes, demonstrating its ability to learn continuously, adapt to user feedback, and accurately recognize objects of interest.

We describe a method for 3D object scanning by aligning depth scans that were taken from around an object with a Time-of-Flight (ToF) camera. These ToF cameras can measure depth scans at video rate. Due to comparably simple technology, they bear potential for economical production in big volumes. Our easy-to-use, cost-effective scanning solution, which is based on such a sensor, could make 3D scanning technology more accessible to everyday users. The algorithmic challenge we face is that the sensor's level of random noise is substantial and there is a nontrivial systematic bias. In this paper, we show the surprising result that 3D scans of reasonable quality can also be obtained with a sensor of such low data quality. Established filtering and scan alignment techniques from the literature fail to achieve this goal. In contrast, our algorithm is based on a new combination of a 3D superresolution method with a probabilistic scan alignment approach that explicitly takes into account the sensor's noise characteristics.

We consider the problem of segmenting and tracking deformable objects in color video with depth (RGBD) data available from commodity sensors such as the Asus Xtion Pro Live or Microsoft Kinect. We frame this problem with very few assumptions-no prior object model, no stationary sensor, and no prior 3-D map-thus making a solution potentially useful for a large number of applications, including semi-supervised learning, 3-D model capture, and object recognition. Our approach makes use of a rich feature set, including local image appearance, depth discontinuities, optical flow, and surface normals to inform the segmentation decision in a conditional random field model. In contrast to previous work in this field, the proposed method learns how to best make use of these features from ground-truth segmented sequences. We provide qualitative and quantitative analyses which demonstrate substantial improvement over the state of the art. This paper is an extended version of our previous work. Building on our previous work, we show that it is possible to achieve an order of magnitude speedup and thus real-time performance ( ~ 20 FPS) on a laptop computer by applying simple algorithmic optimizations to the original work. This speedup comes at only a minor cost in overall accuracy and thus makes this approach applicable to a broader range of tasks. We demonstrate one such task: real-time, online, interactive segmentation to efficiently collect training data for an off-the-shelf object detector.

Detecting cars in real-world images is an important task for autonomous driving, yet it remains unsolved. The system described in this paper takes advantage of context and scale to build a monocular single-frame image-based car detector that significantly outperforms the baseline. The system uses a probabilistic model to combine multiple forms of evidence for both context and scale to locate cars in a real-world image. We also use scale filtering to speed up our algorithm by a factor of 3.3 compared to the baseline. By using a calibrated camera and localization on a road map, we are able to obtain context and scale information from a single image without the use of a 3D laser. The system outperforms the baseline by an absolute 9.4% in overall average precision and 11.7% in average precision for cars smaller than 50 pixels in height, for which context and scale cues are especially important.

We present a hand-held system for real-time, interactive acquisition of residential floor plans. The system integrates a commodity range camera, a micro-projector, and a button interface for user input and allows the user to freely move through a building to capture its important architectural elements. The system uses the Manhattan world assumption, which posits that wall layouts are rectilinear. This assumption allows generating floor plans in real time, enabling the operator to interactively guide the reconstruction process and to resolve structural ambiguities and errors during the acquisition. The interactive component aids users with no architectural training in acquiring wall layouts for their residences. We show a number of residential floor plans reconstructed with the system.

The widespread deployment of wireless networks presents an opportunity for localization and mapping using only signal-strength measurements. The current state of the art is to use Gaussian process latent variable models (GP-LVM). This method works well, but relies on a signature uniqueness assumption which limits its applicability to only signal-rich environments. Moreover, it does not scale computationally to large sets of data, requiring O (N3) operations per iteration. We present a GraphSLAM-like algorithm for signal strength SLAM. Our algorithm shares many of the benefits of Gaussian processes, yet is viable for a broader range of environments since it makes no signature uniqueness assumptions. It is also more tractable to larger map sizes, requiring O (N2) operations per iteration. We compare our algorithm to a laser-SLAM ground truth, showing it produces excellent results in practice.

Detection of traffic light state is essential for autonomous driving in cities. Currently, the only reliable systems for determining traffic light state information are non-passive proofs of concept, requiring explicit communication between a traffic signal and vehicle. Here, we present a passive camera based pipeline for traffic light state detection, using (imperfect) vehicle localization and assuming prior knowledge of traffic light location. First, we introduce a convenient technique for mapping traffic light locations from recorded video data using tracking, back-projection, and triangulation. In order to achieve robust real-time detection results in a variety of lighting conditions, we combine several probabilistic stages that explicitly account for the corresponding sources of sensor and data uncertainty. In addition, our approach is the first to account for multiple lights per intersection, which yields superior results by probabilistically combining evidence from all available lights. To evaluate the performance of our method, we present several results across a variety of lighting conditions in a real-world environment. The techniques described here have for the first time enabled our autonomous research vehicle to successfully navigate through traffic-light-controlled intersections in real traffic.

Object recognition is a critical next step for autonomous robots, but a solution to the problem has remained elusive. Prior 3D-sensor-based work largely classifies individual point cloud segments or uses class-specific trackers. In this paper, we take the approach of classifying the tracks of all visible objects. Our new track classification method, based on a mathematically principled method of combining log odds estimators, is fast enough for real time use, is non-specific to object class, and performs well (98.5% accuracy) on the task of classifying correctly-tracked, well-segmented objects into car, pedestrian, bicyclist, and background classes. We evaluate the classifier's performance using the Stanford Track Collection, a new dataset of about 1.3 million labeled point clouds in about 14,000 tracks recorded from an autonomous vehicle research platform. This dataset, which we make publicly available, contains tracks extracted from about one hour of 360-degree, 10Hz depth information recorded both while driving on busy campus streets and parked at busy intersections.

In order to achieve autonomous operation of a vehicle in urban situations with unpredictable traffic, several realtime systems must interoperate, including environment perception, localization, planning, and control. In addition, a robust vehicle platform with appropriate sensors, computational hardware, networking, and software infrastructure is essential. We previously published an overview of Junior, Stanford's entry in the 2007 DARPA Urban Challenge. This race was a closed-course competition which, while historic and inciting much progress in the field, was not fully representative of the situations that exist in the real world. In this paper, we present a summary of our recent research towards the goal of enabling safe and robust autonomous operation in more realistic situations. First, a trio of unsupervised algorithms automatically calibrates our 64-beam rotating LIDAR with accuracy superior to tedious hand measurements. We then generate high-resolution maps of the environment which are subsequently used for online localization with centimeter accuracy. Improved perception and recognition algorithms now enable Junior to track and classify obstacles as cyclists, pedestrians, and vehicles; traffic lights are detected as well. A new planning system uses this incoming data to generate thousands of candidate trajectories per second, choosing the optimal path dynamically. The improved controller continuously selects throttle, brake, and steering actuations that maximize comfort and minimize trajectory error. All of these algorithms work in sun or rain and during the day or night. With these systems operating together, Junior has successfully logged hundreds of miles of autonomous operation in a variety of real-life conditions.

We describe a method for 3D object scanning by aligning depth scans that were taken from around an object with a time-of-flight camera. These ToF cameras can measure depth scans at video rate. Due to comparably simple technology they bear potential for low cost production in big volumes. Our easy-to-use, cost-effective scanning solution based on such a sensor could make 3D scanning technology more accessible to everyday users. The algorithmic challenge we face is that the sensor's level of random noise is substantial and there is a non-trivial systematic bias. In this paper we show the surprising result that 3D scans of reasonable quality can also be obtained with a sensor of such low data quality. Established filtering and scan alignment techniques from the literature fail to achieve this goal. In contrast, our algorithm is based on a new combination of a 3D superresolution method with a probabilistic scan alignment approach that explicitly takes into account the sensor's noise characteristics.

We present a flexible method for fusing information from optical and range sensors based on an accelerated high-dimensional filtering approach. Our system takes as input a sequence of monocular camera images as well as a stream of sparse range measurements as obtained from a laser or other sensor system. In contrast with existing approaches, we do not assume that the depth and color data streams have the same data rates or that the observed scene is fully static. Our method produces a dense, high-resolution depth map of the scene, automatically generating confidence values for every interpolated depth point. We describe how to integrate priors on object motion and appearance and how to achieve an efficient implementation using parallel processing hardware such as GPUs.

Markerless tracking of human pose is a hard yet relevant problem. In this paper, we derive an efficient filtering algorithm for tracking human pose using a stream of monocular depth images. The key idea is to combine an accurate generative model - which is achievable in this setting using programmable graphics hardware - with a discriminative model that provides data-driven evidence about body part locations. In each filter iteration, we apply a form of local model-based search that exploits the nature of the kinematic chain. As fast movements and occlusion can disrupt the local search, we utilize a set of discriminatively trained patch classifiers to detect body parts. We describe a novel algorithm for propagating this noisy evidence about body part locations up the kinematic chain using the unscented transform. The resulting distribution of body configurations allows us to reinitialize the model-based search. We provide extensive experimental results on 28 real-world sequences using automatic ground-truth annotations from a commercial motion capture system.

We present an algorithm that learns invariant features from real data in an entirely unsupervised fashion. The principal benefit of our method is that it can be applied without human intervention to a particular application or data set, learning the specific invariances necessary for excellent feature performance on that data. Our algorithm relies on the ability to track image patches over time using optical flow. With the wide availability of high frame rate video (eg: on the web, from a robot), good tracking is straightforward to achieve. The algorithm then optimizes feature parameters such that patches corresponding to the same physical location have feature descriptors that are as similar as possible while simultaneously maximizing the distinctness of descriptors for different locations. Thus, our method captures data or application specific invariances yet does not require any manual supervision. We apply our algorithm to learn domain-optimized versions of SIFT and HOG. SIFT and HOG features are excellent and widely used. However, they are general and by definition not tailored to a specific domain. Our domain-optimized versions offer a substantial performance increase for classification and correspondence tasks we consider. Furthermore, we show that the features our method learns are near the optimal that would be achieved by directly optimizing the test set performance of a classifier. Finally, we demonstrate that the learning often allows fewer features to be used for some tasks, which has the potential to dramatically improve computational concerns for very large data sets.

A description of the Google Street View Project is provided, from the beginnings to its current state. Google Street View is perhaps the largest image data base ever collected. The goal of this project is to take panoramic images at every public place in this world and to make these images accessible through the Internet, so that people can "tele-port" themselves to anywhere, anytime. While the general idea behind Street View can be traced back to the MIT Movie Maps project that photographed major roads in Aspen in the late 1960s, a project of this sale and proportion has never been tried before. Many of the issues that arose in building up a fleet of vehicles, and processing endless pedabytes of image data in endless Google computer racks are reviewed. In addition, some of the implications of this new database, and key challenges in moving ahead are discussed.

We consider the task of accurately controlling a complex system, such as autonomously sliding a car sideways into a parking spot. Although certain regions of this domain are extremely hard to model (i.e., the dynamics of the car while skidding), we observe that in practice such systems are often remarkably deterministic over short periods of time, even in difficult-to-model regions. Motivated by this intuition, we develop a probabilistic method for combining closed-loop control in the well-modeled regions and open-loop control in the difficult-to-model regions. In particular, we show that by combining 1) an inaccurate model of the system and 2) a demonstration of the desired behavior, our approach can accurately and robustly control highly challenging systems, without the need to explicitly model the dynamics in the most complex regions and without the need to hand-tune the switching control law. We apply our approach to the task of autonomous sideways sliding into a parking spot, and show that we can repeatedly and accurately control the system, placing the car within about 2 feet of the desired location; to the best of our knowledge, this represents the state of the art in terms of accurately controlling a vehicle in such a maneuver.

Autonomous vehicle navigation in dynamic urban environments requires localization accuracy exceeding that available from GPS-based inertial guidance systems. We have shown previously that GPS, IMU, and LIDAR data can be used to generate a high-resolution infrared remittance ground map that can be subsequently used for localization. We now propose an extension to this approach that yields substantial improvements over previous work in vehicle localization, including higher precision, the ability to learn and improve maps over time, and increased robustness to environment changes and dynamic obstacles. Specifically, we model the environment, instead of as a spatial grid of fixed infrared remittance values, as a probabilistic grid whereby every cell is represented as its own gaussian distribution over remittance values. Subsequently, Bayesian inference is able to preferentially weight parts of the map most likely to be stationary and of consistent angular reflectivity, thereby reducing uncertainty and catastrophic errors. Furthermore, by using offline SLAM to align multiple passes of the same environment, possibly separated in time by days or even months, it is possible to build an increasingly robust understanding of the world that can be then exploited for localization. We validate the effectiveness of our approach by using these algorithms to localize our vehicle against probabilistic maps in various dynamic environments, achieving RMS accuracy in the 10cm-range and thus outperforming previous work. Importantly, this approach has enabled us to autonomously drive our vehicle for hundreds of miles in dense traffic on narrow urban roads which were formerly unnavigable with previous localization methods.

We deal with the problem of detecting and identifying body parts in depth images at video frame rates. Our solution involves a novel interest point detector for mesh and range data that is particularly well suited for analyzing human shape. The interest points, which are based on identifying geodesic extrema on the surface mesh, coincide with salient points of the body, which can be classified as, e.g., hand, foot or head using local shape descriptors. Our approach also provides a natural way of estimating a 3D orientation vector for a given interest point. This can be used to normalize the local shape descriptors to simplify the classification problem as well as to directly estimate the orientation of body parts in space. Experiments involving ground truth labels acquired via an active motion capture system show that our interest points in conjunction with a boosted patch classifier are significantly better in detecting body parts in depth images than state-of-the-art sliding-window based detectors.

Safe handling of dynamic highway and inner city scenarios with autonomous vehicles involves the problem of generating traffic-adapted trajectories. In order to account for the practical requirements of the holistic autonomous system, we propose a semi-reactive trajectory generation method, which can be tightly integrated into the behavioral layer. The method realizes long-term objectives such as velocity keeping, merging, following, stopping, in combination with a reactive collision avoidance by means of optimal-control strategies within the Frenét-Frame of the street. The capabilities of this approach are demonstrated in the simulation of a typical high-speed highway scenario.

The interpretation of uncertain sensor streams for localization is usually considered in the context of a robot. Increasingly, however, portable consumer electronic devices, such as smartphones, are equipped with sensors including WiFi radios, cameras, and inertial measurement units (IMUs). Many tasks typically associated with robots, such as localization, would be valuable to perform on such devices. In this paper, we present an approach for indoor localization exclusively using the low-cost sensors typically found on smartphones. Environment modification is not needed. We rigorously evaluate our method using ground truth acquired using a laser range scanner. Our evaluation includes overall accuracy and a comparison of the contribution of individual sensors. We find experimentally that fusion of multiple sensor modalities is necessary for optimal performance and demonstrate sub-meter localization accuracy.

We introduce gesture controllers , a method for animating the body language of avatars engaged in live spoken conversation. A gesture controller is an optimal-policy controller that schedules gesture animations in real time based on acoustic features in the user's speech. The controller consists of an inference layer, which infers a distribution over a set of hidden states from the speech signal, and a control layer, which selects the optimal motion based on the inferred state distribution. The inference layer, consisting of a specialized conditional random field, learns the hidden structure in body language style and associates it with acoustic features in speech. The control layer uses reinforcement learning to construct an optimal policy for selecting motion clips from a distribution over the learned hidden states. The modularity of the proposed method allows customization of a character's gesture repertoire, animation of non-human characters, and the use of additional inputs such as speech recognition or direct user control.

We present a new performance capture approach that incorporates a physically-based cloth model to reconstruct a rigged fully-animatable virtual double of a real person in loose apparel from multi-view video recordings. Our algorithm only requires a minimum of manual interaction. Without the use of optical markers in the scene, our algorithm first reconstructs skeleton motion and detailed time-varying surface geometry of a real person from a reference video sequence. These captured reference performance data are then analyzed to automatically identify non-rigidly deforming pieces of apparel on the animated geometry. For each piece of apparel, parameters of a physically-based real-time cloth simulation model are estimated, and surface geometry of occluded body regions is approximated. The reconstructed character model comprises a skeleton-based representation for the actual body parts and a physically-based simulation model for the apparel. In contrast to previous performance capture methods, we can now also create new real-time animations of actors captured in general apparel.

Depth maps captured with time-of-flight cameras have very low data quality: the image resolution is rather limited and the level of random noise contained in the depth maps is very high. Therefore, such flash lidars cannot be used out of the box for high-quality 3D object scanning. To solve this problem, we present LidarBoost, a 3D depth superresolution method that combines several low resolution noisy depth images of a static scene from slightly displaced viewpoints, and merges them into a high-resolution depth image. We have developed an optimization framework that uses a data fidelity term and a geometry prior term that is tailored to the specific characteristics of flash lidars. We demonstrate both visually and quantitatively that LidarBoost produces better results than previous methods from the literature.

We consider the problem of autonomous driving in semi-structured environments (e.g., parking lots). Such environments have strong topological structure (graphs of drivable lanes), but maneuvers with significant deviations from those graphs are valid and frequent. We address two main challenges of operating in such environments: i) detection of topological structure from sensor data, and ii) using that structure to guide path planning. We present experimental results on both of these topics, demonstrating robust estimation of lane networks in parking lots and the benefits of using these topological networks to guide path planning.

Recently, the problem of autonomous navigation of automobiles has gained substantial interest in the robotics community. Especially during the two recent DARPA grand challenges, autonomous cars have been shown to robustly navigate over extended periods of time through complex desert courses or through dynamic urban traffic environments. In these tasks, the robots typically relied on GPS traces to follow pre-defined trajectories so that only local planners were required. In this paper, we present an approach for autonomous navigation of cars in indoor structures such as parking garages. Our approach utilizes multi-level surface maps of the corresponding environments to calculate the path of the vehicle and to localize it based on laser data in the absence of sufficiently accurate GPS information. It furthermore utilizes a local path planner for controlling the vehicle. In a practical experiment carried out with an autonomous car in a real parking garage we demonstrate that our approach allows the car to autonomously park itself in a large-scale multi-level structure.

This paper describes a new passive approach to capture time-varying scene geometry in large acquisition volumes from multi-view video. It can be applied to reconstruct complete moving models of human actors that feature even slightest dynamic geometry detail, such as wrinkles and folds in clothing, and that can be viewed from 360deg. Starting from multi-view video streams recorded under calibrated lighting, we first perform marker-less human motion capture based on a smooth template with no high-frequency surface detail. Subsequently, surface reflectance and time-varying normal fields are estimated based on the coarse template shape. The main contribution of this paper is a new statistical approach to solve the non-trivial problem of transforming the captured normal field that is defined over the smooth non-planar 3D template into true 3D displacements. Our spatio-temporal reconstruction method outputs displaced geometry that is accurate at each time step of video and temporally smooth, even if the input data are affected by noise.

We present a dense 3D correspondence finding method that enables spatio-temporally coherent reconstruction of surface animations from multi-view video data. Given as input a sequence of shape-from-silhouette volumes of a moving subject that were reconstructed for each time frame individually, our method establishes dense surface correspondences between subsequent shapes independently of surface discretization. This is achieved in two steps: first, we obtain sparse correspondences from robust optical features between adjacent frames. Second, we generate dense correspondences which serve as map between respective surfaces. By applying this procedure subsequently to all pairs of time steps we can trivially align one shape with all others. Thus, the original input can be reconstructed as a sequence of meshes with constant connectivity and small tangential distortion. We exemplify the performance and accuracy of our method using several synthetic and captured real-world sequences.

Motion and path-planning algorithms often use complex cost functions for both global navigation and local smoothing of trajectories. Obtaining good results typically requires carefully hand-engineering the trade-offs between different terms in the cost function. In practice, it is often much easier to demonstrate a few good trajectories. In this paper, we describe an efficient algorithm which - when given access to a few trajectory demonstrations - can automatically infer good trade-offs between the different costs. In our experiments, we apply our algorithm to the problem of navigating a robotic car in a parking lot.

Abstract Recently, it has become increasingly popular to represent animations not by means of a classical skeleton‐based model, but in the form of deforming mesh sequences. The reason for this new trend is that novel mesh deformation methods as well as new surface based scene capture techniques offer a great level of flexibility during animation creation. Unfortunately, the resulting scene representation is less compact than skeletal ones and there is not yet a rich toolbox available which enables easy post‐processing and modification of mesh animations. To bridge this gap between the mesh‐based and the skeletal paradigm, we propose a new method that automatically extracts a plausible kinematic skeleton, skeletal motion parameters, as well as surface skinning weights from arbitrary mesh animations. By this means, deforming mesh sequences can be fully‐automatically transformed into fullyrigged virtual subjects. The original input can then be quickly rendered based on the new compact bone and skin representation, and it can be easily modified using the full repertoire of already existing animation tools.

This paper proposes a new marker-less approach to capturing human performances from multi-view video. Our algorithm can jointly reconstruct spatio-temporally coherent geometry, motion and textural surface appearance of actors that perform complex and rapid moves. Furthermore, since our algorithm is purely meshbased and makes as few as possible prior assumptions about the type of subject being tracked, it can even capture performances of people wearing wide apparel, such as a dancer wearing a skirt. To serve this purpose our method efficiently and effectively combines the power of surface- and volume-based shape deformation techniques with a new mesh-based analysis-through-synthesis framework. This framework extracts motion constraints from video and makes the laser-scan of the tracked subject mimic the recorded performance. Also small-scale time-varying shape detail is recovered by applying model-guided multi-view stereo to refine the model surface. Our method delivers captured performance data at high level of detail, is highly versatile, and is applicable to many complex types of scenes that could not be handled by alternative marker-based or marker-free recording techniques.

As probabilistic computations play an increasing role in solving various problems, researchers have designed probabilistic languages which treat probability distributions as primitive datatypes. Most probabilistic languages, however, focus only on discrete distributions and have limited expressive power. This article presents a probabilistic language, called λ ○ , whose expressive power is beyond discrete distributions. Rich expressiveness of λ ○ is due to its use of sampling functions , that is, mappings from the unit interval (0.0,1.0] to probability domains, in specifying probability distributions. As such, λ ○ enables programmers to formally express and reason about sampling methods developed in simulation theory. The use of λ ○ is demonstrated with three applications in robotics: robot localization, people tracking, and robotic mapping. All experiments have been carried out with real robots.

Traditional image mosaicing usually relies on rigid image transformations. In many medical applications, however, tissue deformation during image acquisition or 3D parallax effects may require nonrigid transformations in the mosaicing process. This paper presents a new method that integrates deformable surface models into the image mosaicing algorithms. Our approach has two main contributions. First, we present a global alignment algorithm to efficiently deal with accumulated image registration errors. Second, we introduce a local alignment algorithm to accommodate local scene deformations. These two problems are integrated into a single optimization problem that simultaneously recovers the motion of the camera as well as the structure of the scene. Our approach is demonstrated on simulations, images from a hand-held digital camera, and microscopic images acquired with a micro-endoscope.

We consider the problem of autonomously estimating position and orientation of an object from tactile data. When initial uncertainty is high, estimation of all six parameters precisely is computationally expensive. We propose an efficient Bayesian approach that is able to estimate all six parameters in both unimodal and multimodal scenarios. The approach is termed scaling series sampling as it estimates the solution region by samples. It performs the search using a series of successive refinements, gradually scaling the precision from low to high. Our approach can be applied to a wide range of manipulation tasks. We demonstrate its portability on two applications: (1) manipulating a box and (2) grasping a door handle

Summary form only given. The DARPA Grand Challenge was the most significant event in the field of robotics in more than a decade. A mobile ground robot had to traverse 132 miles of punishing desert terrain in less than ten hours. In 2004, the best robot only made 7.3 miles. A year later, Stanford won this historical challenge and cashed the } prize. This talk, delivered by the leader of the Stanford Racing Team, will provide insights into the software architecture of Stanford's winning robot "Stanley." The robot heavily relied on advanced artificial intelligence, and it used a pipelining architecture to turn sensor data into vehicle controls. The talk will introduce the audience into the fascinating world of autonomous robotics, share many of the race insights, and discuss some of the implications for the future of our society

The Partially Observable Markov Decision Process has long been recognized as a rich framework for real-world planning and control problems, especially in robotics. However exact solutions in this framework are typically computationally intractable for all but the smallest problems. A well-known technique for speeding up POMDP solving involves performing value backups at specific belief points, rather than over the entire belief simplex. The efficiency of this approach, however, depends greatly on the selection of points. This paper presents a set of novel techniques for selecting informative belief points which work well in practice. The point selection procedure is combined with point-based value backups to form an effective anytime POMDP algorithm called Point-Based Value Iteration (PBVI). The first aim of this paper is to introduce this algorithm and present a theoretical analysis justifying the choice of belief selection technique. The second aim of this paper is to provide a thorough empirical comparison between PBVI and other state-of-the-art POMDP methods, in particular the Perseus algorithm, in an effort to highlight their similarities and differences. Evaluation is performed using both standard POMDP domains and realistic robotic tasks.

We present a Bayesian technique for the reconstruction and subsequent decimation of 3D surface models from noisy sensor data. The method uses oriented probabilistic models of the measurement noise and combines them with feature-enhancing prior probabilities over 3D surfaces. When applied to surface reconstruction, the method simultaneously smooths noisy regions while enhancing features such as corners. When applied to surface decimation, it finds models that closely approximate the original mesh when rendered. The method is applied in the context of computer animation where it finds decimations that minimize the visual error even under nonrigid deformations.

We describe a technique for reconstructing probable occluded surfaces from 3D range images. The technique exploits the fact that many objects possess shape symmetries that can be recognized even from partial 3D views. Our approach identifies probable symmetries and uses them to attend the partial 3D shape model into the occluded space. To accommodate objects consisting of multiple parts, we describe a technique for segmenting objects into parts characterized by different symmetries. Results are provided for a real-world database of 3D range images of common objects, acquired through an active stereo rig

In the real world, noisy sensors and limited communication make it difficult for robot teams to coordinate in tightly coupled tasks. Team members cannot simply apply single-robot solution techniques for partially observable problems in parallel because they do not take into account the recursive effect that reasoning about the beliefs of others has on policy generation. Instead, we must turn to a game theoretic approach to model the problem correctly. Partially observable stochastic games (POSGs) provide a solution model for decentralized robot teams, however, this model quickly becomes intractable. In previous work we presented an algorithm for lookahead search in POSGs. Here we present an extension which reduces computation during lookahead by clustering similar observation histories together. We show that by clustering histories which have similar profiles of predicted reward, we can greatly reduce the computation time required to solve a POSG while maintaining a good approximation to the optimal policy. We demonstrate the power of the clustering algorithm in a real-time robot controller as well as for a simple benchmark problem.

We present a method for learning activity-based ground models based on a multiple particle filter approach to motion tracking in video acquired from a moving aerial platform. Such models offer a number of potential benefits. In this paper we demonstrate the ability of activity-based models to improve the performance of an object motion tracker as well as their applicability to global registration of video sequences.

In this paper we propose an active control strategy for scanning laser sensors on autonomous vehicles traveling offroad at high speeds. As speed increases the amount of sensor information about the terrain decreases. We address the problem of sensor control in the context of this speed-coverage trade off. The algorithm and testing methodologies are described with results comparing our active sensing method to a passive sensing method.

Maintaining the identities of moving objects is an important aspect of most multi-object tracking applications. Uncertainty in sensor data, coupled with the intrinsic combinatorial difficulty of the data association problem, suggests probabilistic formulations over the set of possible identities. While an explicit representation of a distribution over all associations may require exponential storage and computation, in practice the information provided by this distribution is accessed only in certain stylized ways, as when asking for the identity of a given track, or the track with a given identity. Exploiting this observation, we proposed a practical solution to this problem based on maintaining marginal probabilities and demonstrated its effectiveness in the context of tracking within a wireless sensor network. That method, unfortunately, requires extensive communication in the network whenever new identity observations are made, in order for normalization operations to keep the marginals consistent. In this paper, we have proposed a very different solution based on accumulated log-likelihoods that can postpone all normalization computations until actual identity queries are made. In this manner the continuous communication and computational expense of repeated normalizations is avoided and that effort is expended only when actual queries are made of the network. We compare the two methods in terms of their computational complexities, inference accuracies, and distributed implementations. Simulation and experimental results from a RFID system are also presented.

This paper describes a highly successful application of MRFs to the problem of generating high-resolution range images. A new generation of range sensors combines the capture of low-resolution range images with the acquisition of registered high-resolution camera images. The MRF in this paper exploits the fact that discontinuities in range and coloring tend to co-align. This enables it to generate high-resolution, low-noise range images by integrating regular camera images into the range data. We show that by using such an MRF, we can substantially improve over existing range imaging technology.

This paper presents a new filter for online data association problems in high-dimensional spaces. The key innovation is a representation of the data association posterior in information form, in which the “proxim- ity” of objects and tracks are expressed by numerical links. Updating these links requires linear time, compared to exponential time required for computing the exact posterior probabilities. The paper derives the algorithm formally and provides comparative results using data obtained by a real-world camera array and by a large-scale sensor network simu- lation.

We consider the problem of localizing a set of microphones together with a set of external acoustic events (e.g., hand claps), emitted at un- known times and unknown locations. We propose a solution that ap- proximates this problem under a far field approximation defined in the calculus of affine geometry, and that relies on singular value decompo- sition (SVD) to recover the affine structure of the problem. We then define low-dimensional optimization techniques for embedding the solu- tion into Euclidean geometry, and further techniques for recovering the locations and emission times of the acoustic events. The approach is use- ful for the calibration of ad-hoc microphone arrays and sensor networks.

As probabilistic computations play an increasing role in solving various problems, researchers have designed probabilistic languages that treat probability distributions as primitive datatypes. Most probabilistic languages, however, focus only on discrete distributions and have limited expressive power. In this paper, we present a probabilistic language, called λο, which uniformly supports all kinds of probability distributions -- discrete distributions, continuous distributions, and even those belonging to neither group. Its mathematical basis is sampling functions, i.e., mappings from the unit interval (0.0,1.0] to probability domains.We also briefly describe the implementation of λο as an extension of Objective CAML and demonstrate its practicality with three applications in robotics: robot localization, people tracking, and robotic mapping. All experiments have been carried out with real robots.

Standard value function approaches to finding policies for Partially Observable Markov Decision Processes (POMDPs) are generally considered to be intractable for large models. The intractability of these algorithms is to a large extent a consequence of computing an exact, optimal policy over the entire belief space. However, in real-world POMDP problems, computing the optimal policy for the full belief space is often unnecessary for good control even for problems with complicated policy classes. The beliefs experienced by the controller often lie near a structured, low-dimensional subspace embedded in the high-dimensional belief space. Finding a good approximation to the optimal value function for only this subspace can be much easier than computing the full value function. We introduce a new method for solving large-scale POMDPs by reducing the dimensionality of the belief space. We use Exponential family Principal Components Analysis (Collins, Dasgupta & Schapire, 2002) to represent sparse, high-dimensional belief spaces using small sets of learned features of the belief state. We then plan only in terms of the low-dimensional belief features. By planning in this low-dimensional space, we can find policies for POMDP models that are orders of magnitude larger than models that can be handled by conventional techniques. We demonstrate the use of this algorithm on a synthetic problem and on mobile robot navigation tasks.

We introduce the SCAPE method (Shape Completion and Animation for PEople)---a data-driven method for building a human shape model that spans variation in both subject shape and pose. The method is based on a representation that incorporates both articulated and non-rigid deformations. We learn a pose deformation model that derives the non-rigid surface deformation as a function of the pose of the articulated skeleton. We also learn a separate model of variation based on body shape. Our two models can be combined to produce 3D surface models with realistic muscle deformation for different people in different poses, when neither appear in the training set. We show how the model can be used for shape completion --- generating a complete surface mesh given a limited set of markers specifying the target shape. We present applications of shape completion to partial view completion and motion capture animation. In particular, our method is capable of constructing a high-quality animated surface model of a moving person, with realistic muscle deformation, using just a single static scan and a marker motion capture sequence of the person.

Predictive state representations (PSRs) have recently been proposed as an alternative to partially observable Markov decision processes (POMDPs) for representing the state of a dynamical system (Littman et al., 2001). We present a learning algorithm that learns a PSR from observational data. Our algorithm produces a variant of PSRs called transformed predictive state representations (TPSRs). We provide an efficient principal-components-based algorithm for learning a TPSR, and show that TPSRs can perform well in comparison to Hidden Markov Models learned with Baum-Welch in a real world robot tracking task for low dimensional representations and long prediction horizons.

We describe a probabilistic framework for detection and modeling of doors from sensor data acquired in corridor environments with mobile robots. The framework captures shape, color, and motion properties of door and wall objects. The probabilistic model is optimized with a version of the expectation maximization algorithm, which segments the environment into door and wall objects and learns their properties. The framework allows the robot to generalize the properties of detected object instances to new object instances. We demonstrate the algorithm on real-world data acquired by a Pioneer robot equipped with a laser range finder and an omni-directional camera. Our results show that our algorithm reliably segments the environment into walls and doors, finding both doors that move and doors that do not move. We show that our approach achieves better results than models that only capture behavior, or only capture appearance.

Unknown, unexplored and abandoned subterranean voids threaten mining operations, surface developments and the environment. Hazards within these spaces preclude human access to create and verify extensive maps or to characterize and analyze the environment. To that end, we have developed a mobile robot capable of autonomously exploring and mapping abandoned mines. To operate without communications in a harsh environment with little chance of rescue, this robot must have a robust electro-mechanical platform, a reliable software system, and a dependable means of failure recovery. Presented are the mechanisms, algorithms, and analysis tools that enable autonomous mine exploration and mapping along with extensive experimental results from eight successful deployments into the abandoned Mathies coal mine near Pittsburgh, PA.

We describe a heuristic search algorithm for generating optimal plans in a new class of decision problem, characterised by the incorporation of hidden state. The approach exploits the nature of the hidden state to reduce the state space by orders of magnitude. It then interleaves heuristic expansion of the reduced space with forwards and backwards propagation phases to produce a solution in a fraction of the time required by other techniques. Results are provided on an outdoor path planning application.

We present a robotic walking aid capable of learning models of users' walking-related activities. Our walker is instrumented to provide guidance to elderly people when navigating their environments; however, such guidance is difficult to provide without knowing what activity a person is engaged in (e.g., where a person wants to go). The main contribution of this paper is an algorithm for learning models of users of the walker. These models are defined at multiple levels of abstractions, and learned from actual usage data using statistical techniques. We demonstrate that our approach succeeds in determining the specific activity in which a user engages when using the walker. One of our proto-type walkers was tested in an assisted living facility near Pittsburgh, PA; a more recent model was extensively evaluated in a university environment.

To create with an autonomous mobile robot a 3D volumetric map of a scene it is necessary to gage several 3D scans and to merge them into one consistent 3D model. This paper provides a new solution to the simultaneous localization and mapping (SLAM) problem with six degrees of freedom. Robot motion on natural surfaces has to cope with yaw, pitch and roll angles, turning pose estimation into a problem in six mathematical dimensions. A fast variant of the Iterative Closest Points algorithm registers the 3D scans in a common coordinate system and relocalizes the robot. Finally, consistent 3D maps are generated using a global relaxation. The algorithms have been tested with 3D scans taken in the Mathies mine, Pittsburgh, PA. Abandoned mines pose significant problems to society, yet a large fraction of them lack accurate 3D maps.

We present a methodology for a sensor network to answer queries with limited and stochastic information using probabilistic techniques. This capability is useful in that it allows sensor networks to answer queries effectively even when present information is partially corrupt and when more information is unavailable or too costly to obtain. We use a Bayesian network to model the sensor network and Markov Chain Monte Carlo sampling to perform approximate inference. We demonstrate our technique on the specific problem of determining whether a friendly agent is surrounded by enemy agents and present simulation results for it.

Sensor networks present the opportunity to accurately localize the phenomena of interest. To be able to do so however, sensor nodes, need themselves to be accurately localized. We present an algorithm to do this based on uncontrolled environmental sounds observed by each of the sensor nodes. A probabilistic generative model is presented and it is shown that the sensor node localization problem is equivalent to maximum likelihood estimation in the model. Experimental results are presented for both simulated sensor nodes and Crossbow MICA2 sensor nodes.

We present an algorithm for creating globally consistent three-dimensional maps from depth fields produced by camera-based range measurement systems. Our approach is specifically suited to dealing with the high noise levels and the large number of outliers often produced by such systems. Range data is filtered to reject outliers within each scan. The point-to-plane variant of ICP is used for local alignment, including weightings that favor nearby points and a novel outlier rejection strategy that increases the robustness for this class of data while eliminating the burden of user-specified thresholds. Global consistency is imposed on cycles by optimally distributing the cyclic discrepancy according to the local fit correlation matrices. The algorithm is demonstrated on a dataset collected by an active unstructured-light space-time stereo vision system.

We present an unsupervised algorithm for registering 3D surface scans of an object undergoing significant deformations. Our algorithm does not need markers, nor does it assume prior knowledge about object shape, the dynamics of its deformation, or scan alignment. The algorithm registers two meshes by optimizing a joint probabilistic model over all point-to- point correspondences between them. This model enforces preservation of local mesh geometry, as well as more global constraints that capture the preservation of geodesic distance between corresponding point pairs. The algorithm applies even when one of the meshes is an incomplete range scan; thus, it can be used to automatically fill in the remaining sur- faces for this partial scan, even if those surfaces were previously only seen in a different configuration. We evaluate the algorithm on several real-world datasets, where we demonstrate good results in the presence of significant movement of articulated parts and non-rigid surface defor- mation. Finally, we show that the output of the algorithm can be used for compelling computer graphics tasks such as interpolation between two scans of a non-rigid object and automatic recovery of articulated object models. 1 Introduction The construction of 3D object models is a key task for many graphics applications. It is becoming increasingly common to acquire these models from a range scan of a physical object. This paper deals with an important subproblem of this acquisition task -- the problem of registering two deforming surfaces corresponding to different configurations of the same non-rigid object. The main difficulty in the 3D registration problem is determining the correspondences of points on one surface to points on the other. Local regions on the surface are rarely distinc- tive enough to determine the correct correspondence, whether because of noise in the scans, or because of symmetries in the object shape. Thus, the set of candidate correspondences to a given point is usually large. Determining the correspondence for all object points results in a combinatorially large search problem. The existing algorithms for deformable surface A results video is available at http://robotics.stanford.edu/drago/cc/video.mp4 Figure 1: A) Registration results for two meshes. Nonrigid ICP and its variant augmented with spin images get stuck in local maxima. Our CC algorithm produces a largely correct registration, although with an artifact in the right shoulder (inset). B) Illustration of the link deformation process C) The CC algorithm which uses only deformation potentials can violate mesh geometry. Near regions can map to far ones (segment AB) and far regions can map to near ones (points C,D). registration make the problem tractable by assuming significant prior knowledge about the objects being registered. Some rely on the presence of markers on the object [1, 20], while others assume prior knowledge about the object dynamics [16], or about the space of non- rigid deformations [15, 5]. Algorithms that make neither restriction [18, 12] simplify the problem by decorrelating the choice of correspondences for the different points in the scan. However, this approximation is only good in the case when the object deformation is small; otherwise, it results in poor local maxima as nearby points in one scan are allowed to map to far-away points in the other. Our algorithm defines a joint probabilistic model over all correspondences, which ex- plicitly model the correlations between them -- specifically, that nearby points in one mesh should map to nearby points in the other. Importantly, the notion of "nearby" used in our model is defined in terms of geodesic distance over the mesh. We define a probabilistic model over the set of correspondences, that encodes these geodesic distance constraints as well as penalties for link twisting and stretching, and high-level local surface features [14]. We then apply loopy belief propagation [21] to this model, in order to solve for the entire set of correspondences simultaneously. The result is a registration that respects the surface geometry. To the best of our knowledge, the algorithm we present in this paper is the first algorithm which allows the registration of 3D surfaces of an object where the object config- urations can vary significantly, there is no prior knowledge about object shape or dynamics of deformation, and nothing whatsoever is known about the object alignment. Moreover, unlike many methods, our algorithm can be used to register a partial scan to a complete model, greatly increasing its applicability. We apply our approach to three datasets containing 3D scans of a wooden puppet, a human arm and entire human bodies in different configurations. We demonstrate good registration results for scan pairs exhibiting articulated motion, non-rigid deformations, or both. We also describe three applications of our method. In our first application, we show how a partial scan of an object can be registered onto a fully specified model in a dif- ferent configuration. The resulting registration allows us to use the model to "complete" the partial scan in a way that preserves the local surface geometry. In the second, we use the correspondences found by our algorithm to smoothly interpolate between two different poses of an object. In our final application, we use a set of registered scans of the same object in different positions to recover a decomposition of the object into approximately rigid parts, and recover an articulated skeleton linking the parts. All of these applications are done in an unsupervised way, using only the output of our Correlated Correspondence algorithm applied to pairs of poses with widely varying deformations, and unknown initial alignments. These results demonstrate the value of a high-quality solution to the registra- tion problem to a range of graphics tasks. 2 Previous Work Surface registration is a fundamental building block in computer graphics. The classical so- lution for registering rigid surfaces is the Iterative Closest Point algorithm (ICP) [4, 6, 17]. Recently, there has been work extending ICP to non-rigid surfaces [18, 8, 12, 1]. These algorithms treat one of the scans (usually a complete model of the surface) as a deformable template. The links between adjacent points on the surface can be thought of as springs, which are allowed to deform at a cost. Similarly to ICP, these algorithms iterate between two subproblems -- estimating the non-rigid transformation and estimating the set of correspondences C between the scans. The step estimating the correspondences assumes that a good estimate of the nonrigid transformation is available. Under this assumption, the assignments to the correspondence variables become decorrelated: each point in the second scan is associated with the nearest point (in the Euclidean distance sense) in the deformed template scan. However, the decomposition also induces the algorithm's main limitation. By assigning points in the second scan to points on the deformed model inde- pendently, nearby points in the scan can get associated to remote points in the model if the estimate of is poor (Fig. 1A). While several approaches have been proposed to address this problem of incorrect correspondences, their applicability is largely limited to problems where the deformation is local, and the initial alignment is approximately correct. Another line of related work is the work on deformable template matching in the com- puter vision community. In the 3D case, this framework is used for detection of articulated object models in images [13, 22, 19]. The algorithms assume the decomposition of the object into a relatively small number of parts is known, and that a detector for each object part is available. Template matching approaches have also been applied to deformable 2D objects, where very efficient solutions exist [9, 11]. However, these methods do not extend easily to the case of 3D surfaces. 3 The Correlated Correspondence Algorithm The input to the algorithm is a set of two meshes (surfaces tessellated into polygons). The model mesh X = (V X , EX ) is a complete model of the object, in a particular pose. V X = (x1, . . . , xN ) denotes the mesh points, while EX is the set of links between adjacent points on the mesh surface. The data mesh Z = (V Z , EZ ) is either a complete model or a partial view of the object in a different configuration. Each data mesh point zk is associated with a correspondence variable ck, specifying the corresponding model mesh point. The task of registration is one of estimating the set of all correspondences C and a non-rigid transformation which aligns the corresponding points. 3.1 Probabilistic Model We formulate the registration problem as one of finding an embedding of the data mesh Z into the model mesh X, which is encoded as an assignment to all correspondence vari- ables C = (c1, . . . , cK ). The main idea behind our approach is to preserve the consis- tency of the embedding by explicitly correlating the assignments to the correspondence variables. We define a joint distribution over the correspondence variables c1, . . . , cK , rep- resented as a Markov network. For each pair of adjacent data mesh points zk, zl, we want to define a probabilistic potential (ck, cl) that constrains this pair of correspondences to reasonable and consistent. This gives rise to a joint probability distribution of the form p(C) = 1 (c (c Z k k ) k,l k , cl) which contains only single and pairwise potentials. Performing probabilistic inference to find the most likely joint assignment to the entire set of correspondence variables C should yield a good and consistent registration. Deformation Potentials. We want our model to encode a preference for embeddings of mesh Z into mesh X, which minimize the amount of deformation induced by the embedding. In order to quantify the amount of deformation , applied to the model, we will follow the ideas of Hahnel et al. [12] and treat the links in the set EX as springs, which resist stretching and twisting at their endpoints. Stretching is easily quantified by looking at changes in the link length induced by the transformation . Link twisting, however, is ill- specified by looking only at the Cartesian coordinates of the points alone. Following [12], we attach an imaginary local coordinate system to each point on the model. This local coordinate system allows us to quantify the "twist" of a point xj relative to a neighbor xi. A non-rigid transformation defines, for each point xi, a translation of its coordinates and a rotation of its local coordinate system. To evaluate the deformation penalty, we parameterize each link in the model in terms of its length and its direction relative to its endpoints (see Fig. 1B). Specifically, we define li,j to be the distance between xi and xj; dij is a unit vector denoting the direction of the point xj in the coordinate system of xi (and vice versa). We use ei,j to denote the set of edge parameters (li,j, dij, dji). It is now straightforward to specify the penalty for model deformations. Let be a transformation, and let ~ ei,j denote the triple of parameters associated with the link between xi and xj after applying . Our model penalizes twisting and stretching, using a separate zero-mean Gaussian noise model for each: P (~ ei,j | ei,j) = P (~li,j | li,j) P ( ~ dij | dij) P ( ~ dji | dji) (1) In the absence of prior information, we assume that all links are equally likely to deform. In order to quantify the deformation induced by an embedding C, we need to include a potential d(ck, cl) for each link eZ EZ . Every probability k,l d(ck = i, cl = j) corresponds to the deformation penalty incurred by deforming model link ei,j to generate link eZ and is defined in (1). We do not restrict ourselves to the set of links in EX , since k,l the original mesh tessellation is sparse and local. Any two points in X are allowed to implicitly define a link. Unfortunately, we cannot directly estimate the quantity P (eZ | e k,l i,j ), since the link pa- rameters eZ depend on knowing the nonrigid transformation, which is not given as part k,l of the input. The key issue is estimating the (unknown) relative rotation of the link end- points. In effect, this rotation is an additional latent variable, which must also be part of the probabilistic model. To remain within the realm of discrete Markov networks, allowing the application of standard probabilistic inference algorithms, we discretize the space of the possible rotations, and fold it into the domains of the correspondence variables. For each possible value of the correspondence variable ck = i we select a small set of candidate rotations, consistent with local geometry. We do this by aligning local patches around the points xi and zk using rigid ICP. We extend the domain of each correspondence variables ck, where each value encodes a matching point and a particular rotation from the precom- puted set for that point. Now the edge parameters eZ are fully determined and so is the k,l probabilistic potential. Geodesic Distances. Our proposed approach raises the question as to what constitutes the best constraint between neighboring correspondence variables. The literature on scan registration -- for rigid and non-rigid models alike -- relies on the preserving Euclidean distance. While Euclidean distance is meaningful for rigid objects, it is very sensitive to de- formations, especially those induced by moving parts. For example, in Fig. 1C, we see that the two legs in one configuration of our puppet are fairly close together, allowing the algo- rithm to map two adjacent points in the data mesh to the two separate legs, with minimal deformation penalty. In the complementary situation, especially when object symmetries are present, two distant yet similar points in one scan might get mapped to the same region in the other. For example, in the same figure, we see that points in both an arm and a leg in the data mesh get mapped to a single leg in the model mesh. We therefore want to enforce constraints preserving distance along the mesh surface (geodesic distance). Our probabilistic framework easily incorporate such constraints as correlations between pairs of correspondence variables. We encode a nearness preservation Figure 2: A) Automatic interpolation between two scans of an arm and a wooden puppet. B) Regis- tration results on two scans of the same man sitting and standing up (select points were displayed) C) Registration results on scans of a larger man and a smaller woman. The algorithm is robust to small changes in object scale. constraint which prevents adjacent points in mesh Z to be mapped to distant points in X in the geodesic distance sense. For adjacent points zk, zl in the data mesh, we define the following potential: 0 dist Geodesic (xi, xj ) > n(ck = i, cl = j) = (2) 1 otherwise where is the data mesh resolution and is some constant, chosen to be 3.5. The farness preservation potentials encode the complementary constraint. For every pair of points zk, zl whose geodesic distance is more than 5 on the data mesh, we have a potential: 0 dist Geodesic(xi, xj ) < f (ck = i, cl = j) = (3) 1 otherwise where is also a constant, chosen to be 2 in our implementation. The intuition behind this constraint is fairly clear: if zk, zl are far apart on the data mesh, then their corresponding points must be far apart on the model mesh. Local Surface Signatures. Finally, we encode a set of potentials that correspond to the preservation of local surface properties between the model mesh and data mesh. The use of local surface signatures is important, because it helps to guide the optimization in the exponential space of assignments. We use spin images [14] compressed with prin- cipal component analysis to produce a low-dimensional signature sx of the local surface geometry around a point x. When data and model points correspond, we expect their lo- cal signatures to be similar. We introduce a potential whose values s(ck) = i enforce a zero-mean Gaussian penalty for discrepancies between sx and s . i zk 3.2 Optimization In the previous section, we defined a Markov network, which encodes a joint probability distribution over the correspondence variables as a product of single and pairwise poten- tials. Our goal is to find a joint assignment to these variables that maximizes this proba- bility. This problem is one of standard probabilistic inference over the Markov network. However, the Markov network is quite large, and contains a large number of loops, so that exact inference is computationally infeasible. We therefore apply an approximate inference method known as loopy belief propagation (LBP)[21], which has been shown to work in a wide variety of applications. Running LBP until convergence results in a set of probabilis- tic assignments to the different correspondence variables, which are locally consistent. We then simply extract the most likely assignment for each variable to obtain a correspondence. One remaining complication arises from the form of our farness preservation constraints. In general, most pairs of points in the mesh are not close, so that the total number of such potentials grows as O(M 2), where M is the number of points in the data mesh. However, rather than introducing all these potentials into the Markov net from the start, we introduce them as needed. First, we run LBP without any farness preservation potentials. If the solution violates a set of farness preservation constraints, we add it and rerun BP. In practice, this approach adds a very small number of such constraints. 4 Experimental Results Basic Registration. We applied our registration algorithm to three different datasets, containing meshes of a human arm, wooden puppet and the CAESAR dataset of whole human bodies [1], all acquired by a 3D range scanner. The meshes were not complete surfaces, but several techniques exist for filling the holes (e.g., [10]). We ran the Correlated Correspondence algorithm using the same probabilistic model and the same parameters on all data sets. We use a coarse-to-fine strategy, using the result of a coarse sub-sampling of the mesh surface to constrain the correspondences at a finer-grained level. The resulting set of correspondences were used as markers to initialize the non-rigid ICP algorithm of Hahnel et al. [12]. The Correlated Correspondence algorithm successfully aligned all mesh pairs in our hu- man arm data set containing 7 arms. In the puppet data set we registered one of the meshes to the remaining 6 puppets. The algorithm correctly registered 4 out of 6 data meshes to the model mesh. In the two remaining cases, the algorithm produced a registration where the torso was flipped, so that the front was mapped to the back. This problem arises from am- biguities induced by the puppet symmetry, whose front and back are almost identical. Im- portantly, our probabilistic model assigns a higher likelihood score to the correct solution, so that the incorrect registration is a consequence of local maxima in the LBP algorithm. This fact allows us to address the issue in an unsupervised way simply by running loopy BP several times, with different initialization. For details on the unsupervised initialization scheme we used, please refer to our technical report [2]. We ran the modified algorithm to register one puppet mesh to the remaining 6 meshes in the dataset, obtaining the correct registration in all cases. In particular, as shown in Fig. 1A, we successfully deal with the case on which the straightforward nonrigid ICP algorithm failed. The modified algorithm was applied to the CAESAR dataset and produced very good registration for challenging cases exhibiting both articulated motion and deformation (Fig. 2B), or exhibiting deforma- tion and a (small) change in object scale (Fig. 2C). Overall, the algorithm performed robustly, producing a close-to-optimal registrations even for pairs of meshes that involve large deformations, articulated motion or both. The registration is accomplished in an unsupervised way, without any prior knowledge about object shape, dynamics, or alignment. Partial view completion. The Correlated Correspondence algorithm allows us to register a data mesh containing only a partial scan of an object to a known complete surface model of the object, which serves as a template. We can then transform the template mesh to the partial scan, a process which leaves undisturbed the links that are not involved in the partial mesh. The result is a mesh that matches the data on the observed points, while completing the unknown portion of the surface using the template. We take a partial mesh, which is missing the entire back part of the puppet in a particular pose. The resulting partial model is displayed in Fig. 3B-1; for comparison, the correct complete model in this configuration (which was not available to the algorithm), is shown in Fig. 3B-2. We register the partial mesh to models of the object in a different pose (Fig. 3B- 3), and compare the completions we obtain (Fig. 3B-4), to the ground truth represented in Fig. 3B-2. The result demonstrates a largely correct reconstruction of the complete surface geometry from the partial scan and the deformed template. We report additional shape completion results in [2]. Interpolation. Current research [20] shows that if a nonrigid transformation between the poses is available, believable animation can be produced by linear interpolation be- Figure 3: A) The results produced by the CC algorithm were used for unsupervised recovery of articulated models. 15 puppet parts and 4 arm parts, as well as the articulated object skeletons, were recovered. B) Partial view completion results. The missing parts of the surface were estimated by registering the partial view to a complete model of the object in a different configuration. tween the model mesh and the transformed model mesh. The interpolation is performed in the space of local link parameters (li,j, dij, dji), We demonstrate that transforma- tion estimates produced by our algorithm can be used to automatically generate believable animation sequences between fairly different poses, as shown in Fig. 2A. Recovering Articulated Models. Articulated object models have a number of appli- cations in animation and motion capture, and there has been work on recovering them automatically from 3D data [7, 3]. We show that our unsupervised registration capability can greatly assist articulated model recovery. In particular, the algorithm in [3] requires an estimate of the correspondences between a template mesh and the remaining meshes in the dataset. We supplied it with registration computed with the Correlated Correspondence algorithm. As a result we managed to recover in a completely unsupervised way all 15 rigid parts of the puppet, as well as the joints between them (Fig. 3A). We demonstrate successful articulation recovery even for objects which are not purely rigid, as is the case with the human arm (see Fig. 3A).

Planning algorithms designed for deterministic worlds, such as A* search, usually run much faster than algorithms designed for worlds with uncertain action outcomes, such as value iteration. Real-world planning problems often exhibit uncertainty, which forces us to use the slower algorithms to solve them. Many real-world planning problems exhibit sparse uncertainty: there are long sequences of deterministic actions which accomplish tasks like moving sensor platforms into place, inter- spersed with a small number of sensing actions which have uncertain out- comes. In this paper we describe a new planning algorithm, called MCP (short for MDP Compression Planning), which combines A* search with value iteration for solving Stochastic Shortest Path problem in MDPs with sparse stochasticity. We present experiments which show that MCP can run substantially faster than competing planners in domains with sparse uncertainty; these experiments are based on a simulation of a ground robot cooperating with a helicopter to fill in a partial map and move to a goal location. In deterministic planning problems, optimal paths are acyclic: no state is visited more than once. Because of this property, algorithms like A* search can guarantee that they visit each state in the state space no more than once. By visiting the states in an appropriate order, it is possible to ensure that we know the exact value of all of a state's possible successors before we visit that state; so, the first time we visit a state we can compute its correct value. By contrast, if actions have uncertain outcomes, optimal paths may contain cycles: some states will be visited two or more times with positive probability. Because of these cycles, there is no way to order states so that we determine the values of a state's successors before we visit the state itself. Instead, the only way to compute state values is to solve a set of simultaneous equations. In problems with sparse stochasticity, only a small fraction of all states have uncertain outcomes. It is these few states that cause all of the cycles: while a deterministic state s may participate in a cycle, the only way it can do so is if one of its successors has an action with a stochastic outcome (and only if this stochastic action can lead to a predecessor of s). In such problems, we would like to build a smaller MDP which contains only states which are related to stochastic actions. We will call such an MDP a compressed MDP, and we will call its states distinguished states. We could then run fast algorithms like A* search to plan paths between distinguished states, and reserve slower algorithms like value iteration for deciding how to deal with stochastic outcomes. (a) Segbot (b) Robotic helicopter (d) Planning map (e) Execution simulation (c) 3D Map Figure 1: Robot-Helicopter Coordination There are two problems with such a strategy. First, there can be a large number of states which are related to stochastic actions, and so it may be impractical to enumerate all of them and make them all distinguished states; we would prefer instead to distinguish only states which are likely to be encountered while executing some policy which we are considering. Second, there can be a large number of ways to get from one distinguished state to another: edges in the compressed MDP correspond to sequences of actions in the original MDP. If we knew the values of all of the distinguished states exactly, then we could use A* search to generate optimal paths between them, but since we do not we cannot. In this paper, we will describe an algorithm which incrementally builds a compressed MDP using a sequence of deterministic searches. It adds states and edges to the compressed MDP only by encountering them along trajectories; so, it never adds irrelevant states or edges to the compressed MDP. Trajectories are generated by deterministic search, and so undistinguished states are treated only with fast algorithms. Bellman errors in the values for distinguished states show us where to try additional trajectories, and help us build the relevant parts of the compressed MDP as quickly as possible. 1 Robot-Helicopter Coordination Problem The motivation for our research was the problem of coordinating a ground robot and a helicopter. The ground robot needs to plan a path from its current location to a goal, but has only partial knowledge of the surrounding terrain. The helicopter can aid the ground robot by flying to and sensing places in the map. Figure 1(a) shows our ground robot, a converted Segway with a SICK laser rangefinder. Figure 1(b) shows the helicopter, also with a SICK. Figure 1(c) shows a 3D map of the environment in which the robot operates. The 3D map is post-processed to produce a discretized 2D environment (Figure 1(d)). Several places in the map are unknown, either because the robot has not visited them or because their status may have changed (e.g, a car may occupy a driveway). Such places are shown in Figure 1(d) as white squares. The elevation of each white square is proportional to the probability that there is an obstacle there; we assume independence between unknown squares. The robot must take the unknown locations into account when planning for its route. It may plan a path through these locations, but it risks having to turn back if its way is blocked. Alternately, the robot can ask the helicopter to fly to any of these places and sense them. We assign a cost to running the robot, and a somewhat higher cost to running the helicopter. The planning task is to minimize the expected overall cost of running the robot and the helicopter while getting the robot to its destination and the helicopter back to its home base. Figure 1(e) shows a snapshot of the robot and helicopter executing a policy. Designing a good policy for the robot and helicopter is a POMDP planning problem; unfortunately POMDPs are in general difficult to solve (PSPACE-complete [7]). In the POMDP representation, a state is the position of the robot, the current location of the helicopter (a point on a line segment from one of the unknown places to another unknown place or the home base), and the true status of each unknown location. The positions of the robot and the helicopter are observable, so that the only hidden variables are whether each unknown place is occupied. The number of states is (# of robot locations)(# of helicopter locations)2# of unknown places. So, the number of states is exponential in the number of unknown places and therefore quickly becomes very large. We approach the problem by planning in the belief state space, that is, the space of probability distributions over world states. This problem is a continuous-state MDP; in this belief MDP, our state consists of the ground robot's location, the helicopter's location, and a probability of occupancy for each unknown location. We will discretize the continuous probability variables by breaking the interval [0, 1] into several chunks; so, the number of belief states is exponential in the number of unknown places, and classical algorithms such as value iteration are infeasible even on small problems. If sensors are perfect, this domain is acyclic: after we sense a square we know its true state forever after. On the other hand, imperfect sensors can lead to cycles: new sensor data can contradict older sensor data and lead to increased uncertainty. With or without sensor noise, our belief state MDP differs from general MDPs because its stochastic transitions are sparse: large portions of the policy (while the robot and helicopter are traveling be- tween unknown locations) are deterministic. The algorithm we propose in this paper takes advantage of this property of the problem, as we explain in the next section. 2 The Algorithm Our algorithm can be broken into two levels. At a high level, it constructs a compressed MDP, denoted M c, which contains only the start, the goal, and some states which are out- comes of stochastic actions. At a lower level, it repeatedly runs deterministic searches to find new information to put into M c. This information includes newly-discovered stochas- tic actions and their outcomes; better deterministic paths from one place to another; and more accurate value estimates similar to Bellman backups. The deterministic searches can use an admissible heuristic h to focus their effort, so we can often avoid putting many irrelevant actions into M c. Because M c will often be much smaller than M , we can afford to run stochastic plan- ning algorithms like value iteration on it. On the other hand, the information we get by planning in M c will improve the heuristic values that we use in our deterministic searches; so, the deterministic searches will tend to visit only relevant portions of the state space. 2.1 Constructing and Solving a Compressed MDP Each action in the compressed MDP represents several consecutive actions in M : if we see a sequence of states and actions s1, a1, s2, a2, . . . , sk, ak where a1 through ak-1 are deterministic but ak is stochastic, then we can represent it in M c with a single action a, available at s1, whose outcome distribution is P (s | sk, ak) and whose cost is k-1 c(s1, a, s ) = c(si, ai, si+1) + c(sk, ak, s ) i=1 (See Figure 2(a) for an example of such a compressed action.) In addition, if we see a se- quence of deterministic actions ending in sgoal, say s1, a1, s2, a2, . . . , sk, ak, sk+1 = sgoal, we can define a compressed action which goes from s1 to sgoal at cost k c(s i=1 i, ai, si+1). We can label each compressed action that starts at s with (s, s , a) (where a = null if s = sgoal). Among all compressed actions starting at s and ending at (s , a) there is (at least) one with lowest expected cost; we will call such an action an optimal compression of (s, s , a). Write Astoch for the set of all pairs (s, a) such that action a when taken from state s has more than one possible outcome, and include as well (sgoal, null). Write Sstoch for the states which are possible outcomes of the actions in Astoch, and include sstart as well. If we include in our compressed MDP an optimal compression of (s, s , a) for every s Sstoch and every (s , a) Astoch, the result is what we call the full compressed MDP; an example is in Figure 2(b). If we solve the full compressed MDP, the value of each state will be the same as the value of the corresponding state in M . However, we do not need to do that much work: (a) action compression (b) full MDP compression (c) incremental MDP compression Figure 2: MDP compression Main() 01 initialize M c with sstart and sgoal and set their v-values to 0; 02 while (s M c s.t. RHS(s) - v(s) > and s belongs to the current greedy policy) 03 select spivot to be any such state s; 04 [v; vlim] = Search(spivot); 05 v(spivot) = v; 06 set the cost c(spivot, a, sgoal) of the limit action a from spivot to vlim; 07 optionally run some algorithm satisfying req. A for a bounded amount of time to improve the value function in M c; Figure 3: MCP main loop many states and actions in the full compressed MDP are irrelevant since they do not appear in the optimal policy from sstart to sgoal. So, the goal of the MCP algorithm will be to construct only the relevant part of the compressed MDP by building M c incrementally. Figure 2(c) shows the incremental construction of a compressed MDP which contains all of the stochastic states and actions along an optimal policy in M . The pseudocode for MCP is given in Figure 3. It begins by initializing M c to contain only sstart and sgoal, and it sets v(sstart) = v(sgoal) = 0. It maintains the invariant that 0 v(s) v(s) for all s. On each iteration, MCP looks at the Bellman error of each of the states in M c. The Bellman error is v(s) - RHS(s), where RHS(s) = min RHS(s, a) RHS(s, a) = Es succ(s,a)(c(s, a, s ) + v(s )) aA(s) By convention the min of an empty set is , so an s which does not have any compressed actions yet is considered to have infinite RHS. MCP selects a state with negative Bellman error, spivot, and starts a search at that state. (We note that there exist many possible ways to select spivot; for example, we can choose the state with the largest negative Bellman error, or the largest error when weighted by state visitation probabilities in the best policy in M c.) The goal of this search is to find a new compressed action a such that its RHS-value can provide a new lower bound on v(spivot). This action can either decrease the current RHS(spivot) (if a seems to be a better action in terms of the current v-values of action outcomes) or prove that the current RHS(spivot) is valid. Since v(s ) v(s ), one way to guarantee that RHS(spivot, a) v(spivot) is to compute an optimal compression of (spivot, s, a) for all s, a, then choose the one with the smallest RHS. A more sophisticated strategy is to use an A search with appropriate safeguards to make sure we never overestimate the value of a stochastic action. MCP, however, uses a modified A search which we will describe in the next section. As the search finds new compressed actions, it adds them and their outcomes to M c. It is allowed to initialize newly-added states to any admissible values. When the search returns, MCP sets v(spivot) to the returned value. This value is at least as large as RHS(spivot). Consequently, Bellman error for spivot becomes non-negative. In addition to the compressed action and the updated value, the search algorithm returns a "limit value" vlim(spivot). These limit values allow MCP to run a standard MDP planning algorithm on M c to improve its v(s) estimates. MCP can use any planning algorithm which guarantees that, for any s, it will not lower v(s) and will not increase v(s) beyond the smaller of vlim(s) and RHS(s) (Requirement A). For example, we could insert a fake "limit action" into M c which goes directly from spivot to sgoal at cost vlim(spivot) (as we do on line 06 in Figure 3), then run value iteration for a fixed amount of time, selecting for each backup a state with negative Bellman error. After updating M c from the result of the search and any optional planning, MCP begins again by looking for another state with a negative Bellman error. It repeats this process until there are no negative Bellman errors larger than . For small enough , this property guarantees that we will be able to find a good policy (see section 2.3). 2.2 Searching the MDP Efficiently The top level algorithm (Figure 3) repeatedly invokes a search method for finding trajec- tories from spivot to sgoal. In order for the overall algorithm to work correctly, there are several properties that the search must satisfy. First, the estimate v that search returns for the expected cost of spivot should always be admissible. That is, 0 v v(spivot) (Property 1). Second, the estimate v should be no less than the one-step lookahead value of spivot in M c. That is, v RHS(spivot) (Property 2). This property ensures that search either increases the value of spivot or finds additional (or improved) compressed actions. The third and final property is for the vlim value, and it is only important if MCP uses its optional planning step (line 07). The property is that v vlim v(spivot) (Property 3). Here v(spivot) denotes the minimum expected cost of starting at spivot, picking a com- pressed action not in M c, and acting optimally from then on. (Note that v can be larger than v if the optimal compressed action is already part of M c.) Property 3 uses v rather than v since the latter is not known while it is possible to compute a lower bound on the former efficiently (see below). One could adapt A* search to satisfy at least Properties 1 and 2 by assuming that we can control the outcome of stochastic actions. However, this sort of search is highly optimistic and can bias the search towards improbable trajectories. Also, it can only use heuristics which are even more optimistic than it is: that is, h must be admissible with respect to the optimistic assumption of controlled outcomes. We therefore present a version of A*, called MCP-search (Figure 4), that is more effi- cient for our purposes. MCP-search finds the correct expected value for the first stochas- tic action it encounters on any given trajectory, and is therefore far less optimistic. And, MCP-search only requires heuristic values to be admissible with respect to v values, h(s) v(s). Finally, MCP-search speeds up repetitive searches by improving heuris- tic values based on previous searches. A* maintains a priority queue, OPEN, of states which it plans to expand. The OPEN queue is sorted by f (s) = g(s)+h(s), so that A* always expands next a state which appears to be on the shortest path from start to goal. During each expansion a state s is removed from OPEN and all the g-values of s's successors are updated; if g(s ) is decreased for some state s , A* inserts s into OPEN. A* terminates as soon as the goal state is expanded. We use the variant of A* with pathmax [5] to use efficiently heuristics that do not satisfy the triangle inequality. MCP is similar to A, but the OPEN list can also contain state-action pairs {s, a} where a is a stochastic action (line 31). Plain states are represented in OPEN as {s, null}. Just ImproveHeuristic(s) 01 if s M c then h(s) = max(h(s), v(s)); 02 improve heuristic h(s) further if possible using f best and g(s) from previous iterations; procedure fvalue({s, a}) 03 if s = null return ; 04 else if a = null return g(s) + h(s); 05 else return g(s) + max(h(s), E {c(s, a, s ) + h(s )}) s Succ(s,a) ; CheckInitialize(s) 06 if s was accessed last in some previous search iteration 07 ImproveHeuristic(s); 08 if s was not yet initialized in the current search iteration 09 g(s) = ; InsertUpdateCompAction(spivot, s, a) 10 reconstruct the path from spivot to s; 11 insert compressed action (spivot, s, a) into A(spivot) (or update the cost if a cheaper path was found) 12 for each outcome u of a that was not in M c previously 13 set v(u) to h(u) or any other value less than or equal to v(u); 14 set the cost c(u, a, sgoal) of the limit action a from u to v(u); procedure Search(spivot) 15 CheckInitialize(sgoal), CheckInitialize(spivot); 16 g(spivot) = 0; 17 OPEN = {{spivot, null}}; 18 {sbest, abest} = {null, null}, f best = ; 19 while(g(sgoal) > min{s,a}OPEN(fvalue({s, a})) AND f best + > min{s,a}OPEN(fvalue({s, a}))) 20 remove {s, a} with the smallest fvalue({s, a}) from OPEN breaking ties towards the pairs with a = null; 21 if a = null //expand state s 22 for each s Succ(s) 23 CheckInitialize(s ); 24 for each deterministic a A(s) 25 s = Succ(s, a ); 26 h(s ) = max(h(s ), h(s) - c(s, a , s )); 27 if g(s ) > g(s) + c(s, a , s ) 28 g(s ) = g(s) + c(s, a , s ); 29 insert/update {s , null} into OPEN with fvalue({s , null}); 30 for each stochastic a A(s) 31 insert/update {s, a } into OPEN with fvalue({s, a }); 32 else //encode stochastic action a from state s as a compressed action from spivot 33 InsertUpdateCompAction(spivot, s, a); 34 if f best > fvalue({s, a}) then {sbest, abest} = {s, a}, f best = fvalue({s, a}); 35 if (g(sgoal) min{s,a}OPEN(fvalue({s, a})) AND OPEN = ) 36 reconstruct the path from spivot to sgoal; 37 update/insert into A(spivot) a deterministic action a leading to sgoal; 38 if f best g(sgoal) then {sbest, abest} = {sgoal, null}, f best = g(sgoal); 39 return [f best; min{s,a}OPEN(fvalue({s, a}))]; Figure 4: MCP-search Algorithm like A*, MCP-search expands elements in the order of increasing f -values, but it breaks ties towards elements encoding plain states (line 20). The f -value of {s, a} is defined as g(s) + max[h(s), Es Succ(s,a)(c(s, a, s ) + h(s ))] (line 05). This f -value is a lower bound on the cost of a policy that goes from sstart to sgoal by first executing a series of deterministic actions until action a is executed from state s. This bound is as tight as possible given our heuristic values. State expansion (lines 21-31) is very similar to A. When the search removes from OPEN a state-action pair {s, a} with a = null, it adds a compressed action to M c (line 33). It also adds a compressed action if there is an optimal deterministic path to sgoal (line 37). f best tracks the minimum f -value of all the compressed actions found. As a result, f best v(spivot) and is used as a new estimate for v(spivot). The limit value vlim(spivot) is obtained by continuing the search until the minimum f -value of elements in OPEN approaches f best + for some 0 (line 19). This minimum f -value then provides a lower bound on v(spivot). To speed up repetitive searches, MCP-search improves the heuristic of every state that it encounters for the first time in the current search iteration (lines 01 and 02). Line 01 uses the fact that v(s) from M c is a lower bound on v(s). Line 02 uses the fact that f best - g(s) is a lower bound on v(s) at the end of each previous call to Search; for more details see [4]. 2.3 Theoretical Properties of the Algorithm We now present several theorems about our algorithm. The proofs of these and other theo- rems can be found in [4]. The first theorem states the main properties of MCP-search. Theorem 1 The search function terminates and the following holds for the values it re- turns: (a) if sbest = null then v(spivot) f best E{c(spivot, abest, s ) + v(s )} (b) if sbest = null then v(spivot) = f best = (c) f best min{s,a}OPEN(fvalue({s, a})) v(spivot). If neither sgoal nor any state-action pairs were expanded, then sbest = null and (b) says that there is no policy from spivot that has a finite expected cost. Using the above theorem it is easy to show that MCP-search satisfies Properties 1, 2 and 3, considering that f best is returned as variable v and min{s,a}OPEN(fvalue({s, a})) is returned as variable vlim in the main loop of the MCP algorithm (Figure 3). Property 1 follows directly from (a) and (b) and the fact that costs are strictly positive and v-values are non-negative. Property 2 also follows trivially from (a) and (b). Property 3 follows from (c). Given these properties the next theorem states the correctness of the outer MCP algorithm (in the theorem cgreedy denotes a greedy policy that always chooses an action that looks best based on its cost and the v-values of its immediate successors). Theorem 2 Given a deterministic search algorithm which satisfies Properties 13, the MCP algorithm will terminate. Upon termination, for every state s M c c we greedy have RHS(s) - v(s) v(s). Given the above theorem one can show that for 0 < cmin (where cmin is the smallest expected action cost in our MDP) the expected cost of executing c from greedy sstart is at most cmin v(s c start). Picking cmin is not guaranteed to result in a proper min - policy, even though Theorem 2 continues to hold. 3 Experimental Study We have evaluated the MCP algorithm on the robot-helicopter coordination problem de- scribed in section 1. To obtain an admissible heuristic, we first compute a value function for every possible configuration of obstacles. Then we weight the value functions by the probabilities of their obstacle configurations, sum them, and add the cost of moving the helicopter back to its base if it is not already there. This procedure results in optimistic cost estimates because it pretends that the robot will find out the obstacle locations immediately instead of having to wait to observe them. The results of our experiments are shown in Figure 5. We have compared MCP against three algorithms: RTDP [1], LAO* [2] and value iteration on reachable states (VI). RTDP can cope with large size MDPs by focussing its planning efforts along simulated execu- tion trajectories. LAO* uses heuristics to prune away irrelevant states, then repeatedly performs dynamic programming on the states in its current partial policy. We have im- plemented LAO* so that it reduces to AO* [6] when environments are acyclic (e.g., the robot-helicopter problem with perfect sensing). VI was only able to run on the problems with perfect sensing since the number of reachable states was too large for the others. The results support the claim that MCP can solve large problems with sparse stochas- ticity. For the problem with perfect sensing, on average MCP was able to plan 9.5 times faster than LAO*, 7.5 times faster than RTDP, and 8.5 times faster than VI. On average for these problems, MCP computed values for 58633 states while M c grew to 396 states, and MCP encountered 3740 stochastic transitions (to give a sense of the degree of stochastic- ity). The main cost of MCP was in its deterministic search subroutine; this fact suggests that we might benefit from anytime search techniques such as ARA* [3]. The results for the problems with imperfect sensing show that, as the number and den- sity of uncertain outcomes increases, the advantage of MCP decreases. For these problems MCP was able to solve environments 10.2 times faster than LAO* but only 2.2 times faster than RTDP. On average MCP computed values for 127,442 states, while the size of M c was 3,713 states, and 24,052 stochastic transitions were encountered. Figure 5: Experimental results. The top row: the robot-helicopter coordination problem with perfect sensors. The bottom row: the robot-helicopter coordination problem with sensor noise. Left column: running times (in secs) for each algorithm grouped by environments. Middle column: the number of backups for each algorithm grouped by environments. Right column: an estimate of the expected cost of an optimal policy (v(sstart)) vs. running time (in secs) for experiment (k) in the top row and experiment (e) in the bottom row. Algorithms in the bar plots (left to right): MCP, LAO*, RTDP and VI (VI is only shown for problems with perfect sensing). The characteristics of the environments are given in the second and third rows under each of the bar plot. The second row indicates how many cells the 2D plane is discretized into, and the third row indicates the number of initially unknown cells in the environment. 4 Discussion The MCP algorithm incrementally builds a compressed MDP using a sequence of deter- ministic searches. Our experimental results suggest that MCP is advantageous for problems with sparse stochasticity. In particular, MCP has allowed us to scale to larger environments than were otherwise possible for the robot-helicopter coordination problem. Acknowledgements This research was supported by DARPA's MARS program. All conclusions are our own.

This paper presents a real-time algorithm for acquiring compact three-dimensional maps of indoor environments, using a mobile robot equipped with range and imaging sensors. Building on previous work on real-time pose estimation during mapping, our approach extends the popular expectation-maximization algorithm to multisurface models, and makes it amenable to real-time execution. Maps acquired by our algorithm consist of compact sets of textured polygons that can be visualized interactively. Experimental results obtained in corridor-type environments illustrate that compact and accurate maps can be acquired in real time and in a fully automated fashion.

As people move through their environments, they do not move randomly. Instead, they are often engaged in typical motion patterns, related to specific locations they might be interested in approaching. In this paper we propose a method for adapting the behavior of a mobile robot according to the activities of the people in its surrounding. Our approach uses learned models of people's motion behaviors. Whenever the robot detects a person it computes a probabilistic estimate about which motion pattern the person might be engaged in. During the path planning it then uses this belief to improve its navigation behavior. In different practical experiments carried out on a real robot we demonstrate that our approach allows a robot to quickly adapt its navigation plans according to the activities of the persons in its surrounding. We also present experiments illustrating that our approach provides a better behavior than a standard reactive collision avoidance system.

The problem of generating maps with mobile robots has received considerable attention over the past years. Most of the techniques developed so far have been designed for situations in which the environment is static during the mapping process. Dynamic objects, however, can lead to serious errors in the resulting maps such as spurious objects or misalignments due to localization errors. In this paper we consider the problem of creating maps with mobile robots in dynamic environments. We present a new approach that interleaves mapping and localization with a probabilistic technique to identify spurious measurements. In several experiments we demonstrate that our algorithm generates accurate 2D and 3D in different kinds of dynamic indoor and outdoor environments. We also use our algorithm to isolate the dynamic objects and generate 3D representation of them.

In [Thrun, S., et al., 2001], we proposed the sparse extended information filter for efficiently solving the simultaneous localization and mapping (SLAM) problem. In this paper, we extend this algorithm to handle data association problems and report real-world results, obtained with an outdoor vehicle. We find that our approach performs favorably when compared to the extended Kalman filter solution from which it is derived.

The extended Kalman filter (EKF) has been the de facto approach to the Simultaneous Localization and Mapping (SLAM) problem for nearly fifteen years. However, the EKF has two serious deficiencies that prevent it from being applied to large, real-world environments: quadratic complexity and sensitivity to failures in data association. FastSLAM, an alternative approach based on the Rao-Blackwellized Particle Filter, has been shown to scale logarithmically with the number of landmarks in the map. This efficiency enables FastSLAM to be applied to environments far larger than could be handled by the EKF. In this paper, we show that FastSLAM also substantially outperforms the EKF in environments with ambiguous data association. The performance of the two algorithms is compared on a real-world data set with various levels of odometric noise. In addition, we show how negative information can be incorporated into FastSLAM in order to improve the accuracy of the estimated map.

This paper describes a robotic walker designed as an assistive device for frail elderly people with cognitive impairment. Locomotion is most often the primary form of exercise for the elderly, and devices that provide mobility assistance are critical for the health and well being of such individuals. Previous work on walkers focused primarily on safety but offered little or no assistance with navigation and global orientation. Our system provides these features in addition to the stability and support provided by conventional walkers. A software suite of robot localization and navigation combined with a shared-control haptic interface achieves this capability. The system has been tested in a retirement facility near Pittsburgh, PA, USA.

The ability to simultaneously localise a robot and accurately map its surroundings is considered by many to be a key prerequisite of truly autonomous robots. This paper presents a real-world implementation of FastSLAM, an algorithm that recursively estimates the full posterior distribution of both robot pose and landmark locations. In particular, we present an extension to FastSLAM that addresses the data association problem using a nearest neighbor technique. Building on this, we also present a novel multiple hypotheses tracking implementation (MHT) to handle uncertainty in the data association. Finally an extension to the multi-robot case is introduced. Our algorithm has been run successfully using a number of data sets obtained in outdoor environments. Experimental results are presented that demonstrate the performance of the algorithms when compared with standard Kalman filter-based approaches.

This paper describes two robotic systems developed for acquiring accurate volumetric maps of underground mines. One system is based on a cart instrumented by laser range finders, pushed through a mine by people. Another is a remotely controlled mobile robot equipped with laser range finders. To build consistent maps of large mines with many cycles, we describe an algorithm for estimating global correspondences and aligning robot paths. This algorithm enables us to recover consistent maps several hundreds of meters in diameter, without odometric information. We report results obtained in two mines, a research mine in Bruceton, PA, and an abandoned coal mine in Burgettstown, PA.

The simultaneous localization and mapping (SLAM) with detection and tracking of moving objects (DATMO) problem is not only to solve the SLAM problem in dynamic environments but also to detect and track these dynamic objects. In this paper, we derive the Bayesian formula of the SLAM with DATMO problem, which provides a solid basis for understanding and solving this problem. In addition, we provide a practical algorithm for performing DATMO from a moving platform equipped with range sensors. The probabilistic approach to solve the whole problem has been implemented with the Navlab11 vehicle. More than 100 miles of experiments in crowded urban areas indicated that SLAM with DATMO is indeed feasible.

The ability to learn a consistent model of its environment is a prerequisite for autonomous mobile robots. A particularly challenging problem in acquiring environment maps is that of closing loops; loops in the environment create challenging data association problems [J.-S. Gutman et al., 1999]. This paper presents a novel algorithm that combines Rao-Blackwellized particle filtering and scan matching. In our approach scan matching is used for minimizing odometric errors during mapping. A probabilistic model of the residual errors of scan matching process is then used for the resampling steps. This way the number of samples required is seriously reduced. Simultaneously we reduce the particle depletion problem that typically prevents the robot from closing large loops. We present extensive experiments that illustrate the superior performance of our approach compared to previous approaches.

In this paper we describe our open-source robot control software, the Carnegie Mellon Navigation (CARMEN) Toolkit. The ultimate goals of CARMEN are to lower the barrier to implementing new algorithms on real and simulated robots and to facilitate sharing of research and algorithms between different institutions. In order for CARMEN to be as inclusive of various research approaches as possible, we have chosen not to adopt strict software standards, but to instead focus on good design practices. This paper outlines the lessons we have learned in developing these practices.

The design of cooperative multi-robot systems is a highly active research area in robotics. Two lines of research in particular have generated inter- est: the solution of large, weakly coupled MDPs, and the design and im- plementation of market architectures. We propose a new algorithm which joins together these two lines of research. For a class of coupled MDPs, our algorithm automatically designs a market architecture which causes a decentralized multi-robot system to converge to a consistent policy. We can show that this policy is the same as the one which would be produced by a particular centralized planning algorithm. We demonstrate the new algorithm on three simulation examples: multi-robot towing, multi-robot path planning with a limited fuel resource, and coordinating behaviors in a game of paint ball.

We present the software architecture of a robotic system for mapping abandoned mines. The software is capable of acquiring consistent 2D maps of large mines with many cycles, represented as Markov random £elds. 3D C-space maps are acquired from local 3D range scans, which are used to identify navigable paths using A* search. Our system has been deployed in three abandoned mines, two of which inaccessible to people, where it has acquired maps of unprecedented detail and accuracy.

In real world planning problems, time for deliberation is often limited. Anytime planners are well suited for these problems: they find a feasi- ble solution quickly and then continually work on improving it until time runs out. In this paper we propose an anytime heuristic search, ARA*, which tunes its performance bound based on available search time. It starts by finding a suboptimal solution quickly using a loose bound, then tightens the bound progressively as time allows. Given enough time it finds a provably optimal solution. While improving its bound, ARA* reuses previous search efforts and, as a result, is significantly more effi- cient than other anytime search methods. In addition to our theoretical analysis, we demonstrate the practical utility of ARA* with experiments on a simulated robot kinematic arm and a dynamic path planning prob- lem for an outdoor rover.

Recent developments in grid-based and point-based approximation algo- rithms for POMDPs have greatly improved the tractability of POMDP planning. These approaches operate on sets of belief points by individ- ually learning a value function for each point. In reality, belief points exist in a highly-structured metric simplex, but current POMDP algo- rithms do not exploit this property. This paper presents a new metric-tree algorithm which can be used in the context of POMDP planning to sort belief points spatially, and then perform fast value function updates over groups of points. We present results showing that this approach can re- duce computation in point-based POMDP algorithms for a wide range of problems.

We propose a method for learning models of people's motion behaviors in an indoor environment. As people move through their environments, they do not move randomly. Instead, they often engage in typical motion patterns, related to specific locations that they might be interested in approaching and specific trajectories that they might follow in doing so. Knowledge about such patterns may enable a mobile robot to develop improved people following and obstacle avoidance skills. This paper proposes an algorithm that learns collections of typical trajectories that characterize a person's motion patterns. Data, recorded by mobile robots equipped with laser range finders, is clustered into different types of motion using the popular expectation maximization algorithm, while simultaneously learning multiple motion patterns. Experimental results, obtained using data collected in a domestic residence and in an office building, illustrate that highly predictive models of human motion patterns can be learned.

Describes an online algorithm for generating compact 3D maps of mobile robot environments. Maps generated by our approach consist of small numbers of planar surfaces, which are augmented by fine-grained polygons for non-flat environmental features. Our approach builds on the expectation maximization (EM) algorithm, but develops a new, incremental version that can be executed in real-time. Experimental results obtained in corridor-type environments illustrate that compact and accurate maps can be acquired in real-time, from range and camera data collected by a mobile robot.

Presents a probabilistic algorithm for simultaneously estimating the pose of a mobile robot and the positions of nearby people in a previously mapped environment. This approach, called the conditional particle filter, tracks a large distribution of person locations conditioned upon a smaller distribution of robot poses over time. This method is robust to sensor noise, occlusion, and uncertainty in robot localization. In fact, conditional particle filters can accurately track people in situations with global uncertainty over robot pose. The number of samples required by this filter scales linearly with the number of people being tracked, making the algorithm feasible to implement in real-time in environments with large numbers of people. Experimental results illustrate the accuracy of tracking and model selection, as well as the performance of an active following behavior based on this algorithm.

Virtually all existing mobile robot localization techniques operate on a static map of the environment. When the environment changes (e.g., doors are opened or closed), there is an opportunity to simultaneously estimate the robot's pose and the state of the environment. The resulting estimation problem is high-dimensional, rendering current localization techniques inapplicable. This paper proposes an efficient, factored estimation algorithm for mixed discrete-continuous state estimation. Our algorithm integrates particle filters for robot localization, and conditional binary Bayes filters for estimating the dynamic state of the environment. Experimental results illustrate that our algorithm is highly effective in estimating the status of doors, and outperforms a state-of-the-art localizer in dynamic environments.

We propose a method for learning models of people's motion behaviors in indoor environments. As people move through their environments, they do not move randomly. Instead, they often engage in typical motion patterns, related to specific locations that they might be interested in approaching and specific trajectories that they might follow in doing so. Knowledge about such patterns may enable a mobile robot to develop improved people following and obstacle avoidance skills. This paper proposes an algorithm that learns collections of typical trajectories that characterize a person's motion patterns. Data, recorded by mobile robots equipped with laser-range finders, is clustered into different types of motion using the popular expectation maximization algorithm, while simultaneously learning multiple motion patterns. Experimental results, obtained using data collected in a domestic residence and in an office building, illustrate that highly predictive models of human motion patterns can be learned.

We propose an occupancy grid mapping algorithm for mobile robots operating in environments where objects change their locations over time. Our approach uses a straightforward map differencing technique to detect changes in an environment over time. It employs the expectation maximization algorithm to learn models of non-stationary objects, and to determine the location of such objects in individual occupancy grid maps built at different points in time. By combining data from multiple maps when learning object models, the resulting models have higher fidelity than could be obtained from any single map. A Bayesian complexity measure is applied to determine the number of different objects in the model, making it possible to apply the approach to situations where not all objects are present at all times in the map.

A map of the environment map is a model of the surroundings of the robot. In many robot application domains, it is infeasible to obtain a prior map of the environment. It is therefore desirable that robots are able to generate maps by themselves. This problem is challenging, since it involves a simultaneous localization problem of the robot relative to its (incomplete) map. This article presents a new probabilistic algorithm for the simultaneous localization and mapping problem. The algorithm is based on matching of a set of local maps that have been obtained from range data. The main advantage of the proposed method is of computational nature. The use of segmented maps permits to eliminate unnecessary details, leading the method to be very appropriate for dynamical environments.

We propose a motion planning algorithm. for performing policy search in the full pose and velocity space of a mobile robot. By comparison, existing techniques optimize high-level plans, but fail to optimize the low-level motion controls. We use policy search in a high dimensional control space to find plans that lead. to measurably better motion planning. Our experimental results suggest that our approach leads to superior robot motion than many existing techniques.

The coordination of robot motions is one of the fundamental problems for multi-robot systems. A popular approach to avoid planning in the high-dimensional composite configuration space is the prioritized and decoupled technique. In this paper we present a method for optimizing priority schemes for such prioritized and decoupled planning technique. Our approach performs a randomized search with hill-climbing to find solutions and to minimize the overall path lengths. The technique has been implemented and tested on real robots and in extensive simulation runs. The experimental results demonstrate that our method is able to greatly reduce the number of failures and to significantly reduce the overall path length for different prioritized and decoupled path planning techniques and even for large teams of robots.

Coordinating the motion of multiple mobile robots is one of the fundamental problems in robotics. The predominant algorithms for coordinating teams of robots are decoupled and prioritized, thereby avoiding combinatorially hard planning problems typically faced by centralized approaches. We present a method for finding solvable priority schemes for such prioritized and decoupled planning techniques. Existing approaches apply a single priority scheme which makes them overly prone to failure in cases where valid solutions exists. By searching in the space of priorization schemes, our approach overcomes this limitation. To focus the search, our algorithm is guided by constraints generated from the task specification. To illustrate the appropriateness of this approach, the paper discusses experimental results obtained with real robots and through systematic robot simulation. The experimental results demonstrate that our approach successfully solves many more coordination problems than previous decoupled and prioritized techniques.

Presents a way to acquire occupancy grid maps with mobile robots. Virtually all existing occupancy grid mapping algorithms decompose the high-dimensional mapping problem into a collection of one-dimensional problems, where the occupancy of each grid cell is estimated independently of others. This induces conflicts that can lead to inconsistent maps. The paper shows how to solve the mapping problem in the original, high-dimensional space, thereby maintaining all dependencies between neighboring cells. As a result, maps generated by our approach are often more accurate than those generated using traditional techniques. Our approach relies on a rigorous statistical formulation of the mapping problem using forward models. It employs the expectation maximization algorithm for estimating maps, and a Laplacian approximation to determine uncertainty.

We propose a new particle filter that incorporates a model of costs when generating particles. The approach is motivated by the observation that the costs of accidentally not tracking hypotheses might be significant in some areas of state space, and next to irrelevant in others. By incorporat- ing a cost model into particle filtering, states that are more critical to the system performance are more likely to be tracked. Automatic calculation of the cost model is implemented using an MDP value function calcula- tion that estimates the value of tracking a particular state. Experiments in two mobile robot domains illustrate the appropriateness of the approach.

% '& ( *) + , -) .# / "0 1 2 3 4 5# 6 7 #

A method is presented to recover 3D scene structure and camera motion from multiple images without the need for correspondence information. The problem is framed as finding the maximum likelihood structure and motion given only the 2D measurements, integrating over all possible assignments of 3D features to 2D measurements. This goal is achieved by means of an algorithm which iteratively refines a probability distribution over the set of all correspondence assignments. At each iteration a new structure from motion problem is solved, using as input a set of 'virtual measurements' derived from this probability distribution. The distribution needed can be efficiently obtained by Markov Chain Monte Carlo sampling. The approach is cast within the framework of Expectation-Maximization, which guarantees convergence to a local maximizer of the likelihood. The algorithm works well in practice, as will be demonstrated using results on several real image sequences.

In this paper we consider the problem of exploring an unknown environment by a team of robots. As in single-robot exploration the goal is to minimize the overall exploration time. The key problem to be solved therefore is to choose appropriate target points for the individual robots so that they simultaneously explore different regions of their environment. We present a probabilistic approach for the coordination of multiple robots which, in contrast to previous approaches, simultaneously takes into account the costs of reaching a target point and the utility of target points. The utility of target points is given by the size of the unexplored area that a robot can cover with its sensors upon reaching a target position. Whenever a target point is assigned to a specific robot, the utility of the unexplored area visible from this target position is reduced for the other robots. This way, a team of multiple robots assigns different target points to the individual robots. The technique has been implemented and tested extensively in real-world experiments and simulation runs. The results given in this paper demonstrate that our coordination technique significantly reduces the exploration time compared to previous approaches.

This paper describes a programming language extension of C++, called CES, specifically targeted towards mobile robot control. CES's design is motivated by a recent series of successful probabilistic methods for mobile robot control, with the goal of facilitating the development of such probabilistic software in future robot applications. CES extends C++ by two ideas: Computing with probability distributions, and built-in mechanisms for learning from examples as a new means of programming. An example program, used to control a mail-delivering robot with gesture command interface, illustrates that CES may reduce the code development by two orders of magnitude. CES differs from other special-purpose programming languages in the field, which typically emphasize concurrency and real-time/event-driven processing.

We present an incremental method for concurrent mapping and localization for mobile robots equipped with 2D laser range finders. The approach uses a fast implementation of scan-matching for mapping, paired with a sample-based probabilistic method for localization. Compact 3D maps are generated using a multi-resolution approach adopted from the computer graphics literature, fed by data from a dual laser system. Our approach builds 3D maps of large, cyclic environments in real-time, and it is robust. Experimental results illustrate that accurate maps of large, cyclic environments can be generated even in the absence of any odometric data.

To be truly useful, mobile robots need to be fairly autonomous and easy to control. This is especially true in situations where multiple robots are used, due to the increase in sensory information and the fact that the robots can interfere with one another. The paper describes a system that integrates autonomous navigation, a task executive, task planning, and an intuitive graphical user interface to control multiple, heterogeneous robots. We have demonstrated a prototype system that plans and coordinates the deployment of teams of robots. Testing has shown the effectiveness and robustness of the system, and of the coordination strategies in particular.

When trying to recover 3D structure from a set of images, the most difficult problem is establishing the correspondence between the measurements. Most existing approaches assume that features can be tracked across frames, whereas methods that exploit rigidity constraints to facilitate matching do so only under restricted cam(cid:173) era motion. In this paper we propose a Bayesian approach that avoids the brittleness associated with singling out one "best" cor(cid:173) respondence, and instead consider the distribution over all possible correspondences. We treat both a fully Bayesian approach that yields a posterior distribution, and a MAP approach that makes use of EM to maximize this posterior. We show how Markov chain Monte Carlo methods can be used to implement these techniques in practice, and present experimental results on real data.

To navigate reliably in indoor environments, a mobile robot must know where it is. This includes both the ability of globally localizing the robot from scratch, as well as tracking the robot's position once its location is known. Vision has long been advertised as providing a solution to these problems, but we still lack efficient solutions in unmodified environments. Many existing approaches require modification of the environment to function properly, and those that work within unmodified environments seldomly address the problem of global localization. In this paper we present a novel, vision-based localization method based on the CONDENSATION algorithm, a Bayesian filtering method that uses a sampling-based density representation. We show how the CONDENSATION algorithm can be rued in a novel way to track the position of the camera platform rather than tracking an object in the scene. In addition, it can also be used to globally localize the camera platform, given a visual map of the environment. Based on these two observations, we present a vision-based robot localization method that provides a solution to a difficult and open problem in the mobile robotics community. As evidence for the viability of our approach, we show both global localization and tracking results in the context of a state of the art robotics application.

To navigate reliably in indoor environments, a mobile robot must know where it is. Thus, reliable position estimation is a key problem in mobile robotics. We believe that probabilistic approaches are among the most promising candidates to providing a comprehensive and real-time solution to the robot localization problem. However, current methods still face considerable hurdles. In particular the problems encountered are closely related to the type of representation used to represent probability densities over the robot's state space. Earlier work on Bayesian filtering with particle-based density representations opened up a new approach for mobile robot localization based on these principles. We introduce the Monte Carlo localization method, where we represent the probability density involved by maintaining a set of samples that are randomly drawn from it. By using a sampling-based representation we obtain a localization method that can represent arbitrary distributions. We show experimentally that the resulting method is able to efficiently localize a mobile robot without knowledge of its starting location. It is faster, more accurate and less memory-intensive than earlier grid-based methods,.

Ships often use the coasts of continents for navigation in the absence of better tools such as GPS, since being close to land allows sailors to determine with high accuracy where they are. Similarly for mobile robots, in many environments global and accurate localization is not always feasible. Environments can lack features, and dynamic obstacles such as people can confuse and block sensors. We demonstrate a technique for generating trajectories that take into account both the information content of the environment, and the density of the people in the environment. These trajectories reduce the average positional certainty as the robot moves, reducing the likelihood the robot will become lost at any point. Our method was successfully implemented and used by the mobile robot Minerva, a museum tourguide robot, for a 2 week period in the Smithsonian National Museum of American History.

This paper proposes a statistical method for calibrating the odometry of mobile robots. In contrast to previous approaches, which require explicit measurements of actual motion when calibrating a robot's odometry, the algorithm proposed here uses the robot's sensors to automatically calibrate the robot as it operates. An efficient, incremental maximum likelihood algorithm enables the robot to adapt to changes in its kinematics online, as they occur. The appropriateness of the approach is demonstrated in two large-scale environments, where the amount of odometric error is reduced by an order of magnitude.

This paper considers a specific type of interaction: short-term and spontaneous interaction with crowds of people. Such patterns of interactions are found when service robots operate in public places, for example information kiosks, receptionists, and tour-guide robots applications. We describe our approach to spontaneous short-term interaction: a robot designed to be a believable social agent. The approach has been implemented using a mobile robot with a motorized face as focal point for interaction, an architecture that suggests the robot has moods, and a method for learning how to interact with people. Our system was recently deployed at a Smithsonian museum in Washington, DC. During a two week period it interacted with thousands of people. The robot's interactive capabilities were essential for its high on-task performance, and thus its practical success.

This paper describes an interactive tour-guide robot, which was successfully exhibited in a Smithsonian museum. During its two weeks of operation, the robot interacted with thousands of people, traversing more than 44 km at speeds of up to 163 cm/sec. Our approach specifically addresses issues such as safe navigation in unmodified and dynamic environments, and short-term human-robot interaction. It uses learning pervasively at all levels of the software architecture.

Localization, that is the estimation of a robot's location from sensor data, is a fundamental problem in mobile robotics. This papers presents a version of Markov localization which provides accurate position estimates and which is tailored towards dynamic environments. The key idea of Markov localization is to maintain a probability density over the space of all locations of a robot in its environment. Our approach represents this space metrically, using a fine-grained grid to approximate densities. It is able to globally localize the robot from scratch and to recover from localization failures. It is robust to approximate models of the environment (such as occupancy grid maps) and noisy sensors (such as ultrasound sensors). Our approach also includes a filtering technique which allows a mobile robot to reliably estimate its position even in densely populated environments in which crowds of people block the robot's sensors for extended periods of time. The method described here has been implemented and tested in several real-world applications of mobile robots, including the deployments of two mobile robots as interactive museum tour-guides.

Proposes a hybrid approach to the problem of collision avoidance for indoor mobile robots. The /spl mu/DWA (model-based dynamic window approach) integrates sensor data from various sensors with information extracted from a map of the environment, to generate collision-free motion. A novel integration rule ensures that with high likelihood, the robot avoids collisions with obstacles not detectable with its sensors, even if it is uncertain about its position. The approach was implemented and tested extensively as part of an installation, in which a mobile robot gave interactive tours to visitors of the "Deutsches Museum Bonn." Here our approach was essential for the success of the entire mission, because a large number of ill-shaped obstacles prohibited the use of purely sensor-based methods for collision avoidance.

Sensor based exploration is a task which enables a robot to explore and map an unknown environment, using sensor information. The map used in this paper is the generalized Voronoi graph (GVG). The robot explores an unknown environment using an already developed incremental construction procedure to generate the GVG using sensor information. This paper presents some initial results which uses the GVG for robot localization, while mitigating the need to update encoder values. Experimental results verify the described work.

Localization addresses the problem of determining the position of a mobile robot from sensor data. This paper presents an algorithm, called BaLL, which enables a mobile robot to learn a set of landmarks used in localization and to learn how to recognize them using artificial neural networks. BaLL is based on a statistical localization approach. It is applicable to a large variety of sensors and environments. Experiments with a mobile robot equipped with sonar sensors and a camera illustrate that BaLL identifies highly useful landmarks.

This paper addresses the problem of building large-scale maps of indoor environments with mobile robots. It proposes a statistical approach that describes the map building problem as a constrained maximum-likelihood estimation problem, for which it devises a practical algorithm. Experimental results in large, cyclic environments illustrate the appropriateness of the approach.

We present an approach to tracking planar surface patches over time. In addition to tracking a patch with full six degrees of freedom, the algorithm also produces a super-resolved estimate of the texture present on the patch. This texture estimate is kept as an explicit model texture image which is refined over time. We then use it to infer the 3D motion of the patch from the image sequence. The main idea behind the approach is to use a technique from computer graphics, known as texture mapping, as the measurement model in an extended Kalman filter. We also calculate the partial derivative of this image formation process with respect to the 3D pose of the patch, which functions as the measurement Jacobian. The super-resolved estimate of the texture is obtained using the standard extended Kalman filter measurement update, with one essential approximation that makes this computationally feasible. The resulting equations are remarkably simple, yet lead to estimates that are properly super-resolved. In addition to developing the theory behind the approach, we also demonstrate both the tracking and the super-resolution aspect of the algorithm on real image sequences.

We present a Kalman filter based approach to perform model-based motion estimation and tracking. Unlike previous approaches, the tracking process is not formulated as an SSD minimization problem, but is developed by using texture mapping as the measurement model in an extended Kalman filter. During tracking, a super-resolved estimate of the texture present on the object or in the scene is obtained. A key result is the notion of Jacobian images, which can be viewed as a generalization of traditional gradient images, and represent the crucial computation in the tracking process. The approach is illustrated with three sample applications: full 3D tracking of planar surface patches, a projective surface tracker for uncalibrated camera scenarios, and a fast, Kalman filtered version of mosaicking with detection of independently moving objects.

This paper proposes the dynamic window approach to reactive collision avoidance for mobile robots equipped with synchro-drives. The approach is derived directly from the motion dynamics of the robot and is therefore particularly well-suited for robots operating at high speed. It differs from previous approaches in that the search for commands controlling the translational and rotational velocity of the robot is carried out directly in the space of velocities. The advantage of our approach is that it correctly and in a rigorous way incorporates the dynamics of the robot. This is done by reducing the search space to the dynamic window, which consists of the velocities reachable within a short time interval. Within the dynamic window the approach only considers admissible velocities yielding a trajectory on which the robot is able to stop safely. Among these velocities the combination of translational and rotational velocity is chosen by maximizing an objective function. The objective function includes a measure of progress towards a goal location, the forward velocity of the robot, and the distance to the next obstacle on the trajectory. In extensive experiments the approach presented here has been found to safely control our mobile robot RHINO with speeds of up to 95 cm/sec, in populated and dynamic environments.

Designing robots that learn by themselves to perform complex real-world tasks is a still-open challenge for the field of robotics and artificial intelligence. In this paper the author presents the robot learning problem as a lifelong problem, in which a robot faces a collection of tasks over its entire lifetime. Such a scenario provides the opportunity to gather general-purpose knowledge that transfers across tasks. The author illustrates a particular leaning mechanism, explanation-based neural network learning, that transfers knowledge between related tasks via neural network action models. The learning approach is illustrated using a mobile robot, equipped with visual, ultrasonic and laser sensors. In less than 10 minutes operation time, the robot is able to learn to navigate to a marked target object in a natural office environment.>

This paper presents NeuroChess, a program which learns to play chess from the final outcome of games. NeuroChess learns chess board evaluation functions, represented by artificial neural networks. It integrates inductive neural network learning, temporal differencing, and a variant of explanation-based learning. Performance results illustrate some of the strengths and weaknesses of this approach.

Reinforcement learning addresses the problem of learning to select actions in order to maximize one's performance in unknown environments. To scale reinforcement learning to complex real-world tasks, such as typically studied in AI, one must ultimately be able to discover the structure in the world, in order to abstract away the myriad of details and to operate in more tractable problem spaces. This paper presents the SKILLS algorithm. SKILLS discovers skills, which are partially defined action policies that arise in the context of multiple, related tasks. Skills collapse whole action sequences into single operators. They are learned by minimizing the com(cid:173) pactness of action policies, using a description length argument on their representation. Empirical results in simple grid navigation tasks illustrate the successful discovery of structure in reinforcement learning.