Fast and Robust Data Association Using Posterior Based Approximate Joint Compatibility Test

Abstract

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 <formula formulatype="inline" xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex Notation="TeX">$O(n)$</tex> </formula> complexity (compared with <formula formulatype="inline" xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex Notation="TeX">$O(n^{2})$</tex> </formula> for traditional tests), where <formula formulatype="inline" xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex Notation="TeX">$n$</tex></formula> 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.