This paper presents a UWB-IMU fusion approach to obtain location and orientation of an object in 6 degrees of freedom at the room level, without use of optical motion capture systems. When tested with different human movement patterns such as walking, running, jumping, and swirling on a wheeled chair, we obtain less than 10cm of 3D localization error and under 5° of orientation error at the 90thpercentile. We expect our system, called ObjectTrack, to enable spatial audio and interaction for VR/AR applications, enable precision tracking of objects, and for localization of robotic motion systems. ObjectTrack significantly reduces the cost barrier by about 50× compared to popular motion capture systems.

This paper presents Neural Visibility Field (NVF), a novel uncertainty quantification method for Neural Radi-ance Fields (NeRF) applied to active mapping. Our key insight is that regions not visible in the training views lead to inherently unreliable color predictions by NeRF at this region, resulting in increased uncertainty in the synthesized views. To address this, we propose to use Bayesian Networks to composite position-based field uncertainty into ray-based uncertainty in camera observations. Consequently, NVF nat-urally assigns higher uncertainty to unobserved regions, aiding robots to select the most informative next viewpoints. Extensive evaluations show that NVF excels not only in un-certainty quantification but also in scene reconstruction for active mapping, outperforming existing methods. More de-tails can be found at https://sites.google.com/view/nvf-cvpr24/.

We propose a new metric for robot state estimation based on the recently introduced SE2(3) Lie group definition. Our metric is related to prior metrics for SLAM but explicitly takes into account the linear velocity of the state estimate, improving over current pose-based trajectory analysis. This has the benefit of providing a single, quantitative metric to evaluate state estimation algorithms against, while being compatible with existing tools and libraries. Since ground truth data generally consists of pose data from motion capture systems, we also propose an approach to compute the ground truth linear velocity based on polynomial interpolation. Using Chebyshev interpolation and a pseudospectral parameterization, we can accurately estimate the ground truth linear velocity of the trajectory in an optimal fashion with best approximation error. We demonstrate how this approach performs on multiple robotic platforms where accurate state estimation is vital, and compare it to alternative approaches such as finite differences. The pseudospectral parameterization also provides a means of trajectory data compression as an additional benefit. Experimental results show our method provides a valid and accurate means of comparing state estimation systems, which is also easy to interpret and report.

Trajectory retiming is the task of computing a feasible time parameterization to traverse a path. It is commonly used in the decoupled approach to trajectory optimization whereby a path is first found, then a retiming algorithm computes a speed profile that satisfies kino-dynamic and other constraints. While trajectory retiming is most often formulated with the minimum-time objective (i.e. traverse the path as fast as possible), it is not always the most desirable objective, particularly when we seek to balance multiple objectives or when bang-bang control is unsuitable. In this paper, we present a novel algorithm based on factor graph variable elimination that can solve for the global optimum of the retiming problem with quadratic objectives as well (e.g. minimize control effort or match a nominal speed by minimizing squared error), which may extend to arbitrary objectives with iteration. Our work extends prior works, which find only solutions on the boundary of the feasible region, while maintaining the same linear time complexity from a single forward-backward pass. We experimentally demonstrate that (1) we achieve better real-world robot performance by using quadratic objectives in place of the minimum-time objective, and (2) our implementation is comparable or faster than state-of-the-art retiming algorithms.

Robot art presents an opportunity to both showcase and advance state-of-the-art robotics through the challenging task of creating art. Creating large-scale artworks in particular engages the public in a way that small-scale works cannot, and the distinct qualities of brush strokes contribute to an organic and human-like quality. Combining the large scale of murals with the strokes of the brush medium presents an especially impactful result, but also introduces unique challenges in maintaining precise, dextrous motion control of the brush across such a large workspace. In this work, we present the first robot to our knowledge that can paint architectural-scale murals with a brush. We create a hybrid robot consisting of a cable-driven parallel robot and 4 degree of freedom (DoF) serial manipulator to paint a 27m by 3.7m mural on windows spanning 2-stories of a building. We discuss our approach to achieving both the scale and accuracy required for brush-painting a mural through a combination of novel mechanical design elements, coordinated planning and control, and on-site calibration algorithms with experimental validations.

We propose a novel hybrid cable-based robot with manipulator and camera for high-accuracy, medium-throughput plant monitoring in a vertical hydroponic farm and, as an example application, demonstrate non-destructive plant mass estimation. Plant monitoring with high temporal and spatial resolution is important to both farmers and researchers to detect anomalies and develop predictive models for plant growth. The availability of high-quality, off-the-shelf structure-from-motion (SfM) and photogrammetry packages has enabled a vibrant community of roboticists to apply computer vision for non-destructive plant monitoring. While existing approaches tend to focus on either high-throughput (e.g. satellite, unmanned aerial vehicle (UAV), vehicle-mounted, conveyor-belt imagery) or high-accuracy/robustness to occlusions (e.g. turn-table scanner or robot arm), we propose a middle-ground that achieves high accuracy with a medium-throughput, highly automated robot. Our design pairs the workspace scalability of a cable-driven parallel robot (CDPR) with the dexterity of a 4 degree-of-freedom (DoF) robot arm to autonomously image many plants from a variety of viewpoints. We describe our robot design and demonstrate it experimentally by collecting daily photographs of 54 plants from 64 viewpoints each. We show that our approach can produce scientifically useful measurements, operate fully autonomously after initial calibration, and produce better reconstructions and plant property estimates than those of over-canopy methods (e.g. UAV). As example applications, we show that our system can successfully estimate plant mass with a Mean Absolute Error (MAE) of 0.586g and, when used to perform hypothesis testing on the relationship between mass and age, produces p-values comparable to ground-truth data (p=0.0020 and p=0.0016, respectively).

We propose a manifold optimization approach for solving constrained inference and planning problems. The approach employs a framework that transforms an arbitrary nonlinear equality constrained optimization problem into an unconstrained manifold optimization problem. The core of the transformation process is the formulation of constraint manifolds that represent sets of variables subject to equality constraints. We propose various approaches to define the tan-gent spaces and retraction operations of constraint manifolds, which are crucial for manifold optimization. We evaluate our constraint manifold optimization approach on multiple constrained inference and planning problems, and show that it generates strictly feasible results with increased efficiency as compared to state-of-the-art constrained optimization methods.

We present Panoptic Neural Fields (PNF), an object-aware neural scene representation that decomposes a scene into a set of objects (things) and background (stuff). Each object is represented by an oriented 3D bounding box and a multi-layer perceptron (MLP) that takes position, direction, and time and outputs density and radiance. The background stuff is represented by a similar MLP that additionally outputs semantic labels. Each object MLPs are instance-specific and thus can be smaller and faster than previous object-aware approaches, while still leveraging category-specific priors incorporated via meta-learned initialization. Our model builds a panoptic radiance field representation of any scene from just color images. We use off-the-shelf algorithms to predict camera poses, object tracks, and 2D image semantic segmentations. Then we jointly optimize the MLP weights and bounding box parameters using analysis-by-synthesis with self-supervision from color images and pseudo-supervision from predicted semantic segmentations. During experiments with real-world dynamic scenes, we find that our model can be used effectively for several tasks like novel view synthesis, 2D panoptic segmentation, 3D scene editing, and multiview depth prediction.

We present GTGraffiti, a graffiti painting system from Georgia Tech that tackles challenges in art, hardware, and human-robot collaboration. The problem of painting graffiti in a human style is particularly challenging and requires a system-level approach because the robotics and art must be designed around each other. The robot must be highly dynamic over a large workspace while the artist must work within the robot's limitations. Our approach consists of three stages: artwork capture, robot hardware, and planning & control. We use motion capture to capture collaborator painting motions which are then composed and processed into a time-varying linear feedback controller for a cable-driven parallel robot (CDPR) to execute. In this work, we will describe the capturing process, the design and construction of a purpose-built CDPR, and the software for turning an artist's vision into control commands. Our work represents an important step towards faithfully recreating human graffiti artwork by demonstrating that we can reproduce artist motions up to 2m/s and 20m/s2within 9.3mm RMSE to paint artworks.

We propose factor graph optimization for simultaneous planning, control, and trajectory estimation for collision-free navigation of autonomous systems in environments with moving objects. The proposed online probabilistic motion planning and trajectory estimation navigation technique generates optimal collision-free state and control trajectories for autonomous vehicles when the obstacle motion model is both unknown and known. We evaluate the utility of the algorithm to support future autonomous robotic space missions.

We present a locally optimal tracking controller for Cable Driven Parallel Robot (CDPR) control based on a time-varying Linear Quadratic Gaussian (TV-LQG) controller. In contrast to many methods which use fixed feedback gains, our time-varying controller computes the optimal gains depending on the location in the workspace and the future trajectory. Meanwhile, we rely heavily on offline computation to reduce the burden of online implementation and feasibility checking. Following the growing popularity of probabilistic graphical models for optimal control, we use factor graphs as a tool to formulate our controller for their efficiency, intuitiveness, and modularity. The topology of a factor graph encodes the relevant structural properties of equations in a way that facilitates insight and efficient computation using sparse linear algebra solvers. We first use factor graph optimization to compute a nominal trajectory, then linearize the graph and apply variable elimination to compute the locally optimal, time varying linear feedback gains. Next, we leverage the factor graph formulation to compute the locally optimal, time-varying Kalman Filter gains, and finally combine the locally optimal linear control and estimation laws to form a TV-LQG controller. We compare the tracking accuracy of our TV-LQG controller to a state-of-the-art dual-space feed-forward controller on a 2.9m x 2.3m, 4-cable planar robot and demonstrate improved tracking accuracies of 0.8° and 11.6 mm root mean square error in rotation and translation respectively.

In this work, we investigate the problem of incre-mentally solving constrained non-linear optimization problems formulated as factor graphs. Prior incremental solvers were either restricted to the unconstrained case or required periodic batch relinearizations of the objective and constraints which are expensive and detract from the online nature of the algorithm. We present InCOpt, an Augmented Lagrangian-based incremental constrained optimizer that views matrix operations as message passing over the Bayes tree. We first show how the linear system, resulting from linearizing the constrained objective, can be represented as a Bayes tree. We then propose an algorithm that views forward and back substitutions, which naturally arise from solving the Lagrangian, as upward and downward passes on the tree. Using this formulation, In-COpt can exploit properties such as fluid/online relinearization leading to increased accuracy without a sacrifice in runtime. We evaluate our solver on different applications (navigation and manipulation) and provide an extensive evaluation against existing constrained and unconstrained solvers.

We present a manifold optimization approach to solve inference and planning problems with range constraints. The core of our approach is the definition of a manifold that represents points or poses with range constraints. We discover that the manifold of range-constrained points is homogeneous under the rigid transformation group action, and utilize the group action to derive the tangent space, retraction and topology of the manifold. We evaluate the performance of manifold optimization approach on solving range-constrained inference problems over state-of-the-art constrained optimization methods. The results show that manifold optimization with the range-constraint manifold achieves both faster speed and better constraint satisfaction. We further study the conditions of inference problems that we can treat range measurements as constraints in practice.

We present a novel continuous time trajectory representation based on a Chebyshev polynomial basis, which when governed by known dynamics models, allows for full trajectory and robot dynamics estimation, particularly useful for high-performance robotics applications such as unmanned aerial vehicles. We show that we can gracefully incorporate model dynamics to our trajectory representation, within a factor-graph based framework, and leverage ideas from pseudo- spectral optimal control to parameterize the state and the control trajectories as interpolating polynomials. This allows us to perform efficient optimization at specifically chosen points derived from the theory, while recovering full trajectory estimates. Through simulated experiments we demonstrate the applicability of our representation for accurate flight dynamics estimation for multirotor aerial vehicles. The representation framework is general and can thus be applied to a multitude of high-performance applications beyond multirotor platforms.

Roboticists have increasingly sought to incorporate mechanical compliance into legged robots to realize a range of potential benefits, from improved agility to resilience in complex environments. A promising approach for building compliance into robot legs is to utilize the pneumatic artificial muscle, a pneumatic actuator with inherent compliance due to the compressibility of air. While previous work has explored the capabilities of pneumatic-muscle driven robots in highly dynamic tasks like jumping, there is a lack of trajectory planning strategies for such robots. In this paper, we detail our approach to planning vertical jumping trajectories for a planar two-legged robot driven by four pneumatic artificial muscles using on/off "burst inflation" control. The trajectory optimization problem is represented as a factor graph and solved with the GTSAM optimizer. A hybrid dynamics approach is used to handle foot-ground contacts. The average jump height error between simulation and experiment across multiple jumping trajectories of varying heights was 9.5 cm; the average RMS error between all four joints was 5.6 deg. This work provides a basis to plan more complex jumping and leaping trajectories for pneumatic muscle-driven robots.

This paper presents a novel factor graph-based approach to solve the discrete-time finite-horizon Linear Quadratic Regulator problem subject to auxiliary linear equality constraints within and across time steps. We represent such optimal control problems using constrained factor graphs and optimize the factor graphs to obtain the optimal trajectory and the feedback control policies using the variable elimination algorithm with a modified Gram-Schmidt process. We prove that our approach has the same order of computational complexity as the state-of-the-art dynamic programming approach. Furthermore, current dynamic programming approaches can only handle equality constraints between variables at the same time step, but ours can handle equality constraints among any combination of variables at any time step while maintaining linear complexity with respect to trajectory length. Our approach can be used to efficiently generate trajectories and feedback control policies to achieve periodic motion or repetitive manipulation.

We present multi-robot iSAM2 (MR-iSAM2), an efficient incremental smoothing and mapping (iSAM) algorithm to solve multi-robot simultaneous localization and mapping (SLAM) inference problems. MR-iSAM2 is based on a novel data structure multi-root Bayes tree (MRBT), which packs multiple Bayes trees with the same undirected clique structure. In multi-robot scenarios, the MRBT enables new measurements from different robots to be updated in different root branches, while all updates are performed around the single root of the Bayes tree in the original iSAM2 algorithm. As a result, the MRBT better reveals the underlying sparsity and information flow in multi-robot SLAM inference problems than the Bayes tree. Based on this insight, we further develop MR-iSAM2 to incrementally update and maintain the sparsity structure of the MRBT and enable efficient information propagation among the roots for inter-robot inference. We analyze the properties of the MR-iSAM2 algorithm, and show with both synthetic and real world datasets that it significantly outperforms iSAM2 in efficiency when solving multi-robot SLAM problems.

Chinese calligraphy is a unique art form with great artistic value but difficult to master. In this paper, we formulate the calligraphy writing problem as a trajectory optimization problem, and propose an improved virtual brush model for simulating the real writing process. Our approach is inspired by pseudospectral optimal control in that we parameterize the actuator trajectory for each stroke as a Chebyshev polynomial. The proposed dynamic virtual brush model plays a key role in formulating the objective function to be optimized. Our approach shows excellent performance in drawing aesthetically pleasing characters, and does so much more efficiently than previous work, opening up the possibility to achieve real-time closed-loop control.

In this paper, we provide a modern synthesis of the classic inverse compositional algorithm for dense image alignment. We first discuss the assumptions made by this well-established technique, and subsequently propose to relax these assumptions by incorporating data-driven priors into this model. More specifically, we unroll a robust version of the inverse compositional algorithm and replace multiple components of this algorithm using more expressive models whose parameters we train in an end-to-end fashion from data. Our experiments on several challenging 3D rigid motion estimation tasks demonstrate the advantages of combining optimization with learning-based techniques, outperforming the classic inverse compositional algorithm as well as data-driven image-to-pose regression approaches.

Image alignment tasks require accurate pixel correspondences, which are usually recovered by matching local feature descriptors. Such descriptors are often derived using supervised learning on existing datasets with ground truth correspondences. However, the cost of creating such datasets is usually prohibitive. In this paper, we propose a new approach to align two images related by an unknown 2D homography where the local descriptor is learned from scratch from the images and the homography is estimated simultaneously. Our key insight is that a siamese convolutional neural network can be trained jointly while iteratively updating the homography parameters by optimizing a single loss function. Our method is currently weakly supervised because the input images need to be roughly aligned. We have used this method to align images of different modalities such as RGB and near-infra-red (NIR) without using any prior labeled data. Images automatically aligned by our method were then used to train descriptors that generalize to new images. We also evaluated our method on RGB images. On the HPatches benchmark, our method achieves comparable accuracy to deep local descriptors that were trained offline in a supervised setting.

We present an approach to exploit complex 3D symmetries exhibited by urban structures in order to improve 3D reconstruction be used, ranging from primitive symmetries [18] to complex symmetry groups. We achieve this by rigorously modeling complex symmetries, using the mathematical theory underlying them to develop generative models of 3D points involved in a given scene symmetry. A key element of our approach is to express the 3D structure of a symmetric scene, imaged by a camera, as comprised of non-decomposable 3D elements known as asymmetric units. We apply our approach in both single and multi-view settings and show that we can obtain denser, more compressed, and qualitatively better-looking representations of the observed structure by exploiting constraints derived from symmetry.

Continuous-time trajectories are useful for reasoning about robot motion in a wide range of tasks. Sparse Gaussian processes (GPs) can be used as a non-parametric representation for trajectory distributions that enables fast trajectory optimization by sparse GP regression. However, most previous approaches that utilize sparse GPs for trajectory optimization are limited by the fact that the robot state is represented in vector space. In this paper, we first extend previous works to consider the state on general matrix Lie groups by applying a constant-velocity prior and defining locally linear GPs. Next, we discuss how sparse GPs on Lie groups provide a unified continuous-time framework for trajectory optimization for solving a number of robotics problems including state estimation and motion planning. Finally, we demonstrate and evaluate our approach on several different estimation and motion planning tasks with both synthetic and real-world experiments.

Autonomous crop monitoring at high spatial and temporal resolution is a critical problem in precision agriculture. While Structure from Motion and Multi-View Stereo algorithms can finely reconstruct the 3D structure of a field with low-cost image sensors, these algorithms fail to capture the dynamic nature of continuously growing crops. In this paper we propose a 4D reconstruction approach to crop monitoring, which employs a spatio-temporal model of dynamic scenes that is useful for precision agriculture applications. Additionally, we provide a robust data association algorithm to address the problem of large appearance changes due to scenes being viewed from different angles at different points in time, which is critical to achieving 4D reconstruction. Finally, we collected a high-quality dataset with ground-truth statistics to evaluate the performance of our method. We demonstrate that our 4D reconstruction approach provides models that are qualitatively correct with respect to visual appearance and quantitatively accurate when measured against the ground truth geometric properties of the monitored crops.

Current approaches for visual-inertial odometry (VIO) are able to attain highly accurate state estimation via nonlinear optimization. However, real-time optimization quickly becomes infeasible as the trajectory grows over time; this problem is further emphasized by the fact that inertial measurements come at high rate, hence, leading to the fast growth of the number of variables in the optimization. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes into single relative motion constraints. Our first contribution is a preintegration theory that properly addresses the manifold structure of the rotation group. We formally discuss the generative measurement model as well as the nature of the rotation noise and derive the expression for the maximum a posteriori state estimator. Our theoretical development enables the computation of all necessary Jacobians for the optimization and a posteriori bias correction in analytic form. The second contribution is to show that the preintegrated inertial measurement unit model can be seamlessly integrated into a visual-inertial pipeline under the unifying framework of factor graphs. This enables the application of incremental-smoothing algorithms and the use of a structureless model for visual measurements, which avoids optimizing over the 3-D points, further accelerating the computation. We perform an extensive evaluation of our monocular VIO pipeline on real and simulated datasets. The results confirm that our modeling effort leads to an accurate state estimation in real time, outperforming state-of-the-art approaches.

We propose a distributed algorithm to estimate the 3D trajectories of multiple cooperative robots from relative pose measurements. Our approach leverages recent results [1] which show that the maximum likelihood trajectory is well approximated by a sequence of two quadratic subproblems. The main contribution of the present work is to show that these subproblems can be solved in a distributed manner, using the distributed Gauss-Seidel (DGS) algorithm. Our approach has several advantages. It requires minimal information exchange, which is beneficial in presence of communication and privacy constraints. It has an anytime flavor: after few iterations the trajectory estimates are already accurate, and they asymptotically convergence to the centralized estimate. The DGS approach scales well to large teams, and it has a straightforward implementation. We test the approach in simulations and field tests, demonstrating its advantages over related techniques.

Pose graph optimization (PGO) is the problem of estimating a set of poses from pairwise relative measurements. PGO is a nonconvex problem and, currently, no known technique can guarantee the computation of a global optimal solution. In this paper, we show that Lagrangian duality allows computing a globally optimal solution under conditions that are satisfied in most robotics applications and enables to certify optimality of a given estimate. Our first contribution is to frame planar PGO in the complex domain. This makes analysis easier and allows drawing connections with existing literature on unit gain graphs. The second contribution is to formulate and analyze the properties of the Lagrangian dual problem in the complex domain. Our analysis shows that the duality gap is connected to the number of zero eigenvalues of the penalized pose graph matrix. We prove that if this matrix has a single zero eigenvalue, then 1) the duality gap is zero, 2) the primal PGO problem has a unique solution (up to an arbitrary roto-translation), and 3) the primal solution can be computed by scaling an eigenvector of the penalized pose graph matrix. The third contribution is algorithmic: We leverage duality to devise and algorithm that computes the optimal solution when the penalized matrix has a single zero eigenvalue. We also propose a suboptimal variant when the zero eigenvalues are multiple. Finally, we show that duality provides computational tools to verify if a given estimate (e.g., computed using iterative solvers) is globally optimal. We conclude the paper with an extensive numerical analysis. Empirical evidence shows that, in the vast majority of cases (100% of the tests under noise regimes of practical robotics applications), the penalized pose graph matrix has a single zero eigenvalue; hence, our approach allows computing (or verifying) the optimal solution.

As the amount of visual data increases, so does the need for summarization tools that can be used to explore large image collections and to quickly get familiar with their content. In this paper, we propose dataset fingerprints, a new and powerful method based on data mining that extracts meaningful patterns from a set of images. The discovered patterns are compositions of discriminative mid-level features that co-occur in several images. Compared to earlier work, ours stands out because i) it's fully unsupervised, ii) discovered patterns cover large parts of the images, often corresponding to full objects or meaningful parts thereof, and iii) different patterns are connected based on co-occurrence, allowing a user to “browse” the images from one pattern to the next and to group patterns in a semantically meaningful manner.

While iterative optimization techniques for Simultaneous Localization and Mapping (SLAM) are now very efficient and widely used, none of them can guarantee global convergence to the maximum likelihood estimate. Local convergence usually implies artifacts in map reconstruction and large localization errors, hence it is very undesirable for applications in which accuracy and safety are of paramount importance. We provide a technique to verify if a given 2D SLAM solution is globally optimal. The insight is that, while computing the optimal solution is hard in general, duality theory provides tools to compute tight bounds on the optimal cost, via convex programming. These bounds can be used to evaluate the quality of a SLAM solution, hence providing a “sanity check” for state-of-the-art incremental and batch solvers. Experimental results show that our technique successfully identifies wrong estimates (i.e., local minima) in large-scale SLAM scenarios. This work, together with [1], represents a step towards the objective of having SLAM techniques with guaranteed performance, that can be used in safety-critical applications.

Pose graph optimization is the non-convex optimization problem underlying pose-based Simultaneous Localization and Mapping (SLAM). If robot orientations were known, pose graph optimization would be a linear least-squares problem, whose solution can be computed efficiently and reliably. Since rotations are the actual reason why SLAM is a difficult problem, in this work we survey techniques for 3D rotation estimation. Rotation estimation has a rich history in three scientific communities: robotics, computer vision, and control theory. We review relevant contributions across these communities, assess their practical use in the SLAM domain, and benchmark their performance on representative SLAM problems (Fig. 1). We show that the use of rotation estimation to bootstrap iterative pose graph solvers entails significant boost in convergence speed and robustness.

In this paper, we present an information-based approach to select a reduced number of landmarks and poses for a robot to localize itself and simultaneously build an accurate map. We develop an information theoretic algorithm to efficiently reduce the number of landmarks and poses in a SLAM estimate without compromising the accuracy of the estimated trajectory. We also propose an incremental version of the reduction algorithm which can be used in SLAM framework resulting in information based reduced landmark SLAM. The results of reduced landmark based SLAM algorithm are shown on Victoria park dataset and a Synthetic dataset and are compared with standard graph SLAM (SAM [6]) algorithm. We demonstrate a reduction of 40-50% in the number of landmarks and around 55% in the number of poses with minimal estimation error as compared to standard SLAM algorithm.

We demonstrate distributed, online, and real-time cooperative localization and mapping between multiple robots operating throughout an unknown environment using indirect measurements. We present a novel Expectation Maximization (EM) based approach to efficiently identify inlier multi-robot loop closures by incorporating robot pose uncertainty, which significantly improves the trajectory accuracy over long-term navigation. An EM and hypothesis based method is used to determine a common reference frame. We detail a 2D laser scan correspondence method to form robust correspondences between laser scans shared amongst robots. The implementation is experimentally validated using teams of aerial vehicles, and analyzed to determine its accuracy, computational efficiency, scalability to many robots, and robustness to varying environments. We demonstrate through multiple experiments that our method can efficiently build maps of large indoor and outdoor environments in a distributed, online, and real-time setting.

This paper studies an optimization-based approach for solving optimal estimation and optimal control problems through a unified computational formulation. The goal is to perform trajectory estimation over extended past horizons and model-predictive control over future horizons by enforcing the same dynamics, control, and sensing constraints in both problems, and thus solving both problems with identical computational tools. Through such systematic estimation-control formulation we aim to improve the performance of autonomous systems such as agile robotic vehicles. This work focuses on sequential sweep trajectory optimization methods, and more specifically extends the method known as differential dynamic programming to the parameter-dependent setting in order to enable the solutions to general estimation and control problems.

We propose a method of monocular camera-inertial based navigation for computationally limited micro air vehicles (MAVs). Our approach is derived from the recent development of parallel tracking and mapping algorithms, but unlike previous results, we show how the tracking and mapping processes operate using different representations. The separation of representations allows us not only to move the computational load of full map inference to a ground station, but to further reduce the computational cost of on-board tracking for pose estimation. Our primary contribution is to show how the cost of tracking the vehicle pose on-board can be substantially reduced by estimating the camera motion directly in the image frame, rather than in the world co-ordinate frame. We demonstrate our method on an Ascending Technologies Pelican quad-rotor, and show that we can track the vehicle pose with reduced on-board computation but without compromised navigation accuracy.

State-of-the-art techniques for simultaneous localization and mapping (SLAM) employ iterative nonlinear optimization methods to compute an estimate for robot poses. While these techniques often work well in practice, they do not provide guarantees on the quality of the estimate. This paper shows that Lagrangian duality is a powerful tool to assess the quality of a given candidate solution. Our contribution is threefold. First, we discuss a revised formulation of the SLAM inference problem. We show that this formulation is probabilistically grounded and has the advantage of leading to an optimization problem with quadratic objective. The second contribution is the derivation of the corresponding Lagrangian dual problem. The SLAM dual problem is a (convex) semidefinite program, which can be solved reliably and globally by off-the-shelf solvers. The third contribution is to discuss the relation between the original SLAM problem and its dual. We show that from the dual problem, one can evaluate the quality (i.e., the suboptimality gap) of a candidate SLAM solution, and ultimately provide a certificate of optimality. Moreover, when the duality gap is zero, one can compute a guaranteed optimal SLAM solution from the dual problem, circumventing non-convex optimization. We present extensive (real and simulated) experiments supporting our claims and discuss practical relevance and open problems.

Large-scale SLAM demands for scalable techniques in which the computational burden and the memory consumption is shared among many processing units. While recent literature offers competitive approaches for scalable mapping, these usually involve approximations to preserve sparsity of the resulting subproblems. We present an approach to scalable SLAM that is exactly sparse. The main insight is that rather than eliminating variables (which induces dense cliques), we split the separators connecting subgraphs. Then, we enforce consistency of the separators in different subgraphs using hard constraints. The resulting constrained optimization problem can be solved in a decentralized manner using the multi-block Alternating Direction Method of Multipliers (ADMM). Our framework is appealing since (i) it preserves the sparsity structure of the original problem, (ii) it has a straightforward implementation, (iii) it allows to easily trade-off between computation time and accuracy. While our approach is currently slower than competitors, it is more accurate than other memory efficient alternatives. Moreover, we believe that the proposed framework can be of interest on its own as it draws connections with recent literature on decentralized optimization.

In this paper, we propose a reconstruction technique that uses 2D regions/superpixels rather than point features. We use pre-segmented RGBD data as input and obtain piecewise planar 3D models of the world. We solve the problem of superpixel labeling within single and multiple views simultaneously by using a Rao-Blackwellized Markov Chain Monte Carlo (MCMC) algorithm. We present our output as a labeled 3D model of the world by integrating out ov er all possible 3D planes in a fully Bayesian fashion. We present our results on the new SUN3D dataset [1].

This paper proposes using hierarchical wavelets as a basis in parametric continuous-time batch estimation. The need for a continuous-time robot pose in the simultaneous localization and mapping (SLAM) problem has arisen as state-of-the-art batch SLAM algorithms attempt to handle more challenging hardware; specifically, the continuous-time framework is particularly beneficial when using high-rate sensors, multiple unsynchronized sensors, or scanning sensors, such as lidar and rolling-shutter cameras, during motion. Although the traditional discrete-time SLAM formulation can be adapted by using temporal pose interpolation, approaches using the continuous-time framework are able to generate smooth robot trajectories with less state variables. In this paper, we focus on the parametric approach using temporal basis functions to develop a finite-element representation of the continuous-time robot trajectory. While the majority of current implementations have utilized a uniformly spaced B-spline basis, we note that trajectory richness is often quite variable; in this paper, we show how a hierarchical system of wavelet basis functions can be used to increase the resolution of the solution only in the temporally local regions of the trajectory that require additional detail. We validate our approach by contrasting uniform B-splines and wavelets in a six-dimensional pose-graph SLAM experiment, using both simulated and real data.

Factor graphs are a general estimation framework that has been widely used in computer vision and robotics. In several classes of problems a natural partition arises among variables involved in the estimation. A subset of the variables are actually of interest for the user: we call those target variables. The remaining variables are essential for the formulation of the optimization problem underlying maximum a posteriori (MAP) estimation; however these variables, that we call support variables, are not strictly required as output of the estimation problem. In this paper, we propose a systematic way to abstract support variables, defining optimization problems that are only defined over the set of target variables. This abstraction naturally leads to the definition of smart factors, which correspond to constraints among target variables. We show that this perspective unifies the treatment of heterogeneous problems, ranging from structureless bundle adjustment to robust estimation in SLAM. Moreover, it enables to exploit the underlying structure of the optimization problem and the treatment of degenerate instances, enhancing both computational efficiency and robustness.

This paper proposes a real-time navigation approach that is able to integrate many sensor types while fulfilling performance needs and system constraints. Our approach uses a plug-and-play factor graph framework, which extends factor graph formulation to encode sensor measurements with different frequencies, latencies, and noise distributions. It provides a flexible foundation for plug-and-play sensing, and can incorporate new evolving sensors. A novel constrained optimal selection mechanism is presented to identify the optimal subset of active sensors to use, during initialization and when any sensor condition changes. This mechanism constructs candidate subsets of sensors based on heuristic rules and a ternary tree expansion algorithm. It quickly decides the optimal subset among candidates by maximizing observability coverage on state variables, while satisfying resource constraints and accuracy demands. Experimental results demonstrate that our approach selects subsets of sensors to provide satisfactory navigation solutions under various conditions, on large-scale real data sets using many sensors.

Using the inverse sensor model has been popular in occupancy grid mapping. However, it is widely known that applying the inverse sensor model to mapping requires certain assumptions that are not necessarily true. Even the works that use forward sensor models have relied on methods like expectation maximization or Gibbs sampling which have been succeeded by more effective methods of maximum a posteriori (MAP) inference over graphical models. In this paper, we propose the use of modern MAP inference methods along with the forward sensor model. Our implementation and experimental results demonstrate that these modern inference methods deliver more accurate maps more efficiently than previously used methods.

This work investigates the problem of planning under uncertainty, with application to mobile robotics. We propose a probabilistic framework in which the robot bases its decisions on the generalized belief, which is a probabilistic description of its own state and of external variables of interest. The approach naturally leads to a dual-layer architecture: an inner estimation layer, which performs inference to predict the outcome of possible decisions, and an outer decisional layer which is in charge of deciding the best action to undertake. The approach does not discretize the state or control space, and allows planning in continuous domain. Moreover, it allows to relax the assumption of maximum likelihood observations: predicted measurements are treated as random variables and are not considered as given. Experimental results show that our planning approach produces smooth trajectories while maintaining uncertainty within reasonable bounds.

This paper presents a novel approach for multirobot pose graph localization and data association without requiring prior knowledge about the initial relative poses of the robots. Without a common reference frame, the robots can only share observations of interesting parts of the environment, and trying to match between observations from different robots will result in many outlier correspondences. Our approach is based on the following key observation: while each multi-robot correspondence can be used in conjunction with the local robot estimated trajectories, to calculate the transformation between the robot reference frames, only the inlier correspondences will be similar to each other. Using this concept, we develop an expectation-maximization (EM) approach to efficiently infer the robot initial relative poses and solve the multi-robot data association problem. Once this transformation between the robot reference frames is estimated with sufficient measure of confidence, we show that a similar EM formulation can be used to solve also the full multi-robot pose graph problem with unknown multi-robot data association. We evaluate the performance of the developed approach both in a statistical synthetic-environment study and in a real-data experiment, demonstrating its robustness to high percentage of outliers.

Towards the goal of autonomous obstacle avoidance for mobile robots, we present a method for superpixel labeling using optical flow templates. Optical flow provides a rich source of information that complements image appearance and point clouds in determining traversability. While much past work uses optical flow towards traversability in a heuristic manner, the method we present here instead classifies flow according to several optical flow templates that are specific to the typical environment shape. Our first contribution over prior work in superpixel labeling using optical flow templates is large improvements in accuracy and efficiency by inference directly from spatiotemporal gradients instead of from independently-computed optical flow, and from improved optical flow modeling for obstacles. Our second contribution over the same is extending superpixel labeling methods to arbitrary camera optics without the need to calibrate the camera, by developing and demonstrating a method for learning optical flow templates from unlabeled video. Our experiments demonstrate successful obstacle detection in an outdoor mobile robot dataset.

Pose graph optimization is an elegant and efficient formulation for robot localization and mapping. Experimental evidence suggests that, in real problems, the set of measurements used to estimate robot poses is prone to contain outliers, due to perceptual aliasing and incorrect data association. While several related works deal with the rejection of outliers during pose estimation, the goal of this paper is to propose a grounded strategy for measurements selection, i.e., the output of our approach is a set of “reliable” measurements, rather than pose estimates. Because the classification in inliers/outliers is not observable in general, we pose the problem as finding the maximal subset of the measurements that is internally coherent. In the linear case, we show that the selection of the maximal coherent set can be (conservatively) relaxed to obtain a linear programming problem with ℓ1objective. We show that this approach can be extended to (nonlinear) planar pose graph optimization using similar ideas as our previous work on linear approaches to pose graph optimization. We evaluate our method on standard datasets, and we show that it is robust to a large number of outliers and different outlier generation models, while entailing the advantages of linear programming (fast computation, scalability).

Object discovery and modeling have been widely studied in the computer vision and robotics communities. SLAM approaches that make use of objects and higher level features have also recently been proposed. Using higher level features provides several benefits: these can be more discriminative, which helps data association, and can serve to inform service robotic tasks that require higher level information, such as object models and poses. We propose an approach for online object discovery and object modeling, and extend a SLAM system to utilize these discovered and modeled objects as landmarks to help localize the robot in an online manner. Such landmarks are particularly useful for detecting loop closures in larger maps. In addition to the map, our system outputs a database of detected object models for use in future SLAM or service robotic tasks. Experimental results are presented to demonstrate the approach's ability to detect and model objects, as well as to improve SLAM results by detecting loop closures.

We propose a novel method to solve online SLAM problems with many loop-closures on the basis of two state-of-the-art SLAM methods, iSAM and SPCG. We first use iSAM to solve a sparse sub-problem to obtain an approximate solution. When the error grows larger than a threshold or the optimal solution is requested, we use subgraph-preconditioned conjugate gradient method to solve the original problem where the subgraph preconditioner and initial estimate are provided by iSAM. Finally we use the optimal solution from SPCG to regularize iSAM in the next steps. The proposed method is consistent, efficient and can find the optimal solution. We apply this method to solve large simulated and real SLAM problems, and obtain promising results.

We present two fast and memory-efficient approximate estimation methods, targeting obstacle avoidance applications on small robot platforms. Our methods avoid a main bottleneck of traditional filtering techniques, which creates densely correlated cliques of landmarks, leading to expensive time and space complexity. We introduce a novel technique to avoid the dense cliques by sparsifying them into a tree structure and maintain that tree structure efficiently over time. Unlike other edge removal graph sparsification methods, our methods sparsify the landmark cliques by introducing new variables to de-correlate them. The first method projects the current density onto a tree rooted at the same variable at each step. The second method improves upon the first one by carefully choosing a new low-dimensional root variable at each step to replace such that the independence and conditional densities of the landmarks given the trajectory are optimally preserved. Our experiments show a significant improvement in time and space complexity of the methods compared to other standard filtering techniques in worst-case scenarios, with small trade-offs in accuracy due to low-rank approximation errors.

This paper proposes a navigation algorithm that provides a low-latency solution while estimating the full nonlinear navigation state. Our approach uses Sliding-Window Factor Graphs, which extend existing incremental smoothing methods to operate on the subset of measurements and states that exist inside a sliding time window. We split the estimation into a fast short-term smoother, a slower but fully global smoother, and a shared map of 3D landmarks. A novel three-stage visual feature model is presented that takes advantage of both smoothers to optimize the 3D landmark map, while minimizing the computation required for processing tracked features in the short-term smoother. This three-stage model is formulated based on the maturity of the estimation of the 3D location of the underlying landmark in the map. Long-range associations are used as global measurements from matured landmarks in the short-term smoother and loop closure constraints in the long-term smoother. Experimental results demonstrate our approach provides highly-accurate solutions on large-scale real data sets using multiple sensors in GPS-denied settings.

This paper presents an consistent decentralized data fusion approach for robust multi-robot SLAM in dangerous, unknown environments. The DDF-SAM 2.0 approach extends our previous work by combining local and neighborhood information in a single, consistent augmented local map, without the overly conservative approach to avoiding information double-counting in the previous DDF-SAM algorithm. We introduce the anti-factor as a means to subtract information in graphical SLAM systems, and illustrate its use to both replace information in an incremental solver and to cancel out neighborhood information from shared summarized maps. This paper presents and compares three summarization techniques, with two exact approaches and an approximation. We evaluated the proposed system in a synthetic example and show the augmented local system and the associated summarization technique do not double-count information, while keeping performance tractable.

In this paper, a two-level path planning algorithm that deals with map uncertainty is proposed. The higher level planner uses modified generalized Voronoi diagrams to guarantee finding a connected path from the start to the goal if a collision-free path exists. The lower level planner considers uncertainty of the observed obstacles in the environment and assigns repulsive forces based on their distance to the robot and their positional uncertainty. The attractive forces from the Voronoi nodes and the repulsive forces from the uncertainty-biased potential fields form a hybrid planner we call Voronoi Uncertainty Fields (VUF). The proposed planner has two strong properties: (1) bias against uncertain obstacles, and (2) completeness. We analytically prove the properties and run simulations to validate our method in a forest-like environment.

This paper presents a new computationally-efficient method for vision-aided navigation (VAN) in autonomous robotic applications. While many VAN approaches are capable of processing incoming visual observations, incorporating loop-closure measurements typically requires performing a bundle adjustment (BA) optimization, that involves both all the past navigation states and the observed 3D points. Our approach extends the incremental light bundle adjustment (LBA) method, recently developed for structure from motion [10], to information fusion in robotics navigation and in particular for including loop-closure information. Since in many robotic applications the prime focus is on navigation rather then mapping, and as opposed to traditional BA, we algebraically eliminate the observed 3D points and do not explicitly estimate them. Computational complexity is further improved by applying incremental inference. To maintain highrate performance over time, consecutive IMU measurements are summarized using a recently-developed technique and navigation states are added to the optimization only at camera rate. If required, the observed 3D points can be reconstructed at any time based on the optimized robot's poses. The proposed method is compared to BA both in terms of accuracy and computational complexity in a statistical simulation study.

Efficiently solving large-scale sparse linear systems is important for robot mapping and navigation. Recently, the subgraph-preconditioned conjugate gradient method has been proposed to combine the advantages of two reigning paradigms, direct and iterative methods, to improve the efficiency of the solver. Yet the question of how to pick a good subgraph is still an open problem. In this paper, we propose a new metric to measure the quality of a spanning tree preconditioner based on support theory. We use this metric to develop an algorithm to find good subgraph preconditioners and apply them to solve the SLAM problem. The results show that although the proposed algorithm is not fast enough, the new metric is effective and resulting subgraph preconditioners significantly improve the efficiency of the state-of-the-art solver.

Presented at the Ninth Conference on 23rd British Machine Vision Conference (BMVC 2012), 3-7 September 2012, Guildford, Surrey, UK.

In this paper we focus on the multi-robot perception problem, and present an experimentally validated end-to-end multi-robot mapping framework, enabling individual robots in a team to see beyond their individual sensor horizons. The inference part of our system is the DDF-SAM algorithm [1], which provides a decentralized communication and inference scheme, but did not address the crucial issue of data association. One key contribution is a novel, RANSAC-based, approach for performing the between-robot data associations and initialization of relative frames of reference. We demonstrate this system with both data collected from real robot experiments, as well as in a large scale simulated experiment demonstrating the scalability of the proposed approach.

In this paper we present an algorithm for constructing nearly drift-free 3D occupancy grids of large indoor environments in an online manner. Our approach combines data from an odometry sensor with output from a visual registration algorithm, and it enforces a Manhattan world constraint by utilizing factor graphs to produce an accurate online estimate of the trajectory of a mobile robotic platform. We also examine the advantages and limitations of the octree data structure representation of a 3D environment. Through several experiments in environments with varying sizes and construction we show that our method reduces rotational and translational drift significantly without performing any loop closing techniques.

We present a generalized subgraph preconditioning (GSP) technique to solve large-scale bundle adjustment problems efficiently. In contrast with previous work which uses either direct or iterative methods as the linear solver, GSP combines their advantages and is significantly faster on large datasets. Similar to [11], the main idea is to identify a sub-problem (subgraph) that can be solved efficiently by sparse factorization methods and use it to build a preconditioner for the conjugate gradient method. The difference is that GSP is more general and leads to much more effective preconditioners. We design a greedy algorithm to build subgraphs which have bounded maximum clique size in the factorization phase, and also result in smaller condition numbers than standard preconditioning techniques. When applying the proposed method to the “bal” datasets [1], GSP displays promising performance.

A crucial step in many vision based applications, such as localization and structure from motion, is the data association between a large map of known 3D points and 2D features perceived by a new camera. In this paper, we propose a novel approach to predict the visibility of known 3D points with respect to a query camera in large-scale environments. In our approach, we model the visibility of each 3D point with respect to a camera pose using a memory-based learning algorithm, in which a distance metric between cameras is learned in an entirely non-parametric way. We show that by fully exploiting the geometric relationships between the 3D map and the camera poses, as well as the related appearance information, the resulting prediction is much more robust and efficient than conventional approaches. We demonstrate the performance of our algorithm on a large urban 3D model in terms of both speed and accuracy.

We present iSAM2, a fully incremental, graph-based version of incremental smoothing and mapping (iSAM). iSAM2 is based on a novel graphical model-based interpretation of incremental sparse matrix factorization methods, afforded by the recently introduced Bayes tree data structure. The original iSAM algorithm incrementally maintains the square root information matrix by applying matrix factorization updates. We analyze the matrix updates as simple editing operations on the Bayes tree and the conditional densities represented by its cliques. Based on that insight, we present a new method to incrementally change the variable ordering which has a large effect on efficiency. The efficiency and accuracy of the new method is based on fluid relinearization, the concept of selectively relinearizing variables as needed. This allows us to obtain a fully incremental algorithm without any need for periodic batch steps. We analyze the properties of the resulting algorithm in detail, and show on various real and simulated datasets that the iSAM2 algorithm compares favorably with other recent mapping algorithms in both quality and efficiency.

In this paper, we propose a method to recover the relative pose of two robots in absolute scale and in real-time using one monocular camera on each robot. We achieve this by fusing measurements from the onboard inertial sensors on each platform with information obtained from feature correspondences between the two cameras using an Extended Kalman Filter (EKF). This forms a flexible stereo rig, providing the ability to treat the two robots as one single dynamic sensor, which can adapt to the environment and thus improve environmental mapping, obstacle avoidance and navigation. We demonstrate the power of this approach on both simulation and real datasets, employing two micro aerial vehicles (MAVs) to illustrate successful operation over general 3D motion.

Modern structure from motion techniques are capable of building city-scale 3D reconstructions from large image collections, but have mostly ignored the problem of large-scale structural changes over time. We present a general framework for estimating temporal variables in structure from motion problems, including an unknown date for each camera and an unknown time interval for each structural element. Given a collection of images with mostly unknown or uncertain dates, we use this framework to automatically recover the dates of all images by reasoning probabilistically about the visibility and existence of objects in the scene. We present results on a collection of over 100 historical images of a city taken over decades of time.

One of the main drawbacks of standard visual EKF-SLAM techniques is the assumption of a general camera motion model. Usually this motion model has been implemented in the literature as a constant linear and angular velocity model. Because of this, most approaches cannot deal with sudden camera movements, causing them to lose accurate camera pose and leading to a corrupted 3D scene map. In this work we propose increasing the robustness of EKF-SLAM techniques by replacing this general motion model with a visual odometry prior, which provides a real-time relative pose prior by tracking many hundreds of features from frame to frame. We perform fast pose estimation using the two-stage RANSAC-based approach from [1]: a two-point algorithm for rotation followed by a one-point algorithm for translation. Then we integrate the estimated relative pose into the prediction step of the EKF. In the measurement update step, we only incorporate a much smaller number of landmarks into the 3D map to maintain real-time operation. Incorporating the visual odometry prior in the EKF process yields better and more robust localization and mapping results when compared to the constant linear and angular velocity model case. Our experimental results, using a handheld stereo camera as the only sensor, clearly show the benefits of our method against the standard constant velocity model.

We aim to perform robust and fast vision-based localization using a pre-existing large map of the scene. A key step in localization is associating the features extracted from the image with the map elements at the current location. Although the problem of data association has greatly benefited from recent advances in appearance-based matching methods, less attention has been paid to the effective use of the geometric relations between the 3D map and the camera in the matching process. In this paper we propose to exploit the geometric relationship between the 3D map and the camera pose to determine the visibility of the features. In our approach, we model the visibility of every map feature with respect to the camera pose using a non-parametric distribution model. We learn these non-parametric distributions during the 3D reconstruction process, and develop efficient algorithms to predict the visibility of features during localization. With this approach, the matching process only uses those map features with the highest visibility score, yielding a much faster algorithm and superior localization results. We demonstrate an integrated system based on the proposed idea and highlight its potential benefits for the localization in large and cluttered environments.

Environmental change is a growing international concern, calling for the regular monitoring, studying and preserving of detailed information about the evolution of underwater ecosystems. For example, fragile coral reefs are exposed to various sources of hazards and potential destruction, and need close observation. Computer vision offers promising technologies to build 3D models of an environment from two-dimensional images. The state of the art techniques have enabled high-quality digital reconstruction of large-scale structures, e.g., buildings and urban environments, but only sparse representations or dense reconstruction of small objects have been obtained from underwater video and still imagery. The application of standard 3D reconstruction methods to challenging underwater environments typically produces unsatisfactory results. Accurate, full camera trajectories are needed to serve as the basis for dense 3D reconstruction. A highly accurate sparse 3D reconstruction is the ideal foundation on which to base subsequent dense reconstruction algorithms. In our application the models are constructed from synchronized high definition videos collected using a wide baseline stereo rig. The rig can be hand-held, attached to a boat, or even to an autonomous underwater vehicle. We solve this problem by employing a smoothing and mapping toolkit developed in our lab specifically for this type of application. The result of our technique is a highly accurate sparse 3D reconstruction of underwater structures such as corals.

We address the problem of multi-robot distributed SLAM with an extended Smoothing and Mapping (SAM) approach to implement Decentralized Data Fusion (DDF). We present DDF-SAM, a novel method for efficiently and robustly distributing map information across a team of robots, to achieve scalability in computational cost and in communication bandwidth and robustness to node failure and to changes in network topology. DDF-SAM consists of three modules: (1) a local optimization module to execute single-robot SAM and condense the local graph; (2) a communication module to collect and propagate condensed local graphs to other robots, and (3) a neighborhood graph optimizer module to combine local graphs into maps describing the neighborhood of a robot. We demonstrate scalability and robustness through a simulated example, in which inference is consistently faster than a comparable naive approach.

In this paper we propose an efficient preconditioned conjugate gradients (PCG) approach to solving large-scale SLAM problems. While direct methods, popular in the literature, exhibit quadratic convergence and can be quite efficient for sparse problems, they typically require a lot of storage and efficient elimination orderings to be found. In contrast, iterative optimization methods only require access to the gradient and have a small memory footprint, but can suffer from poor convergence. Our new method, subgraph preconditioning, is obtained by re-interpreting the method of conjugate gradients in terms of the graphical model representation of the SLAM problem. The main idea is to combine the advantages of direct and iterative methods, by identifying a sub-problem that can be easily solved using direct methods, and solving for the remaining part using PCG. The easy sub-problems correspond to a spanning tree, a planar subgraph, or any other substructure that can be efficiently solved. As such, our approach provides new insights into the performance of state of the art iterative SLAM methods based on re-parameterized stochastic gradient descent. The efficiency of our new algorithm is illustrated on large datasets, both simulated and real.

We propose a novel batch algorithm for SLAM problems that distributes the workload in a hierarchical way. We show that the original SLAM graph can be recursively partitioned into multiple-level submaps using the nested dissection algorithm, which leads to the cluster tree, a powerful graph representation. By employing the nested dissection algorithm, our algorithm greatly minimizes the dependencies between two subtrees, and the optimization of the original SLAM graph can be done using a bottom-up inference along the corresponding cluster tree. To speed up the computation, we also introduce a base node for each submap and use it to represent the rigid transformation of the submap in the global coordinate frame. As a result, the optimization moves the base nodes rather than the actual submap variables. We demonstrate that our algorithm is not only exact but also much faster than alternative approaches in both simulations and real-world experiments.

This paper deals with estimation of dense optical flow and ego-motion in a generalized imaging system by exploiting probabilistic linear subspace constraints on the flow. We deal with the extended motion of the imaging system through an environment that we assume to have some degree of statistical regularity. For example, in autonomous ground vehicles the structure of the environment around the vehicle is far from arbitrary, and the depth at each pixel is often approximately constant. The subspace constraints hold not only for perspective cameras, but in fact for a very general class of imaging systems, including catadioptric and multiple-view systems. Using minimal assumptions about the imaging system, we learn a probabilistic subspace constraint that captures the statistical regularity of the scene geometry relative to an imaging system. We propose an extension to probabilistic PCA (Tipping and Bishop, 1999) as a way to robustly learn this subspace from recorded imagery, and demonstrate its use in conjunction with a sparse optical flow algorithm. To deal with the sparseness of the input flow, we use a generative model to estimate the subspace using only the observed flow measurements. Additionally, to identify and cope with image regions that violate subspace constraints, such as moving objects, objects that violate the depth regularity, or gross flow estimation errors, we employ a per-pixel Gaussian mixture outlier process. We demonstrate results of finding the optical flow subspaces and employing them to estimate dense flow and to recover camera motion for a variety of imaging systems in several different environments.

We present a novel variant of the RANSAC algorithm that is much more efficient, in particular when dealing with problems with low inlier ratios. Our algorithm assumes that there exists some grouping in the data, based on which we introduce a new binomial mixture model rather than the simple binomial model as used in RANSAC. We prove that in the new model it is more efficient to sample data from a smaller numbers of groups and groups with more tentative correspondences, which leads to a new sampling procedure that uses progressive numbers of groups. We demonstrate our algorithm on two classical geometric vision problems: wide-baseline matching and camera resectioning. The experiments show that the algorithm serves as a general framework that works well with three possible grouping strategies investigated in this paper, including a novel optical flow based clustering approach. The results show that our algorithm is able to achieve a significant performance gain compared to the standard RANSAC and PROSAC.

Separating sparse flow provides fast and robust stereo visual odometry that deals with nearly degenerate situations that often arise in practical applications.We make use of the fact that in outdoor situations different constraints are provided by close and far structure, where the notion of close depends on the vehicle speed. The motion of distant features determines the rotational component that we recover with a robust two-point algorithm. Once the rotation is known, we recover the translational component from close features using a robust one-point algorithm. The overall algorithm is faster than estimating the motion in one step by a standard RANSAC-based three-point algorithm. And in contrast to other visual odometry work, we avoid the problem of nearly degenerate data, under which RANSAC is known to return inconsistent results. We confirm our claims on data from an outdoor robot equipped with a stereo rig.

Automatic detection of landmarks, usually special places in the environment such as gateways, for topological mapping has proven to be a difficult task. We present the use of Bayesian surprise, introduced in computer vision, for landmark detection. Further, we provide a novel hierarchical, graphical model for the appearance of a place and use this model to perform surprise-based landmark detection. Our scheme is agnostic to the sensor type, and we demonstrate this by implementing a simple laser model for computing surprise. We evaluate our landmark detector using appearance and laser measurements in the context of a topological mapping algorithm, thus demonstrating the practical applicability of the detector.

We present a novel method for automatically geo-tagging photographs of man-made environments via detection and matching of repeated patterns. Highly repetitive environments introduce numerous correspondence ambiguities and are problematic for traditional wide-baseline matching methods. Our method exploits the highly repetitive nature of urban environments, detecting multiple perspectively distorted periodic 2D patterns in an image and matching them to a 3D database of textured facades by reasoning about the underlying canonical forms of each pattern. Multiple 2D-to-3D pattern correspondences enable robust recovery of camera orientation and location. We demonstrate the success of this method in a large urban environment.

Pose estimation of outdoor robots presents some distinct challenges due to the various uncertainties in the robot sensing and action. In particular, global positioning sensors of outdoor robots do not always work perfectly, causing large drift in the location estimate of the robot. To overcome this common problem, we propose a new approach for global localization using place recognition. First, we learn the location of some arbitrary key places using odometry measurements and GPS measurements only at the start and the end of the robot trajectory. In subsequent runs, when the robot perceives a key place, our fixed-lag smoother fuses odometry measurements with the relative location to the key place to improve its pose estimate. Outdoor mobile robot experiments show that place recognition measurements significantly improve the estimate of the smoother in the absence of GPS measurements.

In this paper, we present incremental smoothing and mapping (iSAM), which is a novel approach to the simultaneous localization and mapping problem that is based on fast incremental matrix factorization. iSAM provides an efficient and exact solution by updating a QR factorization of the naturally sparse smoothing information matrix, thereby recalculating only those matrix entries that actually change. iSAM is efficient even for robot trajectories with many loops as it avoids unnecessary fill-in in the factor matrix by periodic variable reordering. Also, to enable data association in real time, we provide efficient algorithms to access the estimation uncertainties of interest based on the factored information matrix. We systematically evaluate the different components of iSAM as well as the overall algorithm using various simulated and real-world datasets for both landmark and pose-only settings.

In this paper, we describe a technique to temporally sort a collection of photos that span many years. By reasoning about persistence of visible structures, we show how this sorting task can be formulated as a constraint satisfaction problem (CSP). Casting this problem as a CSP allows us to efficiently find a suitable ordering of the images despite the large size of the solution space (factorial in the number of images) and the presence of occlusions. We present experimental results for photographs of a city acquired over a one hundred year period.

Large-scale 3D reconstruction has recently received much attention from the computer vision community. Bundle adjustment is a key component of 3D reconstruction problems. However, traditional bundle adjustment algorithms require a considerable amount of memory and computational resources. In this paper, we present an extremely efficient, inherently out-of-core bundle adjustment algorithm. We decouple the original problem into several submaps that have their own local coordinate systems and can be optimized in parallel. A key contribution to our algorithm is making as much progress towards optimizing the global non-linear cost function as possible using the fragments of the reconstruction that are currently in core memory. This allows us to converge with very few global sweeps (often only two) through the entire reconstruction. We present experimental results on large-scale 3D reconstruction datasets, both synthetic and real.

We introduce incremental smoothing and mapping (iSAM), a novel approach to the problem of simultaneous localization and mapping (SLAM) that addresses the data association problem and allows real-time application in large-scale environments. We employ smoothing to obtain the complete trajectory and map without the need for any approximations, exploiting the natural sparsity of the smoothing information matrix. A QR-factorization of this information matrix is at the heart of our approach. It provides efficient access to the exact covariances as well as to conservative estimates that are used for online data association. It also allows recovery of the exact trajectory and map at any given time by back-substitution. Instead of refactoring in each step, we update the QR-factorization whenever a new measurement arrives. We analyze the effect of loops, and show how our approach extends to the non-linear case. Finally, we provide experimental validation of the overall non-linear algorithm based on the standard Victoria Park data set with unknown correspondences.

Simultaneous localization and mapping (SLAM) is a method that robots use to explore, navigate, and map an unknown environment. However, this method poses inherent problems with regard to cost and time. To lower computation costs, smoothing and mapping (SAM) approaches have shown some promise, and they also provide more accurate solutions than filtering approaches in realistic scenarios. However, in SAM approaches, updating the linearization is still the most time-consuming step. To mitigate this problem, we propose a submap-based approach, tectonic SAM, in which the original optimization problem is solved by using a divide-and-conquer scheme. Submaps are optimized independently and parameterized relative to a local coordinate frame. During the optimization, the global position of the submap may change dramatically, but the positions of the nodes in the submap relative to the local coordinate frame do not change very much. The key contribution of this paper is to show that the linearization of the submaps can be cached and reused when they are combined into a global map. According to the results of both simulation and real experiments, Tectonic SAM drastically speeds up SAM in very large environments while still maintaining its global accuracy.

We present an algorithm for pose estimation using fixed-lag smoothing. We show that fixed-lag smoothing enables inclusion of measurements from multiple asynchronous measurement sources in an optimal manner. Since robots usually have a plurality of uncoordinated sensors, our algorithm has an advantage over filtering-based estimation algorithms, which cannot incorporate delayed measurements optimally. We provide an implementation of the general fixed-lag smoothing algorithm using square root smoothing, a technique that has become prominent. Square root smoothing uses fast sparse matrix factorization and enables our fixed-lag pose estimation algorithm to run at upwards of 20 Hz. Our algorithm has been extensively tested over hundreds of hours of operation on a robot operating in outdoor environments. We present results based on these tests that verify our claims using wheel encoders, visual odometry, and GPS as sensors.

We introduce an extension of switching linear dynamic systems (SLDS) with parameterized duration modeling capabilities. The proposed model allows arbitrary duration models and overcomes the limitation of a geometric distribution induced in standard SLDSs. By incorporating a duration model which reflects the data more closely, the resulting model provides reliable inference results which are robust against observation noise. Moreover, existing inference algorithms for SLDSs can be adopted with only modest additional effort in most cases where an SLDS model can be applied. In addition, we observe the fact that the duration models would vary across data sequences in certain domains, which complicates learning and inference tasks. Such variability in duration is overcome by introducing parameterized duration models. The experimental results on honeybee dance decoding tasks demonstrate the robust inference capabilities of the proposed model.

In this paper, we present an approach of calculating visual odometry for outdoor robots equipped with a stereo rig. Instead of the typical feature matching or tracking, we use an improved stereo-tracking method that simultaneously decides the feature displacement in both cameras. Based on the matched features, a three-point algorithm for the resulting quadrifocal setting is carried out in a RANSAC framework to recover the unknown odometry. In addition, the change in rotation can be derived from infinity homography, and the remaining translational unknowns can be obtained even faster consequently . Both approaches are quite robust and deal well with challenging conditions such as wheel slippage.

We present a particle filtering algorithm to construct topological maps of an uninstrument environment. The algorithm presented here constructs the posterior on the space of all possible topologies given measurements, and is based on our previous work on a Bayesian inference framework for topological maps (A. Ranganathan and F. Dellaert, 2004). Constructing the posterior solves the perceptual aliasing problem in a general, robust manner. The use of a Rao-Blackwellized particle filter (RBPF) for this purpose makes the inference in the space of topologies incremental and run in real-time. The RBPF maintains the joint posterior on topological maps and locations of landmarks. We demonstrate that, using the landmark locations thus obtained, the global metric map can be obtained from the topological map generated by our algorithm through a simple post-processing step. A data-driven proposal is provided to overcome the degeneracy problem inherent in particle filters. The use of a Dirichlet process prior on landmark labels is also a novel aspect of this work. We use laser range scan and odometry measurements to present experimental results on a robot

In several multitarget tracking applications, a target may return more than one measurement per target and interacting targets may return multiple merged measurements between targets. Existing algorithms for tracking and data association, initially applied to radar tracking, do not adequately address these types of measurements. Here, we introduce a probabilistic model for interacting targets that addresses both types of measurements simultaneously. We provide an algorithm for approximate inference in this model using a Markov chain Monte Carlo (MCMC)-based auxiliary variable particle filter. We Rao-Blackwellize the Markov chain to eliminate sampling over the continuous state space of the targets. A major contribution of this work is the use of sparse least squares updating and downdating techniques, which significantly reduce the computational cost per iteration of the Markov chain. Also, when combined with a simple heuristic, they enable the algorithm to correctly focus computation on interacting targets. We include experimental results on a challenging simulation sequence. We test the accuracy of the algorithm using two sensor modalities, video, and laser range data. We also show the algorithm exhibits real time performance on a conventional PC.

Our understanding of social insect behavior has significantly influenced artificial intelligence (AI) and multirobot systems' research (e.g., ant algorithms and swarm robotics). In this work, however, we focus on the opposite question: "How can multirobot systems research contribute to the understanding of social animal behavior?" As we show, we are able to contribute at several levels. First, using algorithms that originated in the robotics community, we can track animals under observation to provide essential quantitative data for animal behavior research. Second, by developing and applying algorithms originating in speech recognition and computer vision, we can automatically label the behavior of animals under observation. In some cases the automatic labeling is more accurate and consistent than manual behavior identification. Our ultimate goal, however, is to automatically create, from observation, executable models of behavior. An executable model is a control program for an agent that can run in simulation (or on a robot). The representation for these executable models is drawn from research in multirobot systems programming. In this paper we present the algorithms we have developed for tracking, recognizing, and learning models of social animal behavior, details of their implementation, and quantitative experimental results using them to study social insects

While probabilistic techniques have previously been investigated extensively for performing inference over the space of metric maps, no corresponding general-purpose methods exist for topological maps. We present the concept of probabilistic topological maps (PTMs), a sample-based representation that approximates the posterior distribution over topologies, given available sensor measurements. We show that the space of topologies is equivalent to the intractably large space of set partitions on the set of available measurements. The combinatorial nature of the problem is overcome by computing an approximate, sample-based representation of the posterior. The PTM is obtained by performing Bayesian inference over the space of all possible topologies, and provides a systematic solution to the problem of perceptual aliasing in the domain of topological mapping. In this paper, we describe a general framework for modeling measurements, and the use of a Markov-chain Monte Carlo algorithm that uses specific instances of these models for odometry and appearance measurements to estimate the posterior distribution. We present experimental results that validate our technique and generate good maps when using odometry and appearance, derived from panoramic images, as sensor measurements.

We introduce mixture trees, a tree-based data-structure for modeling joint probability densities using a greedy hierarchical density estimation scheme. We show that the mixture tree models data efficiently at multiple resolutions, and present fast conditional sampling as one of many possible applications. In particular, the development of this data-structure was spurred by a multi-target tracking application, where memory-based motion modeling calls for fast conditional sampling from large empirical densities. However, it is also suited to applications such as texture synthesis, where conditional densities play a central role. Results are presented for both these applications.

In many multitarget tracking applications in computer vision, a detection algorithm provides locations of potential targets. Subsequently, the measurements are associated with previously estimated target trajectories in a data association step. The output of the detector is often imperfect and the detection data may include multiple, split measurements from a single target or a single merged measurement from several targets. To address this problem, we introduce a multiple hypothesis tracker for interacting targets that generate split and merged measurements. The tracker is based on an efficient Markov chain Monte Carlo (MCMC) based auxiliary variable particle filter. The particle filter is Rao-Blackwellized such that the continuous target state parameters are estimated analytically, and an MCMC sampler generates samples from the large discrete space of data associations. In addition, we include experimental results in a scenario where we track several interacting targets that generate these split and merged measurements.

We introduce parametric switching linear dynamic systems (P-SLDS) for learning and interpretation of parametrized motion, i.e., motion that exhibits systematic temporal and spatial variations. Our motivating example is the honeybee dance: bees communicate the orientation and distance to food sources through the dance angles and waggle lengths of their stylized dances. Switching linear dynamic systems (SLDS) are a compelling way to model such complex motions. However, SLDS does not provide a means to quantify systematic variations in the motion. Previously, Wilson & Bobick (1999) presented parametric HMMs, an extension to HMMs with which they successfully interpreted human gestures. Inspired by their work, we similarly extend the standard SLDS model to obtain parametric SLDS. We introduce additional global parameters that represent systematic variations in the motion, and present general expectation-maximization (EM) methods for learning and inference. In the learning phase, P-SLDS learns canonical SLDS model from data. In the inference phase, P-SLDS simultaneously quantifies the global parameters and labels the data. We apply these methods to the automatic interpretation of honey-bee dances, and present both qualitative and quantitative experimental results on actual bee-tracks collected from noisy video data.

In this paper, we study the problem of going from a real-world, multi-agent system to the generation of control programs in an automatic fashion. In particular, a computer vision system is presented, capable of simultaneously tracking multiple agents, such as social insects. Moreover, the data obtained from this system is fed into a mode-reconstruction module that generates low-complexity control programs, i.e. strings of symbolic descriptions of control-interrupt pairs, consistent with the empirical data. The result is a mechanism for going from the real system to an executable implementation that can be used for controlling multiple mobile robots.

The problem of simultaneous localization and mapping has received much attention over the last years. Especially large scale environments, where the robot trajectory loops back on itself, are a challenge. In this paper we introduce a new solution to this problem of closing the loop. Our algorithm is EM-based, but differs from previous work. The key is a probability distribution over partitions of feature tracks that is determined in the E-step, based on the current estimate of the motion. This virtual structure is then used in the M-step to obtain a better estimate for the motion. We demonstrate the success of our algorithm in experiments on real laser data.

Recently, the acquisition of three-dimensional maps has become more and more popular. This is motivated by the fact that robots act in the three-dimensional world and several tasks such as path planning or localizing objects can be carried out more reliable using three-dimensional representations. In this paper we consider the problem of extracting planes from three-dimensional range data. In contrast to previous approaches our algorithm uses a hierarchical variant of the popular Expectation Maximization (EM) algorithm [1] to simultaneously learn the main directions of the planar structures. These main directions are then used to correct the position and orientation of planes. In practical experiments carried out with real data and in simulations we demonstrate that our algorithm can accurately extract planes and their orientation from range data.

We describe a particle filter that effectively deals with interacting targets--targets that are influenced by the proximity and/or behavior of other targets. The particle filter includes a Markov random field (MRF) motion prior that helps maintain the identity of targets throughout an interaction, significantly reducing tracker failures. We show that this MRF prior can be easily implemented by including an additional interaction factor in the importance weights of the particle filter. However, the computational requirements of the resulting multitarget filter render it unusable for large numbers of targets. Consequently, we replace the traditional importance sampling step in the particle filter with a novel Markov chain Monte Carlo (MCMC) sampling step to obtain a more efficient MCMC-based multitarget filter. We also show how to extend this MCMC-based filter to address a variable number of interacting targets. Finally, we present both qualitative and quantitative experimental results, demonstrating that the resulting particle filters deal efficiently and effectively with complicated target interactions.

Localization from sensor measurements is a fundamental task for navigation. Particle filters are among the most promising candidates to provide a robust and real-time solution to the localization problem. They instantiate the localization problem as a Bayesian altering problem and approximate the posterior density over location by a weighted sample set. In this paper, we introduce map-based priors for localization, using the semantic information available in maps to bias the motion model toward areas of higher probability. We, show that such priors, under a particular assumption, can easily be incorporated in the particle filter by means of a pseudo likelihood. The resulting filter is more reliable and more accurate. We show experimental results on a GPS based outdoor people tracker that illustrate the approach and highlight its potential.

While probabilistic techniques have been considered extensively in the context of metric maps, no general purpose probabilistic methods exist for topological maps. We present the concept of probabilistic topological maps (PTMs), a sample-based representation that approximates the posterior distribution over topologies given the available sensor measurements. The PTM is obtained through the use of MCMC-based Bayesian inference over the space of all possible topologies. It is shown that the space of all topologies is equivalent to the space of set partitions of all available measurements. While the space of possible topologies is intractably large, our use of Markov chain Monte Carlo sampling to infer the approximate histograms overcomes the combinatorial nature of this space and provides a general solution to the correspondence problem in the context of topological mapping. We present experimental results that validate our technique and generate good maps even when using only odometry as the sensor measurements.

Most of the work in head-pose tracking has concentrated on single-camera systems with a relatively small field of view which have limited accuracy because features are only observed in a single viewing direction. We present a multicamera pose tracker that handles an arbitrary configuration of cameras rigidly fixed to the observer's head. By using multiple cameras, we increase the robustness and accuracy by which a 6-DOF pose is tracked. However, in a multicamera rig setting, earlier methods for determining the unknown pose from three world-to-camera correspondences are no longer applicable. We present a RANSAC (M. Fischler and R. Bolles, 1981) based method that handles multicamera rigs by using a fast nonlinear minimization step in each RANSAC round.

We propose a spectral partitioning approach for large-scale optimization problems, specifically structure from motion. In structure from motion, partitioning methods reduce the problem into smaller and better conditioned subproblems which can be efficiently optimized. Our partitioning method uses only the Hessian of the reprojection error and its eigenvector. We show that partitioned systems that preserve the eigenvectors corresponding to small eigenvalues result in lower residual error when optimized. We create partitions by clustering the entries of the eigenvectors of the Hessian corresponding to small eigenvalues. This is a more general technique than relying on domain knowledge and heuristics such as bottom-up structure from motion approaches. Simultaneously, it takes advantage of more information than generic matrix partitioning algorithms.

We investigate intrinsic localization and mapping (ILM) for teams of mobile robots, a multi-robot variant of SLAM where the robots themselves are used as landmarks. We develop what is essentially a straightforward application of Bayesian estimation to the problem, and present two complimentary views on the associated optimization problem that provide insight into the problem and allows one to devise initialization strategies, indispensable in practice. We also provide a discussion of the degrees of freedom and ambiguities in the solution. Finally, we introduce two applications of ILM that bring out its potential: Diffusion Mapping and Marco Polo localization.

We introduce the Marco Polo localization approach, where we apply sound as a tool for gathering range measurements between robots, and use those to solve a range-only simultaneous localization and mapping problem. Range is calculated by correlating two recordings of the same sound, recorded on a pair of robots, after which the resulting time delay estimate is converted to a range measurement. The algorithmic approach we use is a straightforward application of the Bayesian estimation framework. We also present two complementary views on the associated optimization problem that provide insight into the problem and allows one to devise initialization strategies, indispensable in a range-only scenario. We illustrate the approach with both simulated and experimental results.

We describe a multiple hypothesis particle filter for tracking targets that are influenced by the proximity and/or behavior of other targets. Our contribution is to show how a Markov random field motion prior, built on the fly at each time step, can model these interactions to enable more accurate tracking. We present results for a social insect tracking application, where we model the domain knowledge that two targets cannot occupy the same space, and targets actively avoid collisions. We show that using this model improves track quality and efficiency. Unfortunately, the joint particle tracker we propose suffers from exponential complexity in the number of tracked targets. An approximation to the joint filter, however, consisting of multiple nearly independent particle filters can provide similar track quality at substantially lower computational cost.

We show how to recover 2D structure and motion linearly in order to initialize simultaneous mapping and localization (SLAM) for bearings-only measurements and planar motion. The method supplies a good initial estimate of the geometry, even without odometry or in multiple robot scenarios. Hence, it substantially enlarges the scope in which non-linear batch-type SLAM algorithms can be applied. The method is applicable when at least seven landmarks are seen from three different vantage points, whether by one robot that moves over time or by multiple robots that observe a set of common landmarks.

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.

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,.

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.

We present a principled method of obtaining a weighted similarity metric for 3D image retrieval, firmly rooted in Bayes decision theory. The basic idea is to determine a set of most discriminative features by evaluating how well they perform on the task of classifying images according to predefined semantic categories. We propose this indirect method as a rigorous way to solve the difficult feature selection problem that comes up in most content based image retrieval tasks. The method is applied to normal and pathological neuroradiological CT images, where we take advantage of the fact that normal human brains present an approximate bilateral symmetry which is often absent in pathological brains. The quantitative evaluation of the retrieval system shows promising results.

This paper discusses how we integrated our 3D car tracking approach with the lane following module RALPH on the Navlab autonomous vehicles, obtaining a hybrid vision system that tracks both the road and cars better than those two systems in isolation. The tracking system brings precise and crisp measurements of the car in the image, and performs image stabilization. However, because it does not know, about the yaw or lateral offset of the ego-vehicle, its curvature estimate can be misguided. RALPH takes a more global image processing approach and can provide this missing information, as well as a good estimate of curvature, so that the combined curvature estimate is superior to both taken in isolation. The additional information provided by RALPH also improves tracking performance, and allows us to estimate properties of the tracked car that were previously unobservable, in particular its in-lane displacement. Better car tracking, and a better idea of where the road is, gives us a substantial foundation on which to base other capabilities needed to realize fully autonomous vehicles.

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 explores several statistical pattern recognition techniques to classify utterances according to their emotional content.We have recorded a corpus containing emotional speech with over a 1000 utterances from different speakers.We present a new method of extracting prosodic features from speech, based on a smoothing spline approximation of the pitch contour.To make maximal use of the limited amount of training data available, we introduce a novel pattern recognition technique: majority voting of subspace specialists.Using this technique, we obtain classification performance that is close to human performance on the task.