Position Estimation of Multiple Robots: Provable, Practical Approximation Algorithm

Abstract

We consider the problem of matching a pair of point sets in the Euclidean $d$-dimensional space $REAL^d$, where clusters from the first point set are arbitrarily translated with additional noise, resulting in the second point set. The goal is to minimize the sum of squared distances between the paired sets over every translation and possible matching among the $n!$ permutations. The result can be used as a seeding clustering for existing algorithms, e.g. to compute the optimal rotation on each cluster. This is a fundamental problem for tracking systems (e.g. OptiTrack or Vicon) where the user registers $k$ objects (rigid bodies) by attaching a set of markers to each object. Based on the position of these markers in real-time, the system estimates the position of the moving objects by simultaneously clustering, matching and transforming the $n$ observed markers to the $k$ objects. Similarly, an autonomous robot equipped with a camera may estimate its position by tracking $n$ visual features from $k$ recognized objects. We suggest the first provable algorithm for solving this point matching problem. Unlike common heuristics, it yields a constant factor approximation for the emph{global optimum} in expected $dn^{k+2}log n)$ time. We validate our theoretical results with experimental results using low-cost ($