Improve trajectory matching in test scripts
Note: Since our solver doesn't do too bad, this has low priority
Matching trajectories to each other can be seen as an linear assignment problem (or bipartite minimum weight graph matching). We want to match test and truth trajectories such that the distance between matched test and truth is minimal. There are existing algorithms which we could use to replace our heuristic solver.
Here is an example of an implementation using scipy to solve the assignment/matching taken from py-motmetrics
def add_expensive_edges(costs):
"""Replaces non-edge costs (nan, inf) with large number.
If the optimal solution includes one of these edges,
then the original problem was infeasible.
Parameters
----------
costs : np.ndarray
"""
# The graph is probably already dense if we are doing this.
assert isinstance(costs, np.ndarray)
# The linear_sum_assignment function in scipy does not support missing edges.
# Replace nan with a large constant that ensures it is not chosen.
# If it is chosen, that means the problem was infeasible.
valid = np.isfinite(costs)
if valid.all():
return costs.copy()
if not valid.any():
return np.zeros_like(costs)
r = min(costs.shape)
# Assume all edges costs are within [-c, c], c >= 0.
# The cost of an invalid edge must be such that...
# choosing this edge once and the best-possible edge (r - 1) times
# is worse than choosing the worst-possible edge r times.
# l + (r - 1) (-c) > r c
# l > r c + (r - 1) c
# l > (2 r - 1) c
# Choose l = 2 r c + 1 > (2 r - 1) c.
c = np.abs(costs[valid]).max() + 1 # Doesn't hurt to add 1 here.
large_constant = 2 * r * c + 1
return np.where(valid, costs, large_constant)
def _exclude_missing_edges(costs, rids, cids):
subset = [
index for index, (i, j) in enumerate(zip(rids, cids))
if np.isfinite(costs[i, j])
]
return rids[subset], cids[subset]
def lsa_solve_scipy(costs):
"""Solves the LSA problem using the scipy library."""
from scipy.optimize import linear_sum_assignment as scipy_solve
# scipy (1.3.3) does not support nan or inf values
finite_costs = add_expensive_edges(costs)
rids, cids = scipy_solve(finite_costs)
rids, cids = _exclude_missing_edges(costs, rids, cids)
return rids, cids