SORT 追跡アルゴリズムとその Python 実装例の簡単な紹介

#python
import numpy as np
from filterpy.kalman import KalmanFilter
from scipy.optimize import linear_sum_assignment

class Track:

def init(self,prediction,track_id,track_lifetime):
    self.prediction=np.atleast_2d(prediction)
    self.track_id=track_id
    self.track_lifetime=track_lifetime
    self.age=0
    self.total_visible_count=1
    self.consecutive_invisible_count=0

def predict(self, kf):
    self.prediction = kf.predict()
    self.age += 1

def update(self, detection, kf):
    self.prediction = kf.update(detection)
    self.total_visible_count += 1
    self.consecutive_invisible_count = 0

def mark_missed(self):
    self.consecutive_invisible_count += 1

def is_dead(self):
    return self.consecutive_invisible_count >= self.track_lifetime

class Tracker:

def init(self,track_lifetime,detection_variance,process_variance):
    self.next_track_id=0
    self.tracks=[]
    self.track_lifetime=track_lifetime
    self.detection_variance=detection_variance
    self.process_variance=process_variance
    self.kf=KalmanFilter(dim_x=4,dim_z=2)
    self.kf.F=np.array([[1,0,1,0],
                    [0,1,0,1],
                    [0,0,1,0],
                    [0,0,0,1]])
    self.kf.H=np.array([[1,0,0,0],
                    [0,1,0,0]])
    self.kf.R=np.array([[self.detection_variance,0],
                    [0,self.detection_variance]])
    self.kf.Q=np.array([[self.process_variance,0,0,0],
                    [0,self.process_variance,0,0],
                    [0,0,self.process_variance,0],
                    [0,0,0,self.process_variance]])

def update(self, detections):
    # predict track positions using Kalman filter
    for track in self.tracks:
        track.predict(self.kf)

    # associate detections with tracks using Hungarian algorithm
    if len(detections) > 0:
        num_tracks = len(self.tracks)
        num_detections = len(detections)
        cost_matrix = np.zeros((num_tracks, num_detections))
        for i, track in enumerate(self.tracks):
            for j, detection in enumerate(detections):
                diff = track.prediction - detection
                distance = np.sqrt(diff[0,0]**2 + diff[0,1]**2)
                cost_matrix[i,j] = distance
        row_indices, col_indices = linear_sum_assignment(cost_matrix)
        unassigned_tracks = set(range(num_tracks)) - set(row_indices)
        unassigned_detections = set(range(num_detections)) - set(col_indices)
        for i, j in zip(row_indices, col_indices):
            self.tracks[i].update(detections[j], self.kf)
        for i in unassigned_tracks:
            self.tracks[i].mark_missed()
        for j in unassigned_detections:
            new_track = Track(detections[j], self.next_track_id, self.track_lifetime)
            self.tracks.append(new_track)
            self.next_track_id += 1

    # remove dead tracks
    self.tracks = [track for track in self.tracks if not track.is_dead()]

    # return list of track positions
    return [track.prediction.tolist()[0] for track in self.tracks]