目标跟踪中的卡尔曼滤波和匈牙利算法解读。

先解读Sort算法：Simple online and realtime tracking
论文地址 https://arxiv.org/abs/1602.00763
代码地址
https://github.com/abewley/sort
https://github.com/YunYang1994/openwork/tree/master/sort

SORT 流程简介：

整个流程如下图所示：在第 1 帧时，人体检测器 detector 输出 3 个 bbox（黑色），模型会分别为这 3 个 bbox 创建卡尔曼滤波追踪器【tracker】 kf1，kf2 和 kf3。所以注意第一帧的追踪器是用目标检测的框创建的，ID也是我们手动赋予的。
对应人的编号为 1，2，3 。在第 2 帧的过程 a 中，如下图frame-2a，这 3 个跟踪器【每个人都有一个tracker】会利用上一帧的状态分别输出棕红色 bbox、黄色 bbox 和青绿色 bbox。
在这里插入图片描述
由于frame1中三个黑色bbox只是目标检测模型的输出，它们是没有bbox id的，所以，我们需要把目标检测框，和卡尔曼滤波的预测框进行一种关联，使得目标检测框的id就是滤波器的预测框id。
在SORT算法中，关联的核心是目标检测框和滤波器预测框之间的iou + 匈牙利算法的匹配。
我们计算一下Frame1 和 Frame2-a框之间的iou 如下：

iou	黑色bbox1	黑色bbox2	黑色bbox3
棕红色 bbox	0.91	0	0
黄色 bbox	0	0.98	0
青绿色 bbox	0	0	0.99

上述表格可以抽象为一个矩阵。矩阵中每一个值都是目标检测框到滤波器预测框之间的iou 。
我们若要求使得这个iou矩阵的值最大，那么这个矩阵就被称为利益矩阵profit matrix，若要使之最小，则被称为花费矩阵cost matrix。我们如果想让跟踪状态完美，就要求得这个iou矩阵的最大利益矩阵。
而SORT算法对iou值进行了一个变换，将iou值都变成1-iou，求的是1-iou矩阵的最小值。
这里的1-iou被定义为：目标检测框到滤波器预测框之间的iou距离。并且使用匈牙利算法进行最小化求解，这里用1-iou 或者-iou 都可以算法复杂度为O(n^3)。例子如下：

import numpy as np
from scipy.optimize import linear_sum_assignment

cost_matrix = np.array([[0.09, 1.00, 1.00],
                          [1.00, 0.02, 1.00],
                          [1.00, 1.00, 0.01]])

rows, cols = linear_sum_assignment(cost_matrix)
matches = list(zip(rows, cols))        # [(0, 0), (1, 1), (2, 2)] 得到匹配队列

这样我们就根据iou的值得到了匹配队列。

图解1：
在这里插入图片描述
图解2

KalmanBoxTracker

注意对应代码一起看

2.1卡尔曼滤波参数

状态变量 x 的设定是一个 7维向量：x=[u, v, s, r, u^, v^, s^]T。u、v
分别表示目标框的中心点位置的 x、y 坐标，s 表示目标框的面积，r 表示目标框的宽高比。 u^ ，v^ ，s^ 分别表示横向、纵向、面积 s 的运动变化速率。

参数初始化
u、v、s、r 初始化：根据第一帧的观测结果进行初始化。
u^ ，v^ ，s^ 初始化：当第一帧开始的时候初始化为0，到后面帧时会根据预测的结果来进行变化。

转换函数1：
def convert_bbox_to_z(bbox): 将 bbox 从 [x1,y1,x2,y2] 格式变成 [x,y,s,r] 4x1格式
转换函数2：
def convert_x_to_bbox(x,score=None): 将 bbox 从 [x,y,s,r] 格式变成 [x1,y1,x2,y2]格式，score是optional项
定义如下：

def convert_bbox_to_z(bbox):            
    """
    将 bbox 从 [x1,y1,x2,y2] 格式变成 [u,v,s,r] 格式，s是框的面积，r是w/h比例
    """
    w = bbox[2] - bbox[0]
    h = bbox[3] - bbox[1]
    x = bbox[0] + w/2.
    y = bbox[1] + h/2.
    s = w * h    #scale is just area
    r = w / float(h)
    return np.array([x, y, s, r]).reshape((4, 1))


def convert_x_to_bbox(x,score=None):     
    """
    将 bbox 从 [u,v,s,r] 格式变成 [x1,y1,x2,y2] 格式
    """
    s = x[2] #w*h
    ratio = x[3] # w/h
    w = np.sqrt(s * r)
    h = s / w 
    if(score==None):
        return np.array([x[0]-w/2.,x[1]-h/2.,x[0]+w/2.,x[1]+h/2.]).reshape((1,4))
    else:
        return np.array([x[0]-w/2.,x[1]-h/2.,x[0]+w/2.,x[1]+h/2.,score]).reshape((1,5))

状态转移矩阵 F
F被定义为一个7x7的矩阵，跟踪的目标被一个匀速运动目标，通过 7x7 的状态转移矩阵F 乘以 7*1 的状态变量 x 即可得到一个更新后的 7x1 的状态更新向量x
观测矩阵H
H被定义为一个 4x7 的矩阵，乘以 7x1 的状态更新向量 x 即可得到一个 4x1 的 [u,v,s,r] 的估计值。
协方差矩阵 RPQ
测量噪声的协方差矩阵 R: diag([1,1,10,10].T)
先验估计的协方差矩阵 P：diag([10,10,10,10,1e4,1e4,1e4].T)
过程激励噪声的协方差矩阵 Q：diag([1,1,1,1,0.01,0.01,1e-4].T)

hits = 总的匹配次数
hit_streak 连续匹配次数
time_since_update 连续没有匹配到目标检测框的次数
id = KalmanBoxTracker.count 记录当前追踪器的id

2.2 predict 追踪器预测阶段

在预测阶段，追踪器不仅需要预测 bbox，还要记录它自己的当前匹配情况。如果这个追踪器连续多次预测而没有进行一次更新操作，那么表明该跟踪器可能已经“失活”了。因为它没有和检测框匹配上，说明它之前记录的目标有可能已经消失或者误匹配了。但是也不一定会发生这种情况，还一种结果是目标在连续几帧消失后又出现在画面里。
考虑到这种情况，使用 time_since_update 记录了追踪器连续没有匹配上的次数，该变量在每次 predict 时都会加 1，每次 update 时都会归 0。并且使用了 max_age 设置了追踪器的最大存活期限，如果跟踪器出现超过连续 max_age 帧都没有匹配关联上，
即当 tracker.time_since_update > max_age 时，该跟踪器则会被判定失活而被移除列表。

2.3 update 更新阶段

大家都知道，卡尔曼滤波器的更新阶段是使用了观测值 z 来校正误差矩阵和更新卡尔曼增益，并计算出先验估计值和测量值之间的加权结果，该加权结果即为后验估计值。

class KalmanBoxTracker(object):
    """
    This class represents the internal state of individual tracked objects observed as bbox.
    """
    count = 0
    def __init__(self,bbox):
        """
       用初始目标检测框来初始化追踪器
        """
        #定义匀速运动模型
        self.kf = KalmanFilter(dim_x=7, dim_z=4) 
        #状态变量是7维， 观测值是4维的，按照需要的维度构建目标
        # 状态变量x的定义见下文
        self.kf.F = np.array([[1,0,0,0,1,0,0],      # 状态转移矩阵 7x7 维度
                              [0,1,0,0,0,1,0],
                              [0,0,1,0,0,0,1],
                              [0,0,0,1,0,0,0],
                              [0,0,0,0,1,0,0],
                              [0,0,0,0,0,1,0],
                              [0,0,0,0,0,0,1]])

        self.kf.H = np.array([[1,0,0,0,0,0,0],      # 观测矩阵，4x7 维度
                              [0,1,0,0,0,0,0],
                              [0,0,1,0,0,0,0],
                              [0,0,0,1,0,0,0]])

        self.kf.R[2:,2:] *= 10.
        self.kf.P[4:,4:] *= 1000. #give high uncertainty to the unobservable initial velocities
        self.kf.P *= 10.
        self.kf.Q[-1,-1] *= 0.01
        self.kf.Q[4:,4:] *= 0.01

        self.kf.x[:4] = convert_bbox_to_z(bbox)
        self.time_since_update = 0

        self.id = KalmanBoxTracker.count
        KalmanBoxTracker.count += 1

        self.history = [] # 存放着多个  [x1,y1,x2,y2]
        self.hits = 0 # 总的匹配次数
        self.hit_streak = 0 # 连续匹配次数
        self.age = 0

    def update(self,bbox):
        """
        用检测框更新追踪框
        """
        self.time_since_update = 0
        self.history = []
        self.hits += 1
        self.hit_streak += 1
        self.kf.update(convert_bbox_to_z(bbox))  # bbox 是观测值 [x1,y1,x2,y2] --> [u,v,s,r]

    def predict(self):
        """
        Advances the state vector and returns the predicted bounding box estimate.
        """
        if((self.kf.x[6]+self.kf.x[2])<=0):
            self.kf.x[6] *= 0.0
        self.kf.predict()
        self.age += 1
        if(self.time_since_update>0): # 一旦出现不匹配的情况，连续匹配次数归0
            self.hit_streak = 0
        self.time_since_update += 1 # 否则连续匹配次数+1
        self.history.append(convert_x_to_bbox(self.kf.x)) #  # [u,v,s,r] --> [x1,y1,x2,y2]
        return self.history[-1]

    def get_state(self):
        """
        Returns the current bounding box estimate.
        """
        return convert_x_to_bbox(self.kf.x)

3. bbox 关联匹配

bbox 的关联匹配过程在前面已经讲得很详细了，它是将 tracker 输出的预测框（注意是先验估计值）和 detector 输出的检测框相关联匹配起来。输入是 dets： [[x1,y1,x2,y2,score],…] 和 trks： [[x1,y1,x2,y2,tracking_id],…] 以及一个设定的 iou 阈值，该门槛是为了过滤掉那些低重合度的目标。
代码中linear assigment使用的匈牙利算法我们在本文最后面详细介绍。

def associate_detections_to_trackers(dets, trks, iou_threshold = 0.3):
    """
    Assigns detections to tracked object (both represented as bounding boxes)
        dets:
            [[x1,y1,x2,y2,score],...]
        trks:
            [[x1,y1,x2,y2,tracking_id],...]
    Returns 3 lists of matches, unmatched_detections and unmatched_trackers
    """

该过程返回三个列表：
matches（已经匹配成功的追踪器）,
unmatched_detections（没有匹配成功的检测目标）
unmatched_trackers（没有匹配成功的跟踪器）

对于已经匹配成功的追踪器，则需要用观测值（目标检测框）去更新校正 tracker 并输出修正后的 bbox；对于没有匹配成功的检测目标，则需要新增 tracker 与之对应；
对于没有匹配成功的跟踪器，如果长时间处于失活状态，则可以考虑删除了。
所以整理多目标跟踪MOT算法的流程如下：

看完上图流程图，我们来仔细看看objec detection box和 Kalman Filter tracker之间关联的具体逻辑：

def associate_detections_to_trackers(detections,trackers,iou_threshold = 0.3):  

  """
  detections[ x1, y1 ,x2, y2, score, ... ]  Nx5
  trackers [:, x1, y1, x2, y3, tracker_id, ... ] Mx5
  图中IOU Match版块用于将检测与跟踪进行关联
  将目标检测框匹配到滤波器预测框tracker。
  返回 三个列表 matches, unmatched_detections and unmatched_trackers
  """
  if(len(trackers)==0):  #如果跟踪器为空
    return np.empty((0,2),dtype=int), np.arange(len(detections)), np.empty((0,5),dtype=int)
       
  #初始化检测器与跟踪器IOU Matrix
  iou_matrix = np.zeros((len(detections),len(trackers)),dtype=np.float32) 
  
  
  #计算det 和 tracker 之间的iou matrix
  for d, det in enumerate(detections):
    for t, trk in enumerate(trackers):
      iou_matrix[d,t] = iou(det,trk)  #计算检测器与跟踪器的IOU并赋值给 iou matrix 对应位置
  #最终iou_matrix的shape是NxM
  matched_indices = linear_assignment(-iou_matrix)  #只是粗匹配，后面还要过滤iou低的
  # 这里的linear assignment使用的就是匈牙利算法
  # 加上负号是因为linear_assignment求的是最小代价组合，而我们需要的是IOU最大的组合方式，所以取负号 
  # 参考的是：https://blog.csdn.net/herr_kun/article/details/86509591    
  
 
  unmatched_detections = []    #未匹配上的检测器
  for d, det in enumerate(detections):
    if(d not in matched_indices[:, 0]):  #如果检测器中第d个检测结果不在匹配结果索引中，则d未匹配上
      unmatched_detections.append(d)
      
  unmatched_trackers = []      #未匹配上的跟踪器
  for t, trk in enumerate(trackers):
    if(t not in matched_indices[:,1]):  #如果跟踪器中第t个跟踪结果不在匹配结果索引中，则t未匹配上
      unmatched_trackers.append(t)
 
  # filter out matched pair with low IOU，过滤掉那些IOU较小的匹配对
  matches = []  #存放过滤掉低iou之后的最终匹配结果
  for m in matched_indices:   #遍历粗匹配结果
    if(iou_matrix[m[0], m[1]] < iou_threshold):  
    # m[0]是检测框ID， m[1]是跟踪框ID，如果它们的IOU小于阈值则将它们视为未匹配成功
      unmatched_detections.append(m[0])
      unmatched_trackers.append(m[1])
    else:
      matches.append(m.reshape(1,2))         # 匹配上的则以 [[d,t]...] 形式放入 matches 矩阵
  if(len(matches)==0):           #如果过滤后匹配结果为空，那么返回空的匹配结果
    matches = np.empty((0,2),dtype=int)  
  else:      #如果过滤后匹配结果非空，则按0轴【纵向】继续添加匹配对
    matches = np.concatenate(matches,axis=0) 
 
  # 返回：跟踪成功的矩阵，新增物体的矩阵， 消失物体的矩阵
  return matches, np.array(unmatched_detections), np.array(unmatched_trackers)  
  # matches有2列，第1列是检测框id，第2列是预测框id
  # unmatched_detections有5列，前4列是xyxy，第5列是score
  # 其中跟踪器数组是5列的（最后一列是ID）

将上述所有流程串联起来就得到了SORT算法

class Sort(object):
    def __init__(self, max_age=1, min_hits=3, iou_threshold=0.3):
        """
        Sets key parameters for SORT
        """
        self.max_age = max_age      # 在没有目标检测关联的情况下追踪器存活的最大帧数
        self.min_hits = min_hits    # 追踪器初始化前的最小关联检测数量
        self.iou_threshold = iou_threshold

        self.trackers = []          # 用于存储卡尔曼滤波追踪器的列表
        self.frame_count = 0        # 当前追踪帧的编号

    def update(self, dets=np.empty((0, 5))):
        """
        Params:
        输入的是目标检测矩阵，形式是[[x1,y1,x2,y2,score],[x1,y1,x2,y2,score],...]
        这个方法对每一帧都必须使用一次，即使该帧没有检测到任何目标。
        返回相似度矩阵，矩阵最后一列是追踪框ID
        NOTE：输入的检测框个数 和 返回的框个数，可能是不同的
        """
        self.frame_count += 1 # 帧数+1
        trks = [] # 用于存放跟踪预测的 bbox: [x1,y1,x2,y2,id]
        
		# 初始化的时候self.trackers是空的，跳过下面的for循环
        for i, tracker in enumerate(self.trackers): # 遍历卡尔曼跟踪列表
            pos = tracker.predict()[0]   
            # 用卡尔曼跟踪器 trackers预测 bbox
            if not np.any(np.isnan(pos)):     # 如果卡尔曼的预测框有效
                trks.append([pos[0], pos[1], pos[2], pos[3], 0])   # 存放上一帧所有物体预测有效的 bbox
            else:
                self.trackers.remove(tracker)    # 如果无效， 删除该滤波器

        trks = np.array(trks)
        self.trks = trks     # 为了显示跟踪器预测的框，把它拿出来

        # 将目标检测的 bbox 和卡尔曼滤波预测的跟踪 bbox 匹配
        # 获得 跟踪成功的矩阵，新增物体的矩阵，消失物体的矩阵
        matched, unmatched_dets, unmatched_trks = associate_detections_to_trackers(dets, trks, self.iou_threshold)

        # 跟踪成功的物体 bbox 信息更新到对应的卡尔曼滤波器
        for m in matched:
            self.trackers[m[1]].update(dets[m[0], :])

        # 为新增物体创建新的卡尔曼滤波跟踪器
        for i in unmatched_dets:
            tracker = KalmanBoxTracker(dets[i,:])
            self.trackers.append(tracker)

        # 跟踪器更新校正后，输出最新的 bbox 和 id
        ret = []
        for tracker in self.trackers:
            if (tracker.time_since_update < 1) and (tracker.hit_streak >= self.min_hits or self.frame_count <= self.min_hits):
                d = tracker.get_state()[0]
                ret.append([d[0], d[1], d[2], d[3], tracker.id+1]) # +1 as MOT benchmark requires positive

            # 长时间离开画面/跟踪失败的物体从卡尔曼跟踪器列表中删除
            if(tracker.time_since_update > self.max_age):
                self.trackers.remove(tracker)

        # 返回当前画面中所有被跟踪物体的 bbox 和 id，矩阵形式为 [[x1,y1,x2,y2,id]...]
        return np.array(ret) if len(ret) > 0 else np.empty((0,5))