Multi-robot pose graph localization and data association from unknown initial relative poses via expectation maximization

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.