Data association in SLAM is fundamentally challenging, and handling ambiguity well is crucial to achieve robust operation in real-world environments. When ambiguous measurements arise, conservatism often mandates that the measurement is discarded or a new landmark is initialized rather than risking an incorrect association. To address the inevitable “duplicate” landmarks that arise, we present an efficient map-merging framework to detect duplicate constellations of landmarks, providing a high-confidence loopclosure mechanism well-suited for object-level SLAM. This approach uses an incrementally-computable approximation of landmark uncertainty that only depends on local information in the SLAM graph, avoiding expensive recovery of the full system covariance matrix. This enables a search based on geometric consistency (GC) (rather than full joint compatibility (JC)) that inexpensively reduces the search space to a handful of “best” hypotheses. Furthermore, we reformulate the commonly-used interpretation tree to allow for more efficient integration of clique-based pairwise compatibility, accelerating the branch-and-bound max-cardinality search. Our method is demonstrated to match the performance of full JC methods at significantly-reduced computational cost, facilitating robust object-based loop-closure over large SLAM problems.
In this paper, we show that all processes associated with the move-sense-update cycle of extended Kalman filter (EKF) Simultaneous Localization and Mapping (SLAM) can be carried out in time linear with the number of map features. We describe Divide and Conquer SLAM, which is an EKF SLAM algorithm in which the computational complexity per step is reduced from O(n 2) to O(n), and the total cost of SLAM is reduced from O(n 3) to O(n 2). Unlike many current large-scale EKF SLAM techniques, this algorithm computes a solution without relying on approximations or simplifications (other than linearizations) to reduce computational complexity. Also, estimates and covariances are available when needed by data association without any further computation. Furthermore, as the method works most of the time in local maps, where angular errors remain small, the effect of linearization errors is limited. The resulting vehicle and map estimates are more precise than those obtained with standard EKF SLAM. The errors with respect to the true value are smaller, and the computed state covariance is consistent with the real error in the estimation. Both simulated experiments and the Victoria Park dataset are used to provide evidence of the advantages of this algorithm. This paper describes a system for reacquiring features of interest in a shallow-water ocean environment, using autonomous underwater vehicles (AUVs) equipped with low-cost sonar and navigation sensors. In performing mine countermeasures, it is critical to enable AUVs to navigate accurately to previously mapped objects of interest in the water column or on the seabed, for further assessment or remediation. An important aspect of the overall system design is to keep the size and cost of the reacquisition vehicle as low as possible, as it may potentially be destroyed in the reacquisition mission. This low-cost requirement prevents the use of sophisticated AUV navigation sensors, such as a Doppler velocity log (DVL) or an inertial navigation system (INS). Our system instead uses the Proviewer 900-kHz imaging sonar from Blueview Technologies, which produces forward-looking sonar (FLS) images at ranges up to 40 mat approximately 4 Hz. In large volumes, it is hoped that this sensor can be manufactured at low cost. Our approach uses a novel simultaneous localization and mapping (SLAM) algorithm that detects and tracks features in the FLS images to renavigate to a previously mapped target. This feature-based navigation (FBN) system incorporates a number of recent advances in pose graph optimization algorithms for SLAM. The system has undergone extensive field testing over a period of more than four years, demonstrating the potential for the use of this new approach for feature reacquisition. In this report, we review the methodologies and components of the FBN system, describe the system's technological features, review the performance of the system in a series of extensive in-water field tests, and highlight issues for future research. We present a novel approach, entitled acoustic structure from motion (ASFM), for recovering 3D scene structure from multiple 2D sonar images, while at the same time localizing the sonar. Imaging sonar or forward looking sonar (FLS) is commonly used for autonomous underwater vehicle (AUV) navigation. An FLS provides bearing and range information to a target, but the elevation of the target is unknown within the sensor's field of view. Hence, current state-of-the-art techniques commonly make a flat surface (ground) assumption so that the FLS data can be used for navigation. Unlike other methods, our solution does not require a flat surface assumption and is capable of utilizing information from many frames, as opposed to pairwise methods that can only gather information from two frames at once. ASFM is inspired by structure from motion (SFM), the problem of recovering 3D structure from multiple camera images, while also recovering the position and orientation from which the images were taken. In this paper, we formulate and evaluate the optimization of several AUV sensor readings of the same scene from different poses, the sonar equivalent of bundle adjustment. We evaluate our approach on both simulated data and FLS sonar data with the assumption that feature extraction and data association have been completed. The acoustic equivalents of those two important features of SFM are left for future work. We present a lazy data association algorithm for the simultaneous localization and mapping (SLAM) problem. Our approach uses a tree-structured Bayesian representation of map posteriors that makes it possible to revise data association decisions arbitrarily far into the past. We describe a criterion for detecting and repairing poor data association decisions. This technique makes it possible to acquire maps of large-scale environments with many loops, with a minimum of computational overhead for the management of multiple data association hypotheses. A empirical comparison with the popular FastSLAM algorithm shows the advantage of lazy over proactive data association. One of the fundamental challenges in robotics is data-association: determining which sensor observations correspond to the same physical object. A common approach is to consider groups of observations simultaneously: a constellation of observations can be significantly less ambiguous than the observations considered individually. The Joint Compatibility Branch and Bound (JCBB) test is the gold standard method for these data association problems. But its computational complexity and its sensitivity to non-linearities limit its practical usefulness. We propose the Incremental Posterior Joint Compatibility (IPJC) test. While equivalent to JCBB on linear problems, it is significantly more accurate on non-linear problems. When used for feature-cloud matching (an important special case), IPJC is also dramatically faster than JCBB. We demonstrate the advantages of IPJC over JCBB and other commonly-used methods on both synthetic and real-world datasets. In this paper we propose an algorithm to deter- mine the location of a vehicle in an environment represented by a stochastic map, given a set of environment measure- ments obtained by a sensor mounted on the vehicle. We show that the combined use of (1) geometric constraints considering feature correlation, (2) joint compatibility, (3) random sampling and (4) locality, make this algorithm lin- ear with both the size of the stochastic map and the number of measurements. We demonstrate the practicality and ro- bustness of our approach with experiments in an outdoor environment. Data association is a fundamental problem in multisensor fusion, tracking, and localization. The joint compatibility test is commonly regarded as the true solution to the problem. However, traditional joint compatibility tests are computationally expensive, are sensitive to linearization errors, and require the knowledge of the full covariance matrix of state variables. The paper proposes a posterior-based joint compatibility test scheme to conquer the three problems mentioned above. The posterior-based test naturally separates the test of state variables from the test of observations. Therefore, through the introduction of the robot movement and proper approximation, the joint test process is sequentialized to the sum of individual tests; therefore, the test has O(n) complexity (compared with O(n2) for traditional tests), where n denotes the total number of related observations. At the same time, the sequentialized test neither requires the knowledge to the full covariance matrix of state variables nor is sensitive to linearization errors caused by poor pose estimates. The paper also shows how to apply the proposed method to various simultaneous localization and mapping (SLAM) algorithms. Theoretical analysis and experiments on both simulated data and popular datasets show the proposed method outperforms some classical algorithms, including sequential compatibility nearest neighbor (SCNN), random sample consensus (RANSAC), and joint compatibility branch and bound (JCBB), on precision, efficiency, and robustness. Presents a technique for relocation of a mobile robot using sonar data. Relocation is the process of determining the position and orientation of a mobile robot with respect to the global reference frame in situations where no a priori estimate of the robot's location is available. A two dimensional model of the environment is employed consisting of planes, corners, edges, and cylinders. A physically based sonar sensor model is used to characterize the geometric constraints provided by echolocation measurements of different types of objects. The sensor model makes it possible to employ individual range returns as data features in a constraint-based search to determine the robot's position. To increase the efficiency of search, a hypothesize and verify technique is employed in which position of the robot are calculated from all possible combinations of two range returns that satisfy the measurement model. Accurate relocation is demonstrated from sparse sonar data in the presence of unmodeled objects.  In this work, we address the problem of robust data association for feature cloud matching. For matching two feature clouds observed at two different poses, we discover that the covariance matrix of the measurement prediction error can be written as the sum of a low rank matrix and a block diagonal matrix, if we assume that the features are observed independently at each pose. This special structure of the covariance matrix allows us to compute its inverse analytically and efficiently. Together with a good bookkeeping strategy, the complexity of the Joint Compatibility (JC) test is reduced to O(1). Contrary to the approximated JC test, ours is both exact and fast. Based on the efficient JC test algorithm and a branch and bound search procedure, we devise an algorithm, called Fast Joint Compatibility Branch and Bound (FastJCBB), to quickly obtain robust data association. The FastJCBB algorithm is essentially modified from the conventional Joint Compatibility Branch and Bound (JCBB) algorithm and both of these algorithms are able to produce exactly the same data association results. However, with the substantial improvement in the efficiency of JC tests, our FastJCBB algorithm is much faster than the conventional JCBB, especially when matching two large feature clouds. It is reported that our FastJCBB algorithm is more than 740 times faster than the conventional JCBB in carrying out one million JC tests when matching two clouds with about 100 features each. Since both FastJCBB and JCBB share the same branch and bound procedure in exploring the interpretation tree, the search complexity remains exponential. Our main contribution is the significant improvement in the efficiency of exploring each node of the interpretation tree. Addresses real-time implementation of the simultaneous localization and map-building (SLAM) algorithm. It presents optimal algorithms that consider the special form of the matrices and a new compressed filler that can significantly reduce the computation requirements when working in local areas or with high frequency external sensors. It is shown that by extending the standard Kalman filter models the information gained in a local area can be maintained with a cost spl sim O(N sub a sup 2 ), where N sub a is the number of landmarks in the local area, and then transferred to the overall map in only one iteration at full SLAM computational cost. Additional simplifications are also presented that are very close to optimal when an appropriate map representation is used. Finally the algorithms are validated with experimental results obtained with a standard vehicle running in a completely unstructured outdoor environment. This paper presents a robust solution to the mobile robotics data association problem based on solving the maximum clique problem (MCP) in a typically sparse correspondence graph, which contains compatibility information between pairs of observations and landmarks. Bit sparse optimizations are designed and implemented in a new algorithm BBMCS, which reduces computation and memory requirements of a leading general purpose maximum clique solver, to make it possibly the best exact sparse MCP algorithm currently found in the literature. BBMCS is reported to achieve very good results in terms of robustness with few assumptions on noise and visibility, while managing very reasonable computation time and memory usage even for complex large data association problems. With contributions from Tomas LozanoPerez and Daniel P. Huttenlocher.An intelligent system must know "what "the objects are and "where "they are in its environment. Examples of this ubiquitous problem in computer vision arise in tasks involving hand-eye coordination (such as assembling or sorting), inspection tasks, gauging operations, and in navigation and localization of mobile robots. This book describes an extended series of experiments into the role of geometry in the critical area of object recognition. It provides precise definitions of the recognition and localization problems, describes the methods used to address them, analyzes the solutions to these problems, and addresses the implications of this analysis.The solution to problems of object recognition are of fundamental importance in many real applications and versions of the techniques described here are already being used in industrial settings. Although a number of questions remain to be solved, the authors provide a valuable framework for understanding both the strengths and limitations of using object shape to guide recognition.W. Eric L. Grimson is Matsushita Associate Professor in the Department of Electrical Engineering and Computer Science at MIT.Contents: Introduction. Recognition as a Search Problem. Searching for Correspondences. Two-Dimensional Constraints. Three-Dimensional Constraints. Verifying Hypotheses. Controlling the Search Explosion. Selecting Subspaces of the Search Space. Empirical Testing. The Combinatorics of the Matching Process. The Combinatorics of Hough Transforms. The Combinatorics of Verification. The Combinatorics of Indexing. Evaluating the Methods. Recognition from Libraries. Parameterized Objects. The Role of Grouping. Sensing Strategies. Applications. The Next Steps. Data association is the process of relating features observed in the environment to features viewed previously or to features in a map. This paper presents a graph theoretic method that is applicable to data association problems where the features are observed via a batch process. Batch observations detect a set of features simultaneously or with sufficiently small temporal difference that, with motion compensation, the features can be represented with precise relative coordinates. This data association method is described in the context of two possible navigation applications: metric map building with simultaneous localisation, and topological map based localisation. Experimental results are presented using an indoor mobile robot with a 2D scanning laser sensor. Given two scans from different unknown locations, the features common to both scans are mapped to each other and the relative change in pose (position and orientation) of the vehicle between the two scans is obtained. In this paper, we address the problem of robust data association for simultaneous vehicle localization and map building. We show that the classical gated nearest neighbor approach, which considers each matching between sensor observations and features independently, ignores the fact that measurement prediction errors are correlated. This leads to easily accepting incorrect matchings when clutter or vehicle errors increase. We propose a new measurement of the joint compatibility of a set of pairings that successfully rejects spurious matchings. We show experimentally that this restrictive criterion can be used to efficiently search for the best solution to data association. Unlike the nearest neighbor, this method provides a robust solution in complex situations, such as cluttered environments or when revisiting previously mapped regions.