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.
Data association in SLAM is a well-explored problem, with solutions in applications ranging from acoustic sensing @cite_26 @cite_35 , monocular vision @cite_6 , and LIDAR @cite_27 . Traditionally, the search for jointly-compatible hypotheses has been approached as a search over the interpretation tree @cite_5 @cite_14 or a max-clique problem over a correspondence graph @cite_15 @cite_2 . The probabilistic Joint Compatibility (JC) metric introduced by @cite_13 has been widely used as the de facto standard in filtering approaches (in which the full covariance matrix is readily available). A number of hybrid approaches @cite_28 have been proposed, generally leveraging the correspondence graph to generate pairwise-compatible @cite_5 @cite_29 hypotheses and using JC to verify them @cite_30 . In the case of feature cloud matching, which applies directly to sensor modalities such as LIDAR, @cite_7 @cite_24 leverage the specific independence structure to provide linear and constant-time incrementalized evaluations of JC. In contrast to the traditional gating formulation, the equivalent posterior form of the test is exploited in @cite_36 that conveniently decouples different components of the error, allowing sequential computation.
@cite_30 @cite_35 @cite_26 @cite_14 @cite_7 @cite_28 @cite_36 @cite_29 @cite_6 @cite_24 @cite_27 @cite_2 @cite_5 @cite_15 @cite_13
1977455514 2564754945 2101648351 2150440166
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. 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. In this paper, we describe a system that can carry out simultaneous localization and mapping (SLAM) in large indoor and outdoor environments using a stereo pair moving with 6 DOF as the only sensor. Unlike current visual SLAM systems that use either bearing-only monocular information or 3-D stereo information, our system accommodates both monocular and stereo. Textured point features are extracted from the images and stored as 3-D points if seen in both images with sufficient disparity, or stored as inverse depth points otherwise. This allows the system to map both near and far features: the first provide distance and orientation, and the second provide orientation information. Unlike other vision-only SLAM systems, stereo does not suffer from ldquoscale driftrdquo because of unobservability problems, and thus, no other information such as gyroscopes or accelerometers is required in our system. Our SLAM algorithm generates sequences of conditionally independent local maps that can share information related to the camera motion and common features being tracked. The system computes the full map using the novel conditionally independent divide and conquer algorithm, which allows constant time operation most of the time, with linear time updates to compute the full map. To demonstrate the robustness and scalability of our system, we show experimental results in indoor and outdoor urban environments of 210 m and 140 m loop trajectories, with the stereo camera being carried in hand by a person walking at normal walking speeds of 4--5 km h. The problem of associating data with targets in a cluttered multi-target environment is discussed and applied to passive sonar tracking. The probabilistic data association (PDA) method, which is based on computing the posterior probability of each candidate measurement found in a validation gate, assumes that only one real target is present and all other measurements are Poisson-distributed clutter. In this paper, a new theoretical result is presented: the joint probabilistic data association (JPDA) algorithm, in which joint posterior association probabilities are computed for multiple targets (or multiple discrete interfering sources) in Poisson clutter. The algorithm is applied to a passive sonar tracking problem with multiple sensors and targets, in which a target is not fully observable from a single sensor. Targets are modeled with four geographic states, two or more acoustic states, and realistic (i.e., low) probabilities of detection at each sample time. A simulation result is presented for two heavily interfering targets illustrating the dramatic tracking improvements obtained by estimating the targets' states using joint association probabilities.