py: avoid matching trajectories with too high rmse

src/utility.py

@@ -38,9 +38,10 @@ def match_tracks_to_ground_truth(tracks, ground_truth):
             else:
                 N = T.shape[1]
             # Only compare times present in both ground truth and estimate
-            rmse = np.sum((T[:, t[t<N]]-x[:2, t[t<N]])**2)/N
+            rmse = np.sqrt(np.sum((T[:, t[t<N]]-x[:2, t[t<N]])**2)/N)
             if rmse < ormse: # The ground truth with the lowest to track RMSE is assumed to be correct
-                matches[track['identity']] = key
+                if rmse < 100: # If the rmse is too large, assume it is a false match
+                    matches[track['identity']] = key
                 ormse = rmse
     return matches