File size: 7,818 Bytes
ac6e446
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
# vim: expandtab:ts=4:sw=4
from __future__ import absolute_import
import numpy as np
from scipy.optimize import linear_sum_assignment
from . import kalman_filter


INFTY_COST = 1e+5


def min_cost_matching(

        distance_metric, max_distance, tracks, detections, track_indices=None,

        detection_indices=None):
    """Solve linear assignment problem.

    Parameters

    ----------

    distance_metric : Callable[List[Track], List[Detection], List[int], List[int]) -> ndarray

        The distance metric is given a list of tracks and detections as well as

        a list of N track indices and M detection indices. The metric should

        return the NxM dimensional cost matrix, where element (i, j) is the

        association cost between the i-th track in the given track indices and

        the j-th detection in the given detection_indices.

    max_distance : float

        Gating threshold. Associations with cost larger than this value are

        disregarded.

    tracks : List[track.Track]

        A list of predicted tracks at the current time step.

    detections : List[detection.Detection]

        A list of detections at the current time step.

    track_indices : List[int]

        List of track indices that maps rows in `cost_matrix` to tracks in

        `tracks` (see description above).

    detection_indices : List[int]

        List of detection indices that maps columns in `cost_matrix` to

        detections in `detections` (see description above).

    Returns

    -------

    (List[(int, int)], List[int], List[int])

        Returns a tuple with the following three entries:

        * A list of matched track and detection indices.

        * A list of unmatched track indices.

        * A list of unmatched detection indices.

    """
    if track_indices is None:
        track_indices = np.arange(len(tracks))
    if detection_indices is None:
        detection_indices = np.arange(len(detections))

    if len(detection_indices) == 0 or len(track_indices) == 0:
        return [], track_indices, detection_indices  # Nothing to match.

    cost_matrix = distance_metric(
        tracks, detections, track_indices, detection_indices)
    cost_matrix[cost_matrix > max_distance] = max_distance + 1e-5
    row_indices, col_indices = linear_sum_assignment(cost_matrix)

    matches, unmatched_tracks, unmatched_detections = [], [], []
    for col, detection_idx in enumerate(detection_indices):
        if col not in col_indices:
            unmatched_detections.append(detection_idx)
    for row, track_idx in enumerate(track_indices):
        if row not in row_indices:
            unmatched_tracks.append(track_idx)
    for row, col in zip(row_indices, col_indices):
        track_idx = track_indices[row]
        detection_idx = detection_indices[col]
        if cost_matrix[row, col] > max_distance:
            unmatched_tracks.append(track_idx)
            unmatched_detections.append(detection_idx)
        else:
            matches.append((track_idx, detection_idx))
    return matches, unmatched_tracks, unmatched_detections


def matching_cascade(

        distance_metric, max_distance, cascade_depth, tracks, detections,

        track_indices=None, detection_indices=None):
    """Run matching cascade.

    Parameters

    ----------

    distance_metric : Callable[List[Track], List[Detection], List[int], List[int]) -> ndarray

        The distance metric is given a list of tracks and detections as well as

        a list of N track indices and M detection indices. The metric should

        return the NxM dimensional cost matrix, where element (i, j) is the

        association cost between the i-th track in the given track indices and

        the j-th detection in the given detection indices.

    max_distance : float

        Gating threshold. Associations with cost larger than this value are

        disregarded.

    cascade_depth: int

        The cascade depth, should be se to the maximum track age.

    tracks : List[track.Track]

        A list of predicted tracks at the current time step.

    detections : List[detection.Detection]

        A list of detections at the current time step.

    track_indices : Optional[List[int]]

        List of track indices that maps rows in `cost_matrix` to tracks in

        `tracks` (see description above). Defaults to all tracks.

    detection_indices : Optional[List[int]]

        List of detection indices that maps columns in `cost_matrix` to

        detections in `detections` (see description above). Defaults to all

        detections.

    Returns

    -------

    (List[(int, int)], List[int], List[int])

        Returns a tuple with the following three entries:

        * A list of matched track and detection indices.

        * A list of unmatched track indices.

        * A list of unmatched detection indices.

    """
    if track_indices is None:
        track_indices = list(range(len(tracks)))
    if detection_indices is None:
        detection_indices = list(range(len(detections)))

    unmatched_detections = detection_indices
    matches = []
    track_indices_l = [
        k for k in track_indices
        # if tracks[k].time_since_update == 1 + level
    ]
    matches_l, _, unmatched_detections = \
        min_cost_matching(
            distance_metric, max_distance, tracks, detections,
            track_indices_l, unmatched_detections)
    matches += matches_l
    unmatched_tracks = list(set(track_indices) - set(k for k, _ in matches))
    return matches, unmatched_tracks, unmatched_detections


def gate_cost_matrix(

        cost_matrix, tracks, detections, track_indices, detection_indices, mc_lambda,

        gated_cost=INFTY_COST, only_position=False):
    """Invalidate infeasible entries in cost matrix based on the state

    distributions obtained by Kalman filtering.

    Parameters

    ----------

    kf : The Kalman filter.

    cost_matrix : ndarray

        The NxM dimensional cost matrix, where N is the number of track indices

        and M is the number of detection indices, such that entry (i, j) is the

        association cost between `tracks[track_indices[i]]` and

        `detections[detection_indices[j]]`.

    tracks : List[track.Track]

        A list of predicted tracks at the current time step.

    detections : List[detection.Detection]

        A list of detections at the current time step.

    track_indices : List[int]

        List of track indices that maps rows in `cost_matrix` to tracks in

        `tracks` (see description above).

    detection_indices : List[int]

        List of detection indices that maps columns in `cost_matrix` to

        detections in `detections` (see description above).

    gated_cost : Optional[float]

        Entries in the cost matrix corresponding to infeasible associations are

        set this value. Defaults to a very large value.

    only_position : Optional[bool]

        If True, only the x, y position of the state distribution is considered

        during gating. Defaults to False.

    Returns

    -------

    ndarray

        Returns the modified cost matrix.

    """
    gating_dim = 2 if only_position else 4
    gating_threshold = kalman_filter.chi2inv95[gating_dim]
    measurements = np.asarray(
        [detections[i].to_xyah() for i in detection_indices])
    for row, track_idx in enumerate(track_indices):
        track = tracks[track_idx]
        gating_distance = track.kf.gating_distance(track.mean, track.covariance, measurements, only_position)
        cost_matrix[row, gating_distance > gating_threshold] = gated_cost
        cost_matrix[row] = mc_lambda * cost_matrix[row] + (1 - mc_lambda) *  gating_distance
    return cost_matrix