1.SORT核心是卡尔曼滤波和匈牙利算法。
  流程图如下所示，可以看到整体可以拆分为两个部分，分别是匈牙利匹配过程和卡尔曼预测加更新过程，都用灰色框标出来了。
  关键步骤：
    --> 卡尔曼滤波预测出预测框
    --> 使用匈牙利算法将卡尔曼滤波的预测框和yolo的检测框进行IOU匹配来计算相似度
    --> 卡尔曼滤波使用yolo的检测框更新卡尔曼滤波的预测框

2.卡尔曼滤波分为两个过程：预测过程和更新过程。
  SORT引入了线性速度模型与卡尔曼滤波来进行位置预测，先进行位置预测然后再进行匹配。运动模型的结果可以用来预测物体的位置。
  匈牙利算法解决的是一个分配问题，用IOU距离作为权重(也即cost代价矩阵)，并且当IOU小于一定数值(IOU阈值)时，
  不认为是同一个目标，理论基础是视频中两帧之间物体移动不会过多。
  在代码中选取的IOU阈值是0.3。scipy库的linear_sum_assignment实现了匈牙利算法，只需要输入cost_matrix代价矩阵(全部预测框和全部检测框两两IOU计算结果)
  到linear_sum_assignment中就能得到预测框和检测框两两最优匹配的组合。

1.跟踪器链(列表)：
    实际就是多个的卡尔曼滤波KalmanBoxTracker自定义类的实例对象组成的列表。
            每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
            KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
            并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。
    把每个卡尔曼滤波器(KalmanBoxTracker实例对象)都存储到跟踪器链(列表)中。

2.unmatched_detections(列表)：
    检测框中出现新目标，但此时预测框(跟踪框)中仍不不存在该目标，
    那么就需要在创建新目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)，
    然后把新目标对应的KalmanBoxTracker类的实例对象放到跟踪器链(列表)中。

3.unmatched_trackers(列表)：
    当跟踪目标失败或目标离开了画面时，也即目标从检测框中消失了，就应把目标对应的跟踪框(预测框)从跟踪器链中删除。
    unmatched_trackers列表中保存的正是跟踪失败即离开画面的目标，但该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)
    此时仍然存在于跟踪器链(列表)中，因此就需要把该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)从跟踪器链(列表)中删除出去。

DeepSORT是SORT的续作，整体框架没有大改，还是延续了卡尔曼滤波加匈牙利算法的思路，并且在这个基础上增加了鉴别网络Deep Association Metric。
下图是deepSORT流程图，和SORT基本一样，就多了级联匹配(Matching Cascade)和新轨迹的确认(confirmed)。

关键步骤：
    --> 卡尔曼滤波预测出预测框
    --> 使用匈牙利算法将卡尔曼滤波的预测框和yolo的检测框进行级联匹配加IOU匹配两者分别来计算相似度
    --> 卡尔曼滤波使用yolo的检测框更新卡尔曼滤波的预测框

级联匹配计算相似度的流程图如下所示：
    上半部分为相似度估计，也就是计算这个分配问题的代价矩阵。
    下半部分依旧是使用匈牙利算法进行检测框和预测框的匹配。

1.原始图片resize到448x448，经过前面卷积网络之后，将图片输出成了一个7 x 7 x 30的特征图。
  根据7x7的特征图大小在输入原图上分成7x7=49个网格，如果目标的中心落入到某个网格单元cell中，那么该网格单元cell就负责检测该目标。
  7x7的特征图大小：7x7=49个像素值，理解成49个单元格，每个单元格可以代表原图的一个方块。
2.每个网格单元cell都会预测N个边界框bounding boxes和每个bbox框的1个置信度分数confidence scores，
  这些置信度分数反映了该模型对那个框内是否包含目标的信心，以及它对自己的预测的准确度的估量。
3.yolo V1、yolo V2、yolo V3 的bbox(边界框bounding boxes)数目变化
    1.yolo V1：
        1.每个网格单元cell预测2个(默认)bbox(边界框bounding boxes)：输入图像一共有 7x7x2=98个bbox(边界框bounding boxes)
        2.每个bbox(边界框bounding boxes)包含4个预测位置(x、y、w、h)，1个bbox置信度分数confidence scores
        3.一个网格单元预测cell的2个(默认)bbox(边界框bounding boxes)一共的预测数据量：2x(4+1)+20=30
            4个预测位置(x、y、w、h)和1个bbox置信度分数confidence scores代表一个bbox的预测数据量。
            20代表1个bbox的20个类别的预测概率值，每个单元格选出20个类别的预测概率中的最大概率值的一个类别，那么一个单元格就只能代表一个类别，
            这也是yolo V1的缺点，yolo V1就只会1个单元格cell预测1个目标物体，对于检测效果并不好，因此并不建议使用yolo V1。
        4.输入图像一共预测的数据量：7x7x(2x(4+1)+20)=1470
            7x7的特征图大小即输入图像中的网格数。
            2x(4+1)即每个网格都预测2个边界框bounding boxes的4个预测位置(x、y、w、h)，1个bbox置信度分数confidence scores。
            加20即是因为在yolo V1中一个网格中的2个bbox中最终只会有1个bbox用于预测目标物体，因此如果多个目标物体都在一个单元格cell中出现的话，
            如果使用了yolo V1就只会1个单元格cell预测1个目标物体，对于检测效果并不好，因此并不建议使用yolo V1。

    2.yolo V2：
        1.每个网格单元cell都使用5种(默认)不同尺寸的锚框Anchor boxes来预测bbox(边界框bounding boxes)，
          一个网格单元cell中每种不同尺寸的锚框Anchor boxes各预测一个bbox(边界框bounding boxes)，一共预测5个(默认)bbox(边界框bounding boxes)。
          输入图像一共预测有 13x13x5=845个bbox(边界框bounding boxes)。
          注意：5个(默认)的锚框Anchor boxes的尺寸大小都是不一样的。
        2.每种不同尺寸的锚框Anchor boxes所预测的bbox(边界框bounding boxes)包含：4个预测位置(x、y、w、h)，1个bbox置信度分数confidence scores，
          N个分类类别的预测概率值。
        3.一个网格单元cell中5种(默认)不同尺寸的锚框Anchor boxes所预测的5个(默认)bbox(边界框bounding boxes)一共预测的数据量(假如预测20个类别)：
            5x(4+1+20)=125
            5代表5个(默认)bbox(边界框bounding boxes)。
            每个bbox(边界框bounding boxes)都分别有4个预测位置(x、y、w、h)，1个bbox置信度分数confidence scores，20个类别的预测概率值。
        4.输入图像一共预测的数据量(假如预测20个类别和在13x13特征图上做预测)：13x13x(5x(4+1+20))=169*125=21125
        5.YOLO V2基于卷积的Anchor机制(Convolutional With Anchor Boxes)：从YOLOV1中移除全连接层，
              并使用5个(默认)不同尺寸的锚框Anchor boxes来预测bbox(边界框bounding boxes)。
          YOLO V2通过缩减网络，使用416x416的输入，模型下采样的总步长为32，最后得到13x13的特征图，13x13的特征图对应在输入原图分割13x13个单元格cell。
          每个单元格cell预测5个不同尺寸锚框anchor boxes对应的bbox(边界框bounding boxes)，每个锚框anchor box所预测的bbox(边界框bounding boxes)
          包含4个位置信息、1个置信度、N个分类类别的概率值。
          YOLO V2采用的5种不同尺寸锚框Anchor boxes可以预测13x13x5=845个bbox(边界框bounding boxes)。
          YOLO V2引?faster rcnn中anchor机制，anchor尺度就是用来预测网络预测值和目标GT做尺度变换的。
        6.维度聚类
            Faster-RCNN中anchor boxes的个数和宽高维度往往是手动精选的先验框（hand-picked priors)，
            设想能否一开始就选择了更好的、更有代表性的先验boxes维度，那么网络就应该更容易学到准确的预测位置，
            YOLOv2使用k-means聚类算法对训练集中的边界框做了聚类分析，尝试通过维度聚类找到合适尺寸的锚框Anchor boxes。
            YOLOV2没有使用FasterRCNN预测偏移和尺度变换。而是遵循YOLOV1的方法，预测相对于网格单元位置的位置坐标。
            这使得真实值的界限在0到1之间。
 
    3.yolo V3：
        1.特征金字塔(FPN网络)
            yolo V3使用了特征金字塔(FPN网络)，yolov3在3个不同尺度的特征图上做预测。
            比如：13x13,26x26,52x52三个不同尺度特征图上的每个单元格cell分别使用3种(默认)不同尺寸的锚框Anchor boxes来预测bbox(边界框bounding boxes)。
            每种不同尺寸的锚框Anchor boxes所预测的bbox(边界框bounding boxes)包含：4个预测位置(x、y、w、h)，1个bbox置信度分数confidence scores，
            N个分类类别的概率值。
            那么一个NxN的特征图大小就有NxN个网格单元cell，那么每个网格单元cell预测数据量为3x(4+1+N个分类类别的概率值)，
            一整个特征图一共预测数据量为NxNx(3x(4+1+N个分类类别的概率值))。
            yolo V3在13x13,26x26,52x52大小的三个特征图做预测计算，特征图比例大小分别是13x13为NxN，26x26为2x(NxN)，52x52为4x(NxN)，
            那么3个不同尺度特征图一共预测数据量为(NxN + 2x(NxN) + 4x(NxN)) x (3x(4+1+N个分类类别的概率值))
        2.每种不同尺度特征图上所设置的先验框(bbox边界框bounding boxes)大小，
          会从下面的array数组yolo_anchors中选出对应合适的组合作为先验框(bbox边界框bounding boxes)的大小。
            yolo_anchors = np.array([(10, 13), (16, 30), (33, 23), (30, 61), (62, 45), (59, 119), (116, 90), (156, 198), (373, 326)],
                      np.float32) / 416

1.yolo V2、yolo V3中的每个bbox(边界框bounding boxes)的预测值：4个预测位置(x、y、w、h)、1个bbox置信度分数confidence scores、N个类别的预测概率值。
2.(x, y) 表示bbox的中心点相对于单元格(grid cell)原点的偏移值，单元格(grid cell)的原点即为当前单元格的左上角的顶点(top-left)，
  yolo将左上角的顶点(原点)设置为(0, 0)，右下角的顶点设置为(1, 1)，所以x和y的取值范围都分别是在0到1之间。
  x和y将始终介于0到1之间，因为中心点始终位于单元格(grid cell)内。
  之所以把(x, y)预测为相对于网格单元cell的位置坐标，使得真实值的界限在0到1之间，因此参数化更容易学习，从而使网络更加稳定。
3.(w, h) 表示为相对于整张图片的宽和高, 即使用图片的宽和高标准化自己。
  如果边界框bbox的尺寸小于单元格(grid cell)的尺寸的话，w和h的取值范围都分别是在0到1之间。
  如果边界框bbox的尺寸大于单元格(grid cell)的尺寸的话，w和h的取值范围都可以大于1。
4.yolo V2、yolo V3都基于卷积的Anchor机制(Convolutional With Anchor Boxes)
    yolo V2使用5种不同尺寸的锚框Anchor boxes预测边界框的4个位置信息、1个置信度、N个分类类别的概率值。
    yolo V3使用3种不同尺寸的锚框Anchor boxes预测边界框的4个位置信息、1个置信度、N个分类类别的概率值。
5.比如在yolo V2中，13*13特征图上的每个单元格(grid cell)中预测5个不同尺寸的锚框Anchor boxes。
  anchor尺寸就是用来预测网络预测值和目标GT之间做尺度变换的。
  比如下面的蓝色框是预测的bbox(边界框bounding boxes)，黑色点的矩形框是锚框Anchor boxes。
  每一个bbox(边界框bounding boxes)都预测：tx、ty、tw、th、to(置信度)。
  如果这个单元格(grid cell)距离输入原图的左上角原点的边距离为(cx，cy)，该单元格(grid cell)对应的边界框bbox维度(边界框优先bounding box prior)的长和宽分别为(pw，ph)，
  那么对应的边界框bbox计算结果实际为：
    1.yolo V2中不同尺寸的锚框Anchor boxes所预测的bbox(边界框bounding boxes)的4个位置信息为(tx, ty, tw, th)，
      那么tx和ty分别为相对于单元格(grid cell)原点的0到1之间取值的值，tw和th则根据所预测的bbox(边界框bounding boxes)是大于还是小于单元格(grid cell)的尺寸来决定
      tw和th的取值范围是在0到1之间还是在大于1。
    2.pw和ph分别为手动设定的锚框Anchor boxes宽和高，而网络最终计算的预测结果为(bx, by, bw, bh)，因此需要把(tx, ty, tw, th)转换为(bx, by, bw, bh)。
    3.把(tx, ty, tw, th)转换为(bx, by, bw, bh)作为yolo输出层的最终输出：
        σ读作sigma。Cx和Cy分别为当前单元格(grid cell)距离输入原图的左上角原点的边距离。W和H为输入原图像的宽和高。分别除以W和H，目的是归一化。
        tx->bx：bx = (σ(tx) + Cx) / W
        ty->by：by = (σ(ty) + Cy) / H
        tw->bw：bw = (pw * e^tw) / W
        th->bh：bh = (ph * e^th) / H
    4.σ(tx) + Cx：边界框的中心点在输入原图像中的x坐标，也即边界框的中心点离输入原图像原点的x方向长度
      σ(ty) + Cy：边界框的中心点在输入原图像中的y坐标，也即边界框的中心点离输入原图像原点的y方向长度
      pw * e^tw：边界框在输入原图像中的宽度
      ph * e^th：边界框在输入原图像中的高度

from __future__ import print_function
# 对for循环有较好的效果
from numba import jit
import numpy as np
# 用于线性分配，匈牙利匹配的实现
# from sklearn.utils.linear_assignment_ import linear_assignment
from scipy.optimize import linear_sum_assignment
# 使用卡尔曼滤波器
from filterpy.kalman import KalmanFilter


def convert_bbox_to_z(bbox):
    """
    将[x1,y1,x2,y2]形式的检测框转为滤波器的状态表示形式[x,y,s,r]。
    其中x、y是框的中心坐标点，s 是面积尺度，r 是宽高比w/h
    :param bbox: [x1,y1,x2,y2] 分别是左上角坐标和右下角坐标 即 [左上角的x坐标，左上角的y坐标，右下角的x坐标，右下角的y坐标]
    :return: [ x, y, s, r ] 4行1列，其中x、y是box中心位置的坐标，s是面积，r是纵横比w/h
    """
    w = bbox[2] - bbox[0]  # 右下角的x坐标 - 左上角的x坐标 = 检测框的宽
    h = bbox[3] - bbox[1]  # 右下角的y坐标 - 左上角的y坐标 = 检测框的高
    x = bbox[0] + w / 2.  # 左上角的x坐标 + 宽/2 = 检测框中心位置的x坐标
    y = bbox[1] + h / 2.  # 左上角的y坐标 + 高/2 = 检测框中心位置的y坐标
    s = w * h  # 检测框的宽 * 高 = 检测框面积
    r = w / float(h)  # 检测框的宽w / 高h = 宽高比
    # 因为卡尔曼滤波器的输入格式要求为4行1列，因此该[x, y, s, r]的形状要转换为4行1列再输入到卡尔曼滤波器
    return np.array([x, y, s, r]).reshape((4, 1))


"""
将候选框从中心面积的形式[x,y,s,r] 转换为 坐标的形式[x1,y1,x2,y2]
"""
def convert_x_to_bbox(x, score=None):
    """
    将[cx，cy，s，r]的目标框表示转为[x_min，y_min，x_max，y_max]的形式
    :param x:[ x, y, s, r ],其中x,y是box中心位置的坐标，s是面积，r是纵横比w/h
    :param score: 置信度
    :return:[x1,y1,x2,y2],左上角坐标和右下角坐标
    """
    """
    x[2]：s是面积，原公式s的来源为s = w * h，即检测框的宽 * 高 = 检测框面积。
    x[3]：r是纵横比w/h，原公式r的来源为r = w / float(h)，即检测框的宽w / 高h = 宽高比。
    x[2] * x[3]：s*r 即(w * h) * (w / float(h)) = w^2
    sqrt(x[2] * x[3])：sqrt(w^2) = w
    """
    w = np.sqrt(x[2] * x[3])  # sqrt(w^2) = w
    h = x[2] / w  # w * h / w = h
    if score is None:
        return np.array([x[0] - w / 2.,  # 检测框中心位置的x坐标 - 宽 / 2 = 左上角的x坐标
                         x[1] - h / 2.,  # 检测框中心位置的y坐标 - 高 / 2 = 左上角的y坐标
                         x[0] + w / 2.,  # 检测框中心位置的x坐标 + 宽 / 2 = 右下角的x坐标
                         x[1] + h / 2.]  # 检测框中心位置的y坐标 + 高 / 2 = 右下角的y坐标
                        ).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))


"""
卡尔曼滤波器进行跟踪的相关内容的实现
    目标估计模型：
        1.根据上一帧的目标框结果来预测当前帧的目标框状态，预测边界框(目标框)的模型定义为一个等速运动/匀速运动模型。
        2.每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
          KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
          并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。  
        3.yoloV3、卡尔曼滤波器预测/更新流程步骤
            1.第一步：
                yoloV3目标检测阶段：
                    --> 1.检测到目标则创建检测目标链/跟踪目标链，反之检测不到目标则重新循环目标检测。
                    --> 2.检测目标链/跟踪目标链不为空则进入卡尔曼滤波器predict预测阶段，反之为空则重新循环目标检测。
            2.第二步：
                卡尔曼滤波器predict预测阶段：
                    连续多次预测而不进行一次更新操作，那么代表了每次预测之后所进行的“预测目标和检测目标之间的”相似度匹配都不成功，
                    所以才会出现连续多次的“预测然后相似度匹配失败的”情况，导致不会进入一次更新阶段。
                    如果一次预测然后相似度匹配成功的话，那么然后就会进入更新阶段。

                    --> 1.目标位置预测
                                1.kf.predict()：目标位置预测
                                2.目标框预测总次数：age+=1。
                                3.if time_since_update > 0:
                                     hit_streak = 0
                                  time_since_update += 1
                                  1.连续预测的次数，每执行predict一次即进行time_since_update+=1。
                                  2.在连续预测(连续执行predict)的过程中，一旦执行update的话，time_since_update就会被重置为0。
                                  3.在连续预测(连续执行predict)的过程中，只要连续预测的次数time_since_update大于0的话，
                                    就会把hit_streak(连续更新的次数)重置为0，表示连续预测的过程中没有出现过一次更新状态更新向量x(状态变量x)的操作，
                                    即连续预测的过程中没有执行过一次update。
                                  4.在连续更新(连续执行update)的过程中，一旦开始连续执行predict两次或以上的情况下，
                                    当连续第一次执行predict时，因为time_since_update仍然为0，并不会把hit_streak重置为0，
                                    然后才会进行time_since_update+=1；
                                    当连续第二次执行predict时，因为time_since_update已经为1，那么便会把hit_streak重置为0，
                                    然后继续进行time_since_update+=1。
                    --> 2.预测的目标和检测的目标之间的相似度匹配成功则进入update更新阶段，反之匹配失败则删除跟踪目标。
            3.第三步：
                卡尔曼滤波器update更新阶段：
                    如果一次预测然后“预测目标和检测目标之间的”相似度匹配成功的话，那么然后就会进入更新阶段。
                    kf.update([x,y,s,r])：使用的是通过yoloV3得到的“并且和预测框相匹配的”检测框来更新预测框。

                    --> 1.目标位置信息更新到检测目标链/跟踪目标链
                                1.目标框更新总次数：hits+=1。
                                2.history = []
                                  time_since_update = 0
                                  hit_streak += 1
                                    1.history列表用于在预测阶段保存单个目标框连续预测的多个结果，一旦执行update就会清空history列表。
                                    2.连续更新的次数，每执行update一次即进行hit_streak+=1。
                                    3.在连续预测(连续执行predict)的过程中，一旦执行update的话，time_since_update就会被重置为0。
                                    4.在连续预测(连续执行predict)的过程中，只要连续预测的次数time_since_update大于0的话，
                                      就会把hit_streak(连续更新的次数)重置为0，表示连续预测的过程中没有出现过一次更新状态更新向量x(状态变量x)的操作，
                                      即连续预测的过程中没有执行过一次update。
                                    5.在连续更新(连续执行update)的过程中，一旦开始连续执行predict两次或以上的情况下，
                                      当连续第一次执行predict时，因为time_since_update仍然为0，并不会把hit_streak重置为0，
                                      然后才会进行time_since_update+=1；
                                      当连续第二次执行predict时，因为time_since_update已经为1，那么便会把hit_streak重置为0，
                                      然后继续进行time_since_update+=1。
                    --> 2.目标位置修正。
                                1.kf.update([x,y,s,r])：
                                        使用观测到的目标框bbox更新状态变量x(状态更新向量x)。
                                        使用的是通过yoloV3得到的“并且和预测框相匹配的”检测框来更新卡尔曼滤波器得到的预测框。

    1.初始化、预测、更新
        1.__init__(bbox)：
            初始化卡尔曼滤波器的状态更新向量x(状态变量x)、观测输入[x,y,s,r](通过[x1,y1,x2,y2]转化而来)、状态转移矩阵F、
            量测矩阵H(观测矩阵H)、测量噪声的协方差矩阵R、先验估计的协方差矩阵P、过程激励噪声的协方差矩阵Q。
        2.update(bbox)：根据观测输入来对状态更新向量x(状态变量x)进行更新
        3.predict()：根据状态更新向量x(状态变量x)更新的结果来预测目标的边界框

    2.状态变量、状态转移矩阵F、量测矩阵H(观测矩阵H)、测量噪声的协方差矩阵R、先验估计的协方差矩阵P、过程激励噪声的协方差矩阵Q
        1.状态更新向量x(状态变量x)
            状态更新向量x(状态变量x)的设定是一个7维向量：x=[u,v,s,r,u^,v^,s^]T。
            u、v分别表示目标框的中心点位置的x、y坐标，s表示目标框的面积，r表示目标框的纵横比/宽高比。
            u^、v^、s^分别表示横向u(x方向)、纵向v(y方向)、面积s的运动变化速率。
            u、v、s、r初始化：根据第一帧的观测结果进行初始化。
            u^、v^、s^初始化：当第一帧开始的时候初始化为0，到后面帧时会根据预测的结果来进行变化。
        2.状态转移矩阵F
            定义的是一个7*7的方阵(其对角线上的值都是1)。。
            运动形式和转换矩阵的确定都是基于匀速运动模型，状态转移矩阵F根据运动学公式确定，跟踪的目标假设为一个匀速运动的目标。
            通过7*7的状态转移矩阵F 乘以 7*1的状态更新向量x(状态变量x)即可得到一个更新后的7*1的状态更新向量x，
            其中更新后的u、v、s即为当前帧结果。
        3.量测矩阵H(观测矩阵H)
            量测矩阵H(观测矩阵H)，定义的是一个4*7的矩阵。
            通过4*7的量测矩阵H(观测矩阵H) 乘以 7*1的状态更新向量x(状态变量x) 即可得到一个 4*1的[u,v,s,r]的估计值。
        4.测量噪声的协方差矩阵R、先验估计的协方差矩阵P、过程激励噪声的协方差矩阵Q
            1.测量噪声的协方差矩阵R：diag([1,1,10,10]T)
            2.先验估计的协方差矩阵P：diag([10,10,10,10,1e4,1e4,1e4]T)。1e4：1x10的4次方。
            3.过程激励噪声的协方差矩阵Q：diag([1,1,1,1,0.01,0.01,1e-4]T)。1e-4：1x10的-4次方。
            4.1e数字的含义
                1e4：1x10的4次方
                1e-4：1x10的-4次方
            5.diag表示对角矩阵，写作为diag(a1，a2,...,an)的对角矩阵实际表示为主对角线上的值依次为a1，a2,...,an，
              而主对角线之外的元素皆为0的矩阵。
              对角矩阵(diagonal matrix)是一个主对角线之外的元素皆为0的矩阵，常写为diag（a1，a2,...,an) 。
              对角矩阵可以认为是矩阵中最简单的一种，值得一提的是：对角线上的元素可以为 0 或其他值，对角线上元素相等的对角矩阵称为数量矩阵；
              对角线上元素全为1的对角矩阵称为单位矩阵。对角矩阵的运算包括和、差运算、数乘运算、同阶对角阵的乘积运算，且结果仍为对角阵。
"""
"""
1.跟踪器链(列表)：
    实际就是多个的卡尔曼滤波KalmanBoxTracker自定义类的实例对象组成的列表。
    每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
    KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
    并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。
    把每个卡尔曼滤波器(KalmanBoxTracker实例对象)都存储到跟踪器链(列表)中。
2.unmatched_detections(列表)：
    1.检测框中出现新目标，但此时预测框(跟踪框)中仍不不存在该目标，
      那么就需要在创建新目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)，
      然后把新目标对应的KalmanBoxTracker类的实例对象放到跟踪器链(列表)中。
    2.同时如果因为“跟踪框和检测框之间的”两两组合的匹配度IOU值小于iou阈值，
      则也要把目标检测框放到unmatched_detections中。
3.unmatched_trackers(列表)：
    1.当跟踪目标失败或目标离开了画面时，也即目标从检测框中消失了，就应把目标对应的跟踪框(预测框)从跟踪器链中删除。
      unmatched_trackers列表中保存的正是跟踪失败即离开画面的目标，但该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)
      此时仍然存在于跟踪器链(列表)中，因此就需要把该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)从跟踪器链(列表)中删除出去。
    2.同时如果因为“跟踪框和检测框之间的”两两组合的匹配度IOU值小于iou阈值，
      则也要把跟踪目标框放到unmatched_trackers中。
"""


# 目标估计模型-卡尔曼滤波
class KalmanBoxTracker(object):
    """
    每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
    KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
    并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。
    """
    count = 0  # 类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象

    """
    __init__(bbox)
        使用目标框bbox为卡尔曼滤波的状态进行初始化。初始化时传入bbox，即根据观测到的检测框的结果来进行初始化。
        每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
        KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
        并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。

        1.kf = KalmanFilter(dim_x=7, dim_z=4)
                定义一个卡尔曼滤波器，利用这个卡尔曼滤波器对目标的状态进行估计。
                dim_x=7定义是一个7维的状态更新向量x(状态变量x)：x=[u,v,s,r,u^,v^,s^]T。
                dim_z=4定义是一个4维的观测输入，即中心面积的形式[x,y,s,r]，即[检测框中心位置的x坐标,y坐标,面积,宽高比]。
        2.kf.F = np.array(7*7的方阵)
                状态转移矩阵F，定义的是一个7*7的方阵其(对角线上的值都是1)。
                通过7*7的状态转移矩阵F 乘以 7*1的状态更新向量x(状态变量x)即可得到一个更新后的7*1的状态更新向量x，
                其中更新后的u、v、s即为当前帧结果。
                通过状态转移矩阵对当前的观测结果进行估计获得预测的结果，然后用当前的预测的结果来作为下一次估计预测的基础。
        3.kf.H = np.array(4*7的矩阵)
                量测矩阵H(观测矩阵H)，定义的是一个4*7的矩阵。
                通过4*7的量测矩阵H(观测矩阵H) 乘以 7*1的状态更新向量x(状态变量x) 即可得到一个 4*1的[u,v,s,r]的估计值。
        4.相应的协方差参数的设定，根据经验值进行设定。
                1.R是测量噪声的协方差矩阵，即真实值与测量值差的协方差。
                  R=diag([1,1,10,10]T)
                        kf.R[2:, 2:] *= 10.
                2.P是先验估计的协方差矩阵
                  diag([10,10,10,10,1e4,1e4,1e4]T)。1e4：1x10的4次方。
                        kf.P[4:, 4:] *= 1000.  # 设置了一个较大的值，给无法观测的初始速度带来很大的不确定性
                        kf.P *= 10.
                3.Q是过程激励噪声的协方差矩阵
                  diag([1,1,1,1,0.01,0.01,1e-4]T)。1e-4：1x10的-4次方。
                        kf.Q[-1, -1] *= 0.01
                        kf.Q[4:, 4:] *= 0.01
        5.kf.x[:4] = convert_bbox_to_z(bbox)
                convert_bbox_to_z负责将[x1,y1,x2,y2]形式的检测框bbox转为中心面积的形式[x,y,s,r]。
                状态更新向量x(状态变量x)设定是一个七维向量：x=[u,v,s,r,u^,v^,s^]T。
                x[:4]即表示 u、v、s、r初始化为第一帧bbox观测到的结果[x,y,s,r]。
        6.单个目标框对应的单个卡尔曼滤波器中的统计参数的更新
            每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
            KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
            并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。
            1.卡尔曼滤波器的个数
                有多少个目标框就有多少个卡尔曼滤波器，每个目标框都会有一个卡尔曼滤波器，即每个目标框都会有一个KalmanBoxTracker实例对象。
                count = 0：类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象。
                id = KalmanBoxTracker.count：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个.
                KalmanBoxTracker.count += 1：每增加一个目标框，即增加一个KalmanBoxTracker实例对象(卡尔曼滤波器)，那么类属性count+=1。

            2.统计一个目标框对应的卡尔曼滤波器中各参数统计的次数
                1.age = 0：
                    该目标框进行预测的总次数。每执行predict一次，便age+=1。
                2.hits = 0：
                    该目标框进行更新的总次数。每执行update一次，便hits+=1。
                3.time_since_update = 0
                    1.连续预测的次数，每执行predict一次即进行time_since_update+=1。
                    2.在连续预测(连续执行predict)的过程中，一旦执行update的话，time_since_update就会被重置为0。
                    3.在连续预测(连续执行predict)的过程中，只要连续预测的次数time_since_update大于0的话，
                      就会把hit_streak(连续更新的次数)重置为0，表示连续预测的过程中没有出现过一次更新状态更新向量x(状态变量x)的操作，
                      即连续预测的过程中没有执行过一次update。
                4.hit_streak = 0
                    1.连续更新的次数，每执行update一次即进行hit_streak+=1。
                    2.在连续更新(连续执行update)的过程中，一旦开始连续执行predict两次或以上的情况下，
                      当连续第一次执行predict时，因为time_since_update仍然为0，并不会把hit_streak重置为0，
                      然后才会进行time_since_update+=1；
                      当连续第二次执行predict时，因为time_since_update已经为1，那么便会把hit_streak重置为0，
                      然后继续进行time_since_update+=1。
        7.history = []：
                保存单个目标框连续预测的多个结果到history列表中，一旦执行update就会清空history列表。
                将预测的候选框从中心面积的形式[x,y,s,r]转换为坐标的形式[x1,y1,x2,y2] 的bbox 再保存到 history列表中。
    """

    def __init__(self, bbox):
        # 定义等速模型
        # 内部使用KalmanFilter，7个状态变量和4个观测输入
        self.kf = KalmanFilter(dim_x=7, dim_z=4)
        # F是状态变换模型，为7*7的方阵
        self.kf.F = np.array([[1, 0, 0, 0, 1, 0, 0],
                              [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]])
        # H是量测矩阵，是4*7的矩阵
        self.kf.H = np.array([[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, 0, 0, 0]])
        # R是测量噪声的协方差，即真实值与测量值差的协方差
        self.kf.R[2:, 2:] *= 10.
        # P是先验估计的协方差
        self.kf.P[4:, 4:] *= 1000.  # 给无法观测的初始速度带来很大的不确定性
        self.kf.P *= 10.
        # Q是过程激励噪声的协方差
        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 = []
        self.hits = 0
        self.hit_streak = 0
        self.age = 0

    """
    update(bbox)：使用观测到的目标框bbox更新状态更新向量x(状态变量x)

    1.time_since_update = 0
            1.连续预测的次数，每执行predict一次即进行time_since_update+=1。
            2.在连续预测(连续执行predict)的过程中，一旦执行update的话，time_since_update就会被重置为0。
            2.在连续预测(连续执行predict)的过程中，只要连续预测的次数time_since_update大于0的话，
              就会把hit_streak(连续更新的次数)重置为0，表示连续预测的过程中没有出现过一次更新状态更新向量x(状态变量x)的操作，
              即连续预测的过程中没有执行过一次update。
    2.history = []      
           清空history列表。
           history列表保存的是单个目标框连续预测的多个结果([x,y,s,r]转换后的[x1,y1,x2,y2])，一旦执行update就会清空history列表。
    3.hits += 1：
            该目标框进行更新的总次数。每执行update一次，便hits+=1。
    4.hit_streak += 1
            1.连续更新的次数，每执行update一次即进行hit_streak+=1。
            2.在连续更新(连续执行update)的过程中，一旦开始连续执行predict两次或以上的情况下，
              当连续第一次执行predict时，因为time_since_update仍然为0，并不会把hit_streak重置为0，
              然后才会进行time_since_update+=1；
              当连续第二次执行predict时，因为time_since_update已经为1，那么便会把hit_streak重置为0，
              然后继续进行time_since_update+=1。
    5.kf.update(convert_bbox_to_z(bbox))
            convert_bbox_to_z负责将[x1,y1,x2,y2]形式的检测框转为滤波器的状态表示形式[x,y,s,r]，那么传入的为kf.update([x,y,s,r])。
            然后根据观测结果修改内部状态x(状态更新向量x)。
            使用的是通过yoloV3得到的“并且和预测框相匹配的”检测框来更新卡尔曼滤波器得到的预测框。
    """

    # 更新状态变量，使用观测到的目标框bbox更新状态变量
    def update(self, bbox):
        """
        使用观察到的目标框更新状态向量。filterpy.kalman.KalmanFilter.update 会根据观测修改内部状态估计self.kf.x。
        重置self.time_since_update，清空self.history。
        :param bbox:目标框
        :return:
        """
        # 重置
        self.time_since_update = 0
        # 清空history
        self.history = []
        # hits计数加1
        self.hits += 1
        self.hit_streak += 1
        # 根据观测结果修改内部状态x。
        # convert_bbox_to_z负责将[x1,y1,x2,y2]形式的检测框转为滤波器的状态表示形式[x,y,s,r]，那么update传入的为(x,y,s,r)
        self.kf.update(convert_bbox_to_z(bbox))

    """
    predict：进行目标框的预测并返回预测的边界框结果

    1.if(kf.x[6] + kf.x[2]) <= 0:
            self.kf.x[6] *= 0.0
            状态更新向量x(状态变量x)为[u,v,s,r,u^,v^,s^]T，那么x[6]为s^，x[2]为s。
            如果x[6]+x[2]<= 0，那么x[6] *= 0.0，即把s^置为0.0。
    2.kf.predict()
            进行目标框的预测。
    3.age += 1
            该目标框进行预测的总次数。每执行predict一次，便age+=1。
    4.if time_since_update > 0:
        hit_streak = 0
      time_since_update += 1
            1.连续预测的次数，每执行predict一次即进行time_since_update+=1。
            2.在连续预测(连续执行predict)的过程中，一旦执行update的话，time_since_update就会被重置为0。
            3.在连续预测(连续执行predict)的过程中，只要连续预测的次数time_since_update大于0的话，
              就会把hit_streak(连续更新的次数)重置为0，表示连续预测的过程中没有出现过一次更新状态更新向量x(状态变量x)的操作，
              即连续预测的过程中没有执行过一次update。
            4.在连续更新(连续执行update)的过程中，一旦开始连续执行predict两次或以上的情况下，
              当连续第一次执行predict时，因为time_since_update仍然为0，并不会把hit_streak重置为0，
              然后才会进行time_since_update+=1；
              当连续第二次执行predict时，因为time_since_update已经为1，那么便会把hit_streak重置为0，
              然后继续进行time_since_update+=1。
    5.history.append(convert_x_to_bbox(kf.x))
            convert_x_to_bbox(kf.x)：将目标框所预测的结果从中心面积的形式[x,y,s,r] 转换为 坐标的形式[x1,y1,x2,y2] 的bbox。
            history列表保存的是单个目标框连续预测的多个结果([x,y,s,r]转换后的[x1,y1,x2,y2])，一旦执行update就会清空history列表。
    6.predict 返回值：history[-1]
            把目标框当前该次的预测的结果([x,y,s,r]转换后的[x1,y1,x2,y2])进行返回输出。
    """

    # 进行目标框的预测，推进状态变量并返回预测的边界框结果
    def predict(self):
        """
        推进状态向量并返回预测的边界框估计。
        将预测结果追加到self.history。由于 get_state 直接访问 self.kf.x，所以self.history没有用到
        :return:
        """
        # 推进状态变量
        if (self.kf.x[6] + self.kf.x[2]) <= 0:
            self.kf.x[6] *= 0.0
        # 进行预测
        self.kf.predict()
        # 卡尔曼滤波的次数
        self.age += 1
        # 若过程中未更新过，将hit_streak置为0
        if self.time_since_update > 0:
            self.hit_streak = 0
        self.time_since_update += 1
        # 将预测结果追加到history中
        self.history.append(convert_x_to_bbox(self.kf.x))
        return self.history[-1]

    """
    get_state()：
        获取当前目标框预测的结果([x,y,s,r]转换后的[x1,y1,x2,y2])。
        return convert_x_to_bbox(kf.x)：将候选框从中心面积的形式[x,y,s,r] 转换为 坐标的形式[x1,y1,x2,y2] 的bbox并进行返回输出。
        直接访问 kf.x并进行返回，所以history没有用到。
    """
    def get_state(self):
        """
        返回当前边界框估计值。
        由于 get_state 直接访问 self.kf.x，所以self.history没有用到。
        :return:
        """
        # 将候选框从中心面积的形式[x,y,s,r] 转换为 坐标的形式[x1,y1,x2,y2] 的bbox
        return convert_x_to_bbox(self.kf.x)

@jit
def iou(bb_test, bb_gt):
    """
    在两个box间计算IOU
    :param bb_test: box1 = [x1y1x2y2] 即 [左上角的x坐标，左上角的y坐标，右下角的x坐标，右下角的y坐标]
    :param bb_gt: box2 = [x1y1x2y2]
    :return: 交并比IOU
    """
    xx1 = np.maximum(bb_test[0], bb_gt[0]) #获取交集面积四边形的 左上角的x坐标
    yy1 = np.maximum(bb_test[1], bb_gt[1]) #获取交集面积四边形的 左上角的y坐标
    xx2 = np.minimum(bb_test[2], bb_gt[2]) #获取交集面积四边形的 右下角的x坐标
    yy2 = np.minimum(bb_test[3], bb_gt[3]) #获取交集面积四边形的 右下角的y坐标
    w = np.maximum(0., xx2 - xx1) #交集面积四边形的 右下角的x坐标 - 左上角的x坐标 = 交集面积四边形的宽
    h = np.maximum(0., yy2 - yy1) #交集面积四边形的 右下角的y坐标 - 左上角的y坐标 = 交集面积四边形的高
    wh = w * h #交集面积四边形的宽 * 交集面积四边形的高 = 交集面积
    """
    两者的交集面积，作为分子。
    两者的并集面积作为分母。
    一方box框的面积：(bb_test[2] - bb_test[0]) * (bb_test[3] - bb_test[1])
    另外一方box框的面积：(bb_gt[2] - bb_gt[0]) * (bb_gt[3] - bb_gt[1])
    """
    o = wh / ( (bb_test[2] - bb_test[0]) * (bb_test[3] - bb_test[1])
               + (bb_gt[2] - bb_gt[0]) * (bb_gt[3] - bb_gt[1])
               - wh)
    return o

"""
利用匈牙利算法对跟踪目标框和yoloV3检测结果框进行关联匹配，整个流程是遍历检测结果框和跟踪目标框，并进行两两的相似度最大的比对。
相似度最大的认为是同一个目标则匹配成功的将其保留，相似度低的未成功匹配的将其删除。
使用的是通过yoloV3得到的“并且和预测框相匹配的”检测框来更新卡尔曼滤波器得到的预测框。
    detections：此处传入的检测框的位置预测值为“已经把yoloV3得到的检测框的位置预测值”转换成了的[x1,y1,x2,y2,score]。
                x1、y1 代表检测框的左上角坐标；x2、y2代表检测框的右上角坐标；score代表检测框对应预测类别的概率值。
    trackers：通过卡尔曼滤波器得到的预测结果跟踪目标框
    iou_threshold=0.3：大于IOU阈值则认为是同一个目标则匹配成功将其保留，小于IOU阈值则认为不是同一个目标则未成功匹配将其删除。
    return返回值：
        matches：跟踪成功目标的矩阵。即前后帧都存在的目标，并且匹配成功同时大于iou阈值。
        np.array(unmatched_detections)：新增目标指的就是存在于detections检测结果框当中，但不存在于trackers预测结果跟踪目标框当中。
        np.array(unmatched_trackers)：离开画面的目标指的就是存在于trackers预测结果跟踪目标框当中，但不存在于detections检测结果框当中。
 
    matches：
        [[检测框的索引值, 跟踪框的索引值] [检测框的索引值, 跟踪框的索引值] 。。。]
        跟踪成功并且两两匹配组合的IOU值大于iou阈值的检测框和跟踪框组成的矩阵
    unmatched_detections：
        [检测框的索引值,。。。]
        1.新增目标的检测框在detections检测框列表中的索引位置
        2.两两匹配组合的IOU值小于iou阈值的检测框在detections检测框列表中的索引位置
    unmatched_trackers：
        [跟踪框的索引值,。。。]
        1.跟踪失败的跟踪框/预测框在trackers跟踪框列表中的索引位置
        2.两两匹配组合的IOU值小于iou阈值的跟踪框/预测框在trackers跟踪框列表中的索引位置
"""
def associate_detections_to_trackers(detections, trackers, iou_threshold=0.3):
    """
    将检测框bbox与卡尔曼滤波器的跟踪框进行关联匹配
    :param detections:通过yoloV3得到的检测结果框
    :param trackers:通过卡尔曼滤波器得到的预测结果跟踪目标框
    :param iou_threshold:大于IOU阈值则认为是同一个目标则匹配成功将其保留，小于IOU阈值则认为不是同一个目标则未成功匹配将其删除。
    :return:跟踪成功目标的矩阵：matchs。即前后帧都存在的目标，并且匹配成功同时大于iou阈值。
            新增目标的矩阵：unmatched_detections。
                            新增目标指的就是存在于detections检测结果框当中，但不存在于trackers预测结果跟踪目标框当中。
            跟踪失败即离开画面的目标矩阵：unmatched_trackers。
                            离开画面的目标指的就是存在于trackers预测结果跟踪目标框当中，但不存在于detections检测结果框当中。
    """
    """
    1.跟踪器链(列表)：
        实际就是多个的卡尔曼滤波KalmanBoxTracker自定义类的实例对象组成的列表。
        每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
        KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
        并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。
        把每个卡尔曼滤波器(KalmanBoxTracker实例对象)都存储到跟踪器链(列表)中。
    2.unmatched_detections(列表)：
        1.检测框中出现新目标，但此时预测框(跟踪框)中仍不不存在该目标，
          那么就需要在创建新目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)，
          然后把新目标对应的KalmanBoxTracker类的实例对象放到跟踪器链(列表)中。
        2.同时如果因为“跟踪框和检测框之间的”两两组合的匹配度IOU值小于iou阈值，
          则也要把目标检测框放到unmatched_detections中。
    3.unmatched_trackers(列表)：
        1.当跟踪目标失败或目标离开了画面时，也即目标从检测框中消失了，就应把目标对应的跟踪框(预测框)从跟踪器链中删除。
          unmatched_trackers列表中保存的正是跟踪失败即离开画面的目标，但该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)
          此时仍然存在于跟踪器链(列表)中，因此就需要把该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)从跟踪器链(列表)中删除出去。
        2.同时如果因为“跟踪框和检测框之间的”两两组合的匹配度IOU值小于iou阈值，
          则也要把跟踪目标框放到unmatched_trackers中。
    """
    # 跟踪目标数量为0，直接构造结果
    if (len(trackers) == 0) or (len(detections) == 0):
        """
        如果卡尔曼滤波器得到的预测结果跟踪目标框len(trackers)为0 或者 yoloV3得到的检测结果框len(detections)为0 的话，
        跟踪成功目标的矩阵：matchs 为 np.empty((0, 2), dtype=int)
        新增目标的矩阵：unmatched_detections 为 np.arange(len(detections))
        跟踪失败即离开画面的目标矩阵：unmatched_trackers 为 np.empty((0, 5), dtype=int)
        """
        return np.empty((0, 2), dtype=int), np.arange(len(detections)), np.empty((0, 5), dtype=int)

    """ 因为要计算所有检测结果框中每个框 和 所有跟踪目标框中每个框 两两之间 的iou相似度计算，
        即所有检测结果框中每个框 都要和 所有跟踪目标框中每个框 进行两两之间 的iou相似度计算，
        所以iou_matrix需要初始化为len(detections检测结果框) * len(trackers跟踪目标框) 形状的0初始化的矩阵。
    """
    # iou 不支持数组计算。逐个计算两两间的交并比，调用 linear_assignment 进行匹配
    iou_matrix = np.zeros((len(detections), len(trackers)), dtype=np.float32)
    # 遍历目标检测（yoloV3检测）的bbox集合，每个检测框的标识为d，det为检测结果框
    for d, det in enumerate(detections):
        # 遍历跟踪框（卡尔曼滤波器预测）bbox集合，每个跟踪框标识为t，trackers为跟踪目标框
        for t, trk in enumerate(trackers):
            """
            遍历每个检测结果框 和 遍历每个跟踪目标框 进行两两之间 的iou相似度计算。
            行索引值对应的是目标检测框。列索引值对应的是跟踪目标框。
            """
            iou_matrix[d, t] = iou(det, trk)

    """
    row_ind, col_ind=linear_sum_assignment(-iou_matrix矩阵)
        通过匈牙利算法得到最优匹配度的“跟踪框和检测框之间的”两两组合。
        通过相同下标位置的行索引和列索引即可从iou_matrix矩阵得到“跟踪框和检测框之间的”两两组合最优匹配度的IOU值。
        -iou_matrix矩阵：linear_assignment的输入是cost成本矩阵，IOU越大对应的分配代价应越小，所以iou_matrix矩阵需要取负号。
        row_ind：行索引构建的一维数组。行索引值对应的是目标检测框。
        col_ind：列索引构建的一维数组。列索引值对应的是跟踪目标框。
        比如：
            row_ind：[0 1 2 3]。col_ind列索引：[3 2 1 0]。
            np.array(list(zip(*result)))：[[0 3] [1 2] [2 1] [3 0]]
    """
    # 通过匈牙利算法将跟踪框和检测框以[[d,t]...]的二维矩阵的形式存储在match_indices中
    result = linear_sum_assignment(-iou_matrix)
    matched_indices = np.array(list(zip(*result)))

    """ np.array(unmatched_detections)：新增目标指的就是存在于detections检测结果框当中，但不存在于trackers预测结果跟踪目标框当中 """
    # 记录未匹配的检测框及跟踪框
    # 未匹配的检测框放入unmatched_detections中，表示有新的目标进入画面，要新增跟踪器跟踪目标
    unmatched_detections = []
    for d, det in enumerate(detections):
        """ matched_indices[:, 0]：取出的是每行的第一列，代表的是目标检测框。
           如果目标检测框的索引d不存在于匹配成功的matched_indices中每行的第一列的话，代表目标检测框中有新的目标出现在画面中，
           则把未匹配的目标检测框放入到unmatched_detections中表示需要新增跟踪器进行跟踪目标。
        """
        if d not in matched_indices[:, 0]:
            """ 新增目标的检测框在detections检测框列表中的索引位置 """
            unmatched_detections.append(d)

    """ np.array(unmatched_trackers)：离开画面的目标指的就是存在于trackers预测结果跟踪目标框当中，但不存在于detections检测结果框当中 """
    # 未匹配的跟踪框放入unmatched_trackers中，表示目标离开之前的画面，应删除对应的跟踪器
    unmatched_trackers = []
    for t, trk in enumerate(trackers):
        """ matched_indices[:, 1]：取出的是每行的第二列，代表的是跟踪目标框。
           如果跟踪目标框的索引t不存在于匹配成功的matched_indices中每行的第二列的话，代表跟踪目标框中有目标离开了画面，
           则把未匹配的跟踪目标框放入到unmatched_trackers中表示需要删除对应的跟踪器。
        """
        if t not in matched_indices[:, 1]:
            """ 跟踪失败的跟踪框/预测框在trackers跟踪框列表中的索引位置 """
            unmatched_trackers.append(t)

    """ matches：跟踪成功目标的矩阵。即前后帧都存在的目标，并且匹配成功同时大于iou阈值。
        即把匹配成功的matched_indices中的并且小于iou阈值的[d,t]放到matches中。
    """
    # 将匹配成功的跟踪框放入matches中
    matches = []
    for m in matched_indices:
        """
        m[0]：每行的第一列，代表的是目标检测框。m[1]：每行的第二列，代表的是跟踪目标框。
        iou_matrix[m[0], m[1]] < iou_threshold：
            根据目标检测框的索引作为行索引，跟踪目标框的索引作为列索引，
            即能找到“跟踪框和检测框之间的”两两组合最优匹配度的IOU值，如果该IOU值小于iou阈值的话，
            则把目标检测框放到unmatched_detections中，把跟踪目标框放到unmatched_trackers中。
        """
        # 过滤掉IOU低的匹配，将其放入到unmatched_detections和unmatched_trackers
        if iou_matrix[m[0], m[1]] < iou_threshold:
            """ 两两匹配组合的IOU值小于iou阈值的检测框在detections检测框列表中的索引位置 """
            unmatched_detections.append(m[0]) #m[0]：每行的第一列，代表的是目标检测框。
            """ 两两匹配组合的IOU值小于iou阈值的跟踪框/预测框在trackers跟踪框列表中的索引位置 """
            unmatched_trackers.append(m[1])   #m[1]：每行的第二列，代表的是跟踪目标框。
        # 满足条件的以[[d,t]...]的形式放入matches中
        else:
            """ 存储到列表中的每个元素的形状为(1, 2) """
            matches.append(m.reshape(1, 2))

    """
    如果矩阵matches中不存在任何跟踪成功的目标的话，则创建空数组返回。
    numpy.concatenate((a1,a2,...), axis=0)：能够一次完成多个数组a1,a2,...的拼接。
    >>> a=np.array([1,2,3])
    >>> b=np.array([11,22,33])
    >>> c=np.array([44,55,66])
    >>> np.concatenate((a,b,c),axis=0)  # 默认情况下，axis=0可以不写
    array([ 1,  2,  3, 11, 22, 33, 44, 55, 66]) #对于一维数组拼接，axis的值不影响最后的结果
    """
    # 初始化matches,以np.array的形式返回
    if len(matches) == 0:
        """
        np.empty((0, 2), dtype=int)
            输出值：array([], shape=(0, 2), dtype=int32)
            输出值类型：<class 'numpy.ndarray'>
        """
        matches = np.empty((0, 2), dtype=int)
    else:
        """
        np.concatenate(matches, axis=0)：
            [array([[0, 0]], dtype=int64), array([[1, 1]], dtype=int64),  。。。] 转换为 [[0, 0] [1, 1] 。。。]
        """
        matches = np.concatenate(matches, axis=0) # 默认情况下，axis=0可以不写

    """
    matches：
        [[检测框的索引值, 跟踪框的索引值] [检测框的索引值, 跟踪框的索引值] 。。。]
        跟踪成功并且两两匹配组合的IOU值大于iou阈值的检测框和跟踪框组成的矩阵
    unmatched_detections：
        [检测框的索引值,。。。]
        1.新增目标的检测框在detections检测框列表中的索引位置
        2.两两匹配组合的IOU值小于iou阈值的检测框在detections检测框列表中的索引位置
    unmatched_trackers：
        [跟踪框的索引值,。。。]
        1.跟踪失败的跟踪框/预测框在trackers跟踪框列表中的索引位置
        2.两两匹配组合的IOU值小于iou阈值的跟踪框/预测框在trackers跟踪框列表中的索引位置
    """
    return matches, np.array(unmatched_detections), np.array(unmatched_trackers)

"""
利用sort算法完成多目标追踪
    在这里我们主要实现了一个多目标跟踪器，管理多个卡尔曼滤波器对象，主要包括以下内容：
        1.初始化：最大检测数，目标未被检测的最大帧数
        2.目标跟踪结果的更新，即跟踪成功和失败的目标的更新
            该方法实现了SORT算法，输入是当前帧中所有物体的检测框的集合，包括目标的score，输出是当前帧的跟踪框集合，
            包括目标的跟踪的id要求是即使检测框为空，也必须对每一帧调用此方法，返回一个类似的输出数组，
            最后一列是目标对像的id。需要注意的是，返回的目标对象数量可能与检测框的数量不同。
"""

# 1.SORT目标跟踪：
#       1.第一帧刚开始时：对第一帧所有的检测框生成对应的新跟踪框。
#       2.第二帧开始到以后所有帧：
#           上一帧成功跟踪并且保留下来的的跟踪框 在当前帧中 进行新一轮的预测新的跟踪框，
#           并且针对所预测的新跟踪框和当前帧中的检测框进行iou计算和使用匈牙利算法对该两者进行关联匹配，
#           通过上述操作后成功返回跟踪目标成功的跟踪框(即和当前帧中的目标检测框相匹配的跟踪框)，
#           并且另外发现了新出现目标的检测框、跟踪目标失败的跟踪框(即目标离开了画面/两者匹配度IOU值小于iou阈值)，
#           那么首先使用当前帧中的检测框对“成功关联匹配的跟踪框中的”状态向量进行更新，
#           然后对新增目标的检测框生成对应新的跟踪框，最后把跟踪目标失败的跟踪框从跟踪器链列表中移除出去。
# 2.传入的检测框dets：[检测框的左上角的x/y坐标, 检测框的右下角的x/y坐标, 检测框的预测类别的概率值]
# 3.返回值tracks：
#   当前帧中跟踪目标成功的跟踪框/预测框的集合，包含目标的跟踪的id(也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个)
#   第一种返回值方案：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度, trk.id] ...]
#   第二种返回值方案(当前使用的为该种)：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, trk.id] ...]
#   d：[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]
#   trk.id：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个。
class Sort(object):
    """
    Sort 是一个多目标跟踪器的管理类，管理多个 跟踪器链中的多个 KalmanBoxTracker 卡尔曼滤波对象
    """
    def __init__(self, max_age=1, min_hits=3):
        """
        初始化：设置SORT算法的关键参数
        :param max_age: 最大检测数：目标未被检测到的帧数，超过之后会被删除
        :param min_hits: 目标命中的最小次数，小于该次数update函数不返回该目标的KalmanBoxTracker卡尔曼滤波对象
        """
        """
        max_age：跟踪框的最大连续跟丢帧数。如果当前跟踪框连续N帧大于最大连续跟丢帧数的话，则从跟踪器链中删除该卡尔曼滤波对象的预测框(跟踪框)。
        min_hits：跟踪框连续成功跟踪到目标的最小次数(目标连续命中的最小次数)，也即跟踪框至少需要连续min_hits次成功跟踪到目标。
        trackers：卡尔曼滤波跟踪器链，存储多个 KalmanBoxTracker 卡尔曼滤波对象
        frame_count：当前视频经过了多少帧的计数
        """
        self.max_age = max_age # 最大检测数：目标未被检测到的帧数，超过之后会被删
        self.min_hits = min_hits # 目标连续命中的最小次数，小于该次数update函数不返回该目标的KalmanBoxTracker卡尔曼滤波对象
        self.trackers = []   # 卡尔曼滤波跟踪器链，存储多个 KalmanBoxTracker 卡尔曼滤波对象
        self.frame_count = 0  # 帧计数

    """
    update(dets)：
        输入dets：
            当前帧中yolo所检测出的所有目标的检测框的集合，包含每个目标的score
            以[[x1,y1,x2,y2,score]，[x1,y1,x2,y2,score]，...]形式输入的numpy.array
            x1、y1 代表检测框的左上角坐标；x2、y2代表检测框的右上角坐标；score代表检测框对应预测类别的概率值。
        输出ret：
            当前帧中跟踪目标成功的跟踪框/预测框的集合，包含目标的跟踪的id(也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个)
            第一种返回值方案：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度, trk.id] ...]
            第二种返回值方案(当前使用的为该种)：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, trk.id] ...]
            d：[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]
            trk.id：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个。
        注意：
            即使检测框为空，也必须对每一帧调用此方法，返回一个类似的输出数组，最后一列是目标对像的id。
            返回的目标对象数量可能与检测框的数量不同。
    """
    def update(self, dets):
        """
        该方法实现了SORT算法，输入是当前帧中所有物体的检测框的集合，包括目标的score,
        输出是当前帧目标的跟踪框集合，包括目标的跟踪的id
        要求是即使检测框为空，也必须对每一帧调用此方法，返回一个类似的输出数组，最后一列是目标对像的id
        注意：返回的目标对象数量可能与检测框的数量不同
        :param dets:以[[x1,y1,x2,y2,score]，[x1,y1,x2,y2,score]，...]形式输入的numpy.array
        :return:
        """
        """ 每经过一帧，frame_count+=1"""
        self.frame_count += 1

        """
        1.trackers：
            上一帧中的跟踪器链(列表)，保存的是上一帧中成功跟踪目标的跟踪框，也即上一帧中成功跟踪目标的KalmanBoxTracker卡尔曼滤波对象。
           
        2.trks = np.zeros((len(trackers), 5))
            上一帧中的跟踪器链(列表)中的所有跟踪框(卡尔曼滤波对象)在当前帧中成功进行predict预测新跟踪框后返回的值。
            所有新跟踪框的左上角的x坐标和y坐标、右下角的x坐标和y坐标、置信度 的一共5个值。
           
            1.因为一开始第一帧时，trackers跟踪器链(列表)仍然为空，所以此时的trks初始化如下：
                np.zeros((0, 5))    
                    输出值：array([], shape=(0, 5), dtype=float64)
                    输出值类型：<class 'numpy.ndarray'>
            2.np.zeros((len(trackers), 5)) 创建目的：
                1.用于存储上一帧中的跟踪器链中所有跟踪框(KalmanBoxTracker卡尔曼滤波对象)在当前帧中进行predict预测新跟踪框后返回的值，
                  之所以创建的numpy数组的列数为5，是因为一个跟踪框在当前帧中进行predict预测新跟踪框后返回的值为1行5列的矩阵，
                  返回值分别为新跟踪框的左上角的x坐标和y坐标、右下角的x坐标和y坐标、置信度 的一共5个值。
                2.如果是在视频的第一帧中，那么因为跟踪器链不存在任何跟踪框(KalmanBoxTracker卡尔曼滤波对象)，
                  因此np.zeros((len(trackers), 5))创建的是空列表：array([], shape=(0, 5), dtype=float64)。
            3.trackers：跟踪器链(列表)
                1.跟踪器链中存储了上一帧中成功跟踪目标并且在当前帧中的预测框(跟踪框)，
                  同时也存储了“为了当前帧中的检测框中的新增目标所创建的”新预测框(新跟踪框)，
                  但是同时不存储当前帧中预测跟踪失败的预测框(跟踪框)，同时也不存储
                2.跟踪器链实际就是多个的卡尔曼滤波KalmanBoxTracker自定义类的实例对象组成的列表。
                  每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，
                  KalmanBoxTracker类中的实例属性专门负责记录其对应的一个目标框中各种统计参数，
                  并且使用类属性负责记录卡尔曼滤波器的创建个数，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。
                  把每个卡尔曼滤波器(KalmanBoxTracker实例对象)都存储到跟踪器链(列表)中。
        """
        # 在当前帧逐个预测轨迹位置，记录状态异常的跟踪器索引
        # 根据当前所有的卡尔曼跟踪器个数（即上一帧中跟踪的目标个数）创建二维数组：行号为卡尔曼滤波器的标识索引，列向量为跟踪框的位置和ID
        trks = np.zeros((len(self.trackers), 5))  # 存储跟踪器的预测
        """ to_del：存储“跟踪器链中某个要删除的”KalmanBoxTracker卡尔曼滤波对象的索引 """
        to_del = []   # 存储要删除的目标框

        ret = []    # 存储要返回的追踪目标框
        """
        for t, trk in enumerate(ndarray类型的trks)
            t：为从0到列表长度-1的索引值
            trk：ndarray类型的trks中每个(1, 5)形状的一维数组
        """

        """ 遍历trks 用于存储上一帧中的跟踪器链中所有跟踪框(KalmanBoxTracker卡尔曼滤波对象)在当前帧中进行predict预测新跟踪框后返回的值 """
        # 循环遍历卡尔曼跟踪器列表
        for t, trk in enumerate(trks):
            """ 上一帧中的跟踪器链中所有跟踪框(KalmanBoxTracker卡尔曼滤波对象)在当前帧中进行predict预测新跟踪框 """
            # 使用卡尔曼跟踪器t产生对应目标的跟踪框
            pos = self.trackers[t].predict()[0]
            """ 新跟踪框的左上角的x坐标和y坐标、右下角的x坐标和y坐标、置信度 的一共5个值。
               trk中存储了上一帧中目标的跟踪框在当前帧中新的跟踪框的信息值。
            """
            # 遍历完成后，trk中存储了上一帧中跟踪的目标的预测跟踪框
            trk[:] = [pos[0], pos[1], pos[2], pos[3], 0]
            """ 如果预测的新的跟踪框的信息(1行5列一共5个值)中包含空值的话，则将该跟踪框在跟踪器链(列表)中的索引值t放到to_del列表中。
               使用np.any(np.isnan(pos))即能判断这1行5列一共5个值是否包含空值。
               后面下一步将会根据to_del列表中保存的跟踪框的索引值到跟踪器链(列表)中将该跟踪框从其中移除出去。
            """
            # 如果跟踪框中包含空值则将该跟踪框添加到要删除的列表中
            if np.any(np.isnan(pos)):
                to_del.append(t)
        """
        np.ma.masked_invalid(跟踪器链trks矩阵)：
            将会对跟踪器链trks矩阵中出现了NaN或inf的某行进行生成掩码，用于屏蔽出现无效值该整行的跟踪器框。
        np.ma.compress_rows(包含掩码值的跟踪器链trks矩阵)：
            将包含掩码值的整行从中进行移除出去。
        最终跟踪器链trks矩阵：只包含“上一帧中的跟踪器链中所有跟踪框在当前帧中成功进行predict预测”的新跟踪框。
        """
        # numpy.ma.masked_invalid 屏蔽出现无效值的数组（NaN 或 inf）
        # numpy.ma.compress_rows 压缩包含掩码值的2-D 数组的整行，将包含掩码值的整行去除
        # trks中存储了上一帧中成功跟踪目标并且在当前帧中的预测框(跟踪框)
        trks = np.ma.compress_rows(np.ma.masked_invalid(trks))
        """
        1.for t in reversed(列表)：
            1.t：列表中的元素值
            2.要想从List列表中删除任意索引位置的元素的话，必须不能从列表头开始遍历删除元素，必须从列表尾向列表头的方向进行遍历删除元素，
              因为如果从列表头开始遍历删除元素的话，便会导致后面的元素会移动补充到被删除元素的索引位置上，
              那么再向后进行遍历时便会出现漏遍历的元素，也即防止破坏索引，因此删除列表中元素时需要从列表尾向列表头的方向进行遍历。
        2.for t in reversed(to_del)
            1.t：列表中的元素值
            2.此处to_del列表中的元素值保存的是trackers跟踪器链(列表)中要删除元素的索引值，
              因此从to_del列表的列表尾向列表头的方向进行遍历出“trackers跟踪器链(列表)中要删除元素的”索引值。
              然后使用trackers.pop(t)根据trackers跟踪器链(列表)中元素的索引值t自动从列表中移除该元素。
        3.List pop()方法
                1.pop()方法语法：list.pop([index=-1])
                2.pop()函数用于移除列表中的一个元素（默认最后一个元素），并且返回该元素的值。
                3.pop(可选参数)中参数：可选参数，要移除列表元素的索引值，不能超过列表总长度，默认为 index=-1，删除最后一个列表值。
                4.pop()返回值：该方法返回从列表中被移除的元素对象。
                5.pop(要移除的列表中元素的索引值)：根据列表中元素的索引值自动从列表中移除
        """
        # 逆向删除异常的跟踪器，防止破坏索引
        for t in reversed(to_del):
            """
            根据to_del列表中保存的跟踪框的索引值到跟踪器链(列表)中将该跟踪框从其中移除出去。
            trackers：上一帧中的跟踪器链(列表)，保存的是上一帧中成功跟踪目标的跟踪框，也即成功跟踪目标的KalmanBoxTracker卡尔曼滤波对象。
            trackers.pop(要移除的某个跟踪框的索引值)：即能根据该索引值从跟踪器链(列表)中把该跟踪框移除出去
            """
            #pop(要移除的列表中元素的索引值)：根据列表中元素的索引值自动从列表中移除
            self.trackers.pop(t)

        """
        matches：
            [[检测框的索引值, 跟踪框的索引值] [检测框的索引值, 跟踪框的索引值] 。。。]
            跟踪成功并且两两匹配组合的IOU值大于iou阈值的检测框和跟踪框组成的矩阵
        unmatched_detections：
            [检测框的索引值,。。。]
            1.新增目标的检测框在detections检测框列表中的索引位置
            2.两两匹配组合的IOU值小于iou阈值的检测框在detections检测框列表中的索引位置
        unmatched_trackers：
            [跟踪框的索引值,。。。]
            1.跟踪失败的跟踪框/预测框在trackers跟踪框列表中的索引位置
            2.两两匹配组合的IOU值小于iou阈值的跟踪框/预测框在trackers跟踪框列表中的索引位置
           
        1.matched：跟踪成功目标的矩阵。即前后帧都存在的目标，并且匹配成功同时大于iou阈值。
        2.unmatched_detections(列表)：
            1.检测框中出现新目标，但此时预测框(跟踪框)中仍不不存在该目标，
              那么就需要在创建新目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)，
              然后把新目标对应的KalmanBoxTracker类的实例对象放到跟踪器链(列表)中。
            2.同时如果因为“跟踪框和检测框之间的”两两组合的匹配度IOU值小于iou阈值，
              则也要把目标检测框放到unmatched_detections中。
        3.unmatched_trackers(列表)：
            1.当跟踪目标失败或目标离开了画面时，也即目标从检测框中消失了，就应把目标对应的跟踪框(预测框)从跟踪器链中删除。
              unmatched_trackers列表中保存的正是跟踪失败即离开画面的目标，但该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)
              此时仍然存在于跟踪器链(列表)中，因此就需要把该目标对应的预测框/跟踪框(KalmanBoxTracker类的实例对象)从跟踪器链(列表)中删除出去。
            2.同时如果因为“跟踪框和检测框之间的”两两组合的匹配度IOU值小于iou阈值，
              则也要把跟踪目标框放到unmatched_trackers中。
        """
        # 将目标检测框与卡尔曼滤波器预测的跟踪框关联获取跟踪成功的目标，新增的目标，离开画面的目标
        matched, unmatched_dets, unmatched_trks = associate_detections_to_trackers(dets, trks)

        """
        for t, trk in enumerate(trackers列表)
            t：为从0到列表长度-1的索引值
            trk：trackers列表中每个KalmanBoxTracker卡尔曼滤波对象
        """
        # 将跟踪成功的目标框更新到对应的卡尔曼滤波器
        for t, trk in enumerate(self.trackers):
            """
            1.trackers：上一帧中的跟踪器链(列表)，保存的是上一帧中成功跟踪目标的跟踪框，也即成功跟踪目标的KalmanBoxTracker卡尔曼滤波对象。
            2.for t, trk in enumerate(trackers)：
                遍历上一帧中的跟踪器链(列表)中从0到列表长度-1的索引值t 和 每个KalmanBoxTracker卡尔曼滤波对象trk。
            3.if t not in unmatched_trks：
                如果上一帧中的跟踪框(KalmanBoxTracker卡尔曼滤波对)的索引值不在当前帧中的unmatched_trackers(列表)中的话，
                即代表上一帧中的跟踪框在当前帧中成功跟踪到目标，
                并且代表了“上一帧中的跟踪框在当前帧中的”预测框和当前帧中的检测框的匹配度IOU值大于iou阈值。
            4.matched[:, 1]：获取的是跟踪框的索引值，即[[检测框的索引值, 跟踪框的索引值] 。。。]中的跟踪框的索引值。
            5.np.where(matched[:, 1] == t)[0]：
                where返回的为符合条件的“[检测框的索引值, 跟踪框的索引值]”数组在matched矩阵中的索引值，即行值。
                因此最后使用[0]就是从array([索引值/行值])中把索引值/行值取出来。
            6.matched[索引值/行值, 0]：
                根据索引值/行值获取出matched矩阵中的[检测框的索引值, 跟踪框的索引值]，然后获取出第一列的“检测框的索引值”。
            7.dets[d, :]：
                    根据检测框的索引值/行值从当前帧中的dets检测框列表获取出该检测框的所有列值，最终返回的是一个二维矩阵如下所示：
                    第一种方案：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度]]
                    第二种方案(当前使用的为该种)：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]]
            8.dets[d, :][0]：获取出[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]
            9.trk.update(检测框的5个值的列表)：使用检测框进行更新状态更新向量x(状态变量x)，也即使用检测框更新跟踪框。
            """
            if t not in unmatched_trks:
                d = matched[np.where(matched[:, 1] == t)[0], 0]
                # 使用观测的边界框更新状态向量
                trk.update(dets[d, :][0])

        """
        unmatched_detections(列表)
            保存了出现新目标的检测框的索引值，还保存了“因为跟踪框和检测框之间的两两组合的匹配度IOU值小于iou阈值的”目标检测框的索引值。
        dets[i, :]：
            根据索引值从当前帧中的检测框列表dets中获取对应的检测框，即该行的所有列值。
            该检测框的值为：
                第一种方案：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度]]
                第二种方案(当前使用的为该种)：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]]
        KalmanBoxTracker(dets[i, :])：
            传入检测框进行创建该新目标对应的跟踪框KalmanBoxTracker卡尔曼滤波对象trk。
            每个目标框都有对应的一个卡尔曼滤波器(KalmanBoxTracker实例对象)，增加一个目标框就增加一个卡尔曼滤波器(KalmanBoxTracker实例对象)。
        trackers.append(trk)：把新增的卡尔曼滤波器(KalmanBoxTracker实例对象trk)存储到跟踪器链(列表)trackers中
        """
        # 为新增的目标创建新的卡尔曼滤波器对象进行跟踪
        for i in unmatched_dets:
            trk = KalmanBoxTracker(dets[i, :])
            self.trackers.append(trk)

        # 自后向前遍历，仅返回在当前帧出现且命中周期大于self.min_hits（除非跟踪刚开始）的跟踪结果；如果未命中时间大于self.max_age则删除跟踪器。
        # hit_streak忽略目标初始的若干帧
        """ i为trackers跟踪器链(列表)长度，从列表尾向列表头的方向 每遍历trackers跟踪器链(列表)一次 即进行 i-=1 """
        i = len(self.trackers)

        """ reversed逆向遍历trackers跟踪器链(列表)，目的为删除列表中的元素的同时不会造成漏遍历元素的问题 """
        for trk in reversed(self.trackers):
            """
            (跟踪框)KalmanBoxTracker卡尔曼滤波对象trk.get_state()：
                获取跟踪框所预测的在当前帧中的预测结果（已经从[x,y,s,r]转换为[x1,y1,x2,y2]）
                [x1,y1,x2,y2]即为[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]。
           get_state()[0] 中使用[0] 是因为返回的为二维矩阵如下：  
                第一种方案：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度]]
                第二种方案(当前使用的为该种)：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]]
            """
            # 返回当前边界框的估计值
            d = trk.get_state()[0]
            """
            1.trk.time_since_update < 1：
                1.time_since_update：
                    记录了该目标对应的卡尔曼滤波器中的预测框(跟踪框)进行连续预测的次数，每执行predict一次即进行time_since_update+=1。
                    在连续预测(连续执行predict)的过程中，一旦执行update的话，time_since_update就会被重置为0。
                2. time_since_update < 1：
                    该目标对应的卡尔曼滤波器一旦update更新的话该变量值便重置为0，因此要求该目标对应的卡尔曼滤波器必须执行update更新步骤。
                    update更新代表了使用检测框来更新状态更新向量x(状态变量x)的操作，
                    实际即代表了使用“通过yoloV3得到的并且和预测框(跟踪框)相匹配的”检测框来更新该目标对应的卡尔曼滤波器中的预测框(跟踪框)。
           
            2.trk.hit_streak >= min_hits：
                    1.hit_streak
                        1.连续更新的次数，每执行update一次即进行hit_streak+=1。
                        2.在连续更新(连续执行update)的过程中，一旦开始连续执行predict两次或以上的情况下，
                          当连续第一次执行predict时，因为time_since_update仍然为0，并不会把hit_streak重置为0，
                          然后才会进行time_since_update+=1；
                          当连续第二次执行predict时，因为time_since_update已经为1，那么便会把hit_streak重置为0，
                          然后继续进行time_since_update+=1。    
                    2.min_hits
                        跟踪框连续成功跟踪到目标的最小次数，也即跟踪框至少需要连续min_hits次成功跟踪到目标。
                    3.hit_streak >= min_hits
                        跟踪框连续更新的次数hit_streak必须大于等于min_hits。
                        而小于该min_hits次数的话update函数不返回该目标的KalmanBoxTracker卡尔曼滤波对象。
                       
            3.frame_count <= min_hits：
                    因为视频的一开始frame_count为0，而需要每经过一帧frame_count才会+=1。
                    因此在视频的一开始前N帧中，即使frame_count 小于等于min_hits 也可以。
            """
            # 跟踪成功目标的box与id放入ret列表中
            if (trk.time_since_update < 1) and (trk.hit_streak >= self.min_hits or self.frame_count <= self.min_hits):
                """
                1.ret：
                    当前帧中跟踪目标成功的跟踪框/预测框的集合，包含目标的跟踪的id(也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个)
                    第一种返回值方案：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度, trk.id] ...]
                    第二种返回值方案(当前使用的为该种)：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, trk.id] ...]
                    d：[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]
                    trk.id：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个。
                2.np.concatenate((d, [trk.id + 1])).reshape(1, -1)
                    [[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, 该跟踪框是创建出来的第几个]]
                """
                ret.append(np.concatenate((d, [trk.id + 1])).reshape(1, -1))  # +1 as MOT benchmark requires positive

            """ i为trackers跟踪器链(列表)长度，从列表尾向列表头的方向 每遍历trackers跟踪器链(列表)一次 即进行 i-=1 """
            i -= 1

            """
            trk.time_since_update > max_age
                1.time_since_update：
                    记录了该目标对应的卡尔曼滤波器中的预测框(跟踪框)进行连续预测的次数，每执行predict一次即进行time_since_update+=1。
                    在连续预测(连续执行predict)的过程中，一旦执行update的话，time_since_update就会被重置为0。
                2.max_age：
                    最大跟丢帧数。如果当前连续N帧大于最大跟丢帧数的话，则从跟踪器链中删除该卡尔曼滤波对象的预测框(跟踪框)。
                3.time_since_update > max_age：
                    每预测一帧time_since_update就会+=1，只有预测的跟踪框跟踪到目标(即预测的跟踪框和检测框相似度匹配)才会执行update更新，
                    那么time_since_update才会被重置为0。
                    那么当连续time_since_update帧都没有跟踪到目标的话，即当连续time_since_update帧大于最大跟丢帧数时，
                    那么就需要根据该跟踪失败的跟踪器框的索引把该跟踪器框从跟踪器链(列表)trackers中进行移除出去。
            """
            # 跟踪失败或离开画面的目标从卡尔曼跟踪器中删除
            if trk.time_since_update > self.max_age:
                """
                trackers：上一帧中的跟踪器链(列表)，保存的是上一帧中成功跟踪目标的跟踪框，也即成功跟踪目标的KalmanBoxTracker卡尔曼滤波对象。
                trackers.pop(要移除的某个跟踪框的索引值)：即能根据该索引值从跟踪器链(列表)中把该跟踪框移除出去
                """
                # pop(要移除的列表中元素的索引值)：根据列表中元素的索引值自动从列表中移除
                self.trackers.pop(i)

        # 返回当前画面中所有目标的box与id,以二维矩阵形式返回
        if len(ret) > 0:
            """
            ret：
                当前帧中跟踪目标成功的跟踪框/预测框的集合，包含目标的跟踪的id(也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个)
                第一种返回值方案：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度, trk.id] ...]
                第二种返回值方案(当前使用的为该种)：[[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, trk.id] ...]
                d：[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]
                trk.id：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个。
            [
              [左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度, 该跟踪框是创建出来的第几个]
              [...]
              [...]
            ]
            """
            return np.concatenate(ret)
        return np.empty((0, 5))

from kalman import *
import imutils
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt

"""
基于虚拟线圈法的车辆统计
    1.基于虚拟线圈的车流量统计算法原理与交通道路上的常见的传统的物理线圈类似，由于物理线圈需要埋设在路面之下，因此会有安装、维护费用高，
      造成路面破坏等问题，而采用基于视频的虚拟线圈的车辆计数方法完全避免了以上问题，且可以针对多个感兴趣区域进行检测。
    2.虚拟线圈车辆计数法的原理是在采集到的交通流视频中，在需要进行车辆计数的道路或路段上设置一条或一条以上的检测线对通过车辆进行检测，
      从而完成计数工作。检测线的设置原则一般是在检测车道上设置一条垂直于车道线，居中的虚拟线段，通过判断其与通过车辆的相对位置的变化，
      完成车流量统计的工作。如下图所示，绿色的线就是虚拟检测线：
"""

"""
1.虚拟线圈法检测的方法是，计算前后两帧图像的车辆检测框的中心点连线，若该连线与检测线相交，则计数加一，否则计数不变。
2.那怎么判断两条线段是否相交呢？
    假设有两条线段AB，CD，若AB，CD相交，我们可以确定：
        1.线段AB与CD相交，即点A和点B分别在线段CD的两边；
        2.线段CD与AB相交，即点C和点D分别在线段AB的两边；
    上面两个条件同时满足是两线段相交的充要条件，所以我们只需要证明点A和点B分别在线段CD的两边，点C和点D分别在线段AB的两边，
    这样便可以证明线段AB与CD相交了。

3.在上图中，线段AB与线段CD相交，于是我们可以得到两个向量AC、AD，其中C和D分别在AB的两边。
    1.向量AC在向量AB的逆时针方向，得AB×AC > 0。
      AB×AC实际是以A点为时钟圆盘的中心点，AB和AC分别是时钟的两个时针。
      向量AC在向量AB的逆时针方向的意思即为时针AB向时针AC进行逆时针移动，
      也即为B点向C点进行逆时针移动，最终得出AB×AC > 0；
    2.向量AD在向量AB的顺时针方向，得AB×AD < 0。
      AB×AD实际是以A点为时钟圆盘的中心点，AB和AD分别是时钟的两个时针。
      向量AD在向量AB的顺时针方向的意思即为时针AB向时针AD进行顺时针移动，
      也即为B点向D点进行顺时针移动，最终得出AB×AD < 0；
      最终得出 AB×AC > 0 和 AB×AD < 0 两个向量叉乘的结果为异号。
    3.这样，方法就出来了：
        如果线段CD的两个端点C和D，与另一条线段AB中的一个端点(A或B，只能是其中一个)连成的向量(比如AC/AD)，然后AC/AD与向量AB做叉乘。
        若结果异号，表示C和D分别在线段AB的两边；
        若结果同号，则表示CD两点都在AB的其中一边，则肯定不相交。
    所以我们利用叉乘的方法来判断车辆是否经过检测线。

4.此处的叉乘使用的是两个向量进行叉乘计算
    1.向量AB(线段AB)：可以是图像画面中的检测线，检测线的设置原则一般是在检测车道上设置一条垂直于车道的虚拟线段。
    2.向量CD(线段CD)：可以是前后两帧的目标框图像中的中心点所连成的一条线段。
    3.那么当线段CD中的C、D两个点(前后两帧的两个中心点)分别位于线段AB(检测线)的两边时，
      那么此时可以通过两个向量的叉乘计算，得出是否线段CD和线段AB是否相交。      
"""

# 线与线的碰撞检测：叉乘的方法判断两条线是否相交
# 计算叉乘符号
def ccw(A, B, C):
    return (C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0])
"""
(C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0])
    1.[0]：线段的其中一点的x坐标
      [1]：线段的其中一点的y坐标
    2.A点坐标(x1, y1)、B点坐标(x2, y2)、C点坐标(x3, y3)。
      A点坐标(x1, y1) 和 B点坐标(x2, y2) 那么得 A×B = x1*y2 - x2*y1。
    3.BA为(x2-x1, y2-y1)，CA为(x3-x1, y3-y1)。
      CA(x3-x1, y3-y1)中将x3-x1看作是w1，将y3-y1看做是h1；BA(x2-x1, y2-y1)中将x2-x1看作是w2，将y2-y1看做是h2；
      得出CA为(w1, h1)，BA为(w2, h2)，那么CA*BA = w1*h2 - w2*h1。
    4.可以把 (C[1] - A[1]) * (B[0] - A[0]) - (B[1] - A[1]) * (C[0] - A[0])
      转换为 (C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0]) 来使用，两者等同。
    5.BA为(x2-x1, y2-y1)，BA还可以看作为(w2, h2)。CA为(x3-x1, y3-y1)，CA还可以看作为(w1, h1)。
      (C[1] - A[1])：y3-y1，也即h1
      (B[0] - A[0])：x2-x1，也即w2
      (B[1] - A[1])：y2-y1，也即h2
      (C[0] - A[0])：x3-x1，也即w1
    6.(C[1] - A[1]) * (B[0] - A[0]) - (B[1] - A[1]) * (C[0] - A[0]) 即可以看做 BA*CA = h1*w2 - h2*w1
    7.如果线段CD的两个端点C和D，与另一条线段AB中的一个端点(A或B，只能是其中一个)连成的向量(比如AC/AD)，然后AC/AD与向量AB做叉乘。
      此处便使用BA*CA做叉乘，根据BA*CA = h1*w2 - h2*w1 得出 (C[1] - A[1]) * (B[0] - A[0]) - (B[1] - A[1]) * (C[0] - A[0])。
"""
# 检测AB和CD两条直线是否相交
def intersect(A, B, C, D):
    return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)

"""
CA(x3-x1, y3-y1)中将x3-x1看作是w1，将y3-y1看做是h1
BA(x2-x1, y2-y1)中将x2-x1看作是w2，将y2-y1看做是h2
DA(x4-x1, y4-y1)中将x4-x1看作是w3，将y4-y1看做是h3
CB(x3-x2, y3-y2)中将x3-x2看作是w4，将y3-y2看做是h4
DB(x4-x2, y4-y2)中将x4-x2看作是w5，将y4-y2看做是h5

ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)
1.ccw(A, C, D)：(D[1] - A[1]) * (C[0] - A[0]) > (C[1] - A[1]) * (D[0] - A[0]) 即可以看做 CA*DA = h3*w1 - h1*w3
2.ccw(B, C, D)：(D[1] - B[1]) * (C[0] - B[0]) > (C[1] - B[1]) * (D[0] - B[0]) 即可以看做 CB*DB = h5*w4 - h4*w5
3.ccw(A, B, C)：(C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0]) 即可以看做 BA*CA = h1*w2 - h2*w1
4.ccw(A, B, D)：(D[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (D[0] - A[0]) 即可以看做 BA*DA = h3*w2 - h2*w3
5.ccw(A, C, D) != ccw(B, C, D)：
    AC×AD < 0 和 BC×BD > 0 两个向量叉乘的结果为异号。
    向量AD在向量AC的顺时针方向。向量BD在向量BC的逆时针方向。
6.ccw(A, B, C) != ccw(A, B, D)：
    AB×AC > 0 和 AB×AD < 0 两个向量叉乘的结果为异号。
    向量AC在向量AB的逆时针方向。向量AD在向量AB的顺时针方向。
7.ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)：同时符合上述两者关系则得到AB和CD相交。
"""

# 虚拟线圈的检测线line，从图左边的(0, 150)点 画一直线连接到 图右边的(2560, 150)。
# 一旦有同一目标的前后两帧的检测框的中心点所连成的线段 相交于 虚拟线圈的检测线line 时，则认为两条线段相交。
line = [(0, 150), (2560, 150)]
# 车辆总数
counter = 0
# 正向车道的车辆数据
counter_up = 0
# 逆向车道的车辆数据
counter_down = 0

# 创建跟踪器对象
tracker = Sort()
#当前帧跟踪成功的跟踪框(KalmanBoxTracker卡尔曼滤波对象)
#key：跟踪框是创建出来的第几个的序号。value：跟踪框[左上角的x坐标, 左上角的x坐标y坐标, 右下角的x坐标, 右下角的y坐标]。
memory = {}

# 利用yoloV3模型进行目标检测
# 加载模型相关信息
# 加载可以检测的目标的类型
# 1.加载可以识别物体的名称，将其存放在LABELS中，一共有80种，在这我们只使用car
labelPath = "./yolo-coco/coco.names"
LABELS = open(labelPath).read().strip().split("\n")
# 设置随机数种子，生成多种不同的颜色，当一个画面中有多个目标时，使用不同颜色的框将其框起来
np.random.seed(42)
COLORS = np.random.randint(0, 255, size=(200, 3), dtype='uint8')
# 加载已训练好的yolov3网络的权重和相应的配置数据
# 加载好数据之后，开始利用上述数据恢复yolo神经网络
weightsPath = "./yolo-coco/yoloV3.weights"
configPath = "./yolo-coco/yoloV3.cfg"
#创建出yoloV3网络
net = cv2.dnn.readNetFromDarknet(configPath, weightsPath)
# 获取yolo中每一层的名称
ln = net.getLayerNames()
# print("yolo中每一层的名称",ln)
#['conv_0', 'bn_0', 'relu_1', 'conv_1', 'bn_1', 'relu_2', 'conv_2', 'bn_2', 'relu_3', 'conv_3', 'bn_3', 'relu_4',
# 'shortcut_4', 'conv_5', 'bn_5', 'relu_6', 'conv_6', 'bn_6', 'relu_7', 'conv_7', 'bn_7', 'relu_8', 'shortcut_8',
# 'conv_9', 'bn_9', 'relu_10', 'conv_10', 'bn_10', 'relu_11', 'shortcut_11', 'conv_12', 'bn_12', 'relu_13',
# 'conv_13', 'bn_13', 'relu_14', 'conv_14', 'bn_14', 'relu_15', 'shortcut_15', 'conv_16', 'bn_16', 'relu_17',
# 'conv_17', 'bn_17', 'relu_18', 'shortcut_18', 'conv_19', 'bn_19', 'relu_20', 'conv_20', 'bn_20', 'relu_21',
# 'shortcut_21', 'conv_22', 'bn_22', 'relu_23', 'conv_23', 'bn_23', 'relu_24', 'shortcut_24', 'conv_25', 'bn_25',
# 'relu_26', 'conv_26', 'bn_26', 'relu_27', 'shortcut_27', 'conv_28', 'bn_28', 'relu_29', 'conv_29', 'bn_29',
# 'relu_30', 'shortcut_30', 'conv_31', 'bn_31', 'relu_32', 'conv_32', 'bn_32', 'relu_33', 'shortcut_33',
# 'conv_34', 'bn_34', 'relu_35', 'conv_35', 'bn_35', 'relu_36', 'shortcut_36', 'conv_37', 'bn_37', 'relu_38',
# 'conv_38', 'bn_38', 'relu_39', 'conv_39', 'bn_39', 'relu_40', 'shortcut_40', 'conv_41', 'bn_41', 'relu_42',
# 'conv_42', 'bn_42', 'relu_43', 'shortcut_43', 'conv_44', 'bn_44', 'relu_45', 'conv_45', 'bn_45', 'relu_46',
# 'shortcut_46', 'conv_47', 'bn_47', 'relu_48', 'conv_48', 'bn_48', 'relu_49', 'shortcut_49', 'conv_50',
# 'bn_50', 'relu_51', 'conv_51', 'bn_51', 'relu_52', 'shortcut_52', 'conv_53', 'bn_53', 'relu_54', 'conv_54',
# 'bn_54', 'relu_55', 'shortcut_55', 'conv_56', 'bn_56', 'relu_57', 'conv_57', 'bn_57', 'relu_58', 'shortcut_58',
# 'conv_59', 'bn_59', 'relu_60', 'conv_60', 'bn_60', 'relu_61', 'shortcut_61', 'conv_62', 'bn_62', 'relu_63',
# 'conv_63', 'bn_63', 'relu_64', 'conv_64', 'bn_64', 'relu_65', 'shortcut_65', 'conv_66', 'bn_66', 'relu_67',
# 'conv_67', 'bn_67', 'relu_68', 'shortcut_68', 'conv_69', 'bn_69', 'relu_70', 'conv_70', 'bn_70', 'relu_71',
# 'shortcut_71', 'conv_72', 'bn_72', 'relu_73', 'conv_73', 'bn_73', 'relu_74', 'shortcut_74', 'conv_75',
# 'bn_75', 'relu_76', 'conv_76', 'bn_76', 'relu_77', 'conv_77', 'bn_77', 'relu_78', 'conv_78', 'bn_78',
# 'relu_79', 'conv_79', 'bn_79', 'relu_80', 'conv_80', 'bn_80', 'relu_81', 'conv_81', 'permute_82', 'yolo_82',
# 'identity_83', 'conv_84', 'bn_84', 'relu_85', 'upsample_85', 'concat_86', 'conv_87', 'bn_87', 'relu_88',
# 'conv_88', 'bn_88', 'relu_89', 'conv_89', 'bn_89', 'relu_90', 'conv_90', 'bn_90', 'relu_91', 'conv_91',
# 'bn_91', 'relu_92', 'conv_92', 'bn_92', 'relu_93', 'conv_93', 'permute_94', 'yolo_94', 'identity_95',
# 'conv_96', 'bn_96', 'relu_97', 'upsample_97', 'concat_98', 'conv_99', 'bn_99', 'relu_100', 'conv_100',
# 'bn_100', 'relu_101', 'conv_101', 'bn_101', 'relu_102', 'conv_102', 'bn_102', 'relu_103', 'conv_103',
# 'bn_103', 'relu_104', 'conv_104', 'bn_104', 'relu_105', 'conv_105', 'permute_106', 'yolo_106']

""" un connected Out Layers(未连接的输出层)： [[200] [227] [254]]"""
print("net.getUnconnectedOutLayers()",net.getUnconnectedOutLayers())
# 获取输出层在网络中的索引位置，并以列表的形式：['yolo_82', 'yolo_94', 'yolo_106']
ln = [ln[i[0] - 1] for i in net.getUnconnectedOutLayers()]

# 读取图像
# frame = cv2.imread('./images/car2.jpg')
# (W,H) = (None,None)
# (H,W) = frame.shape[:2]
# 初始化vediocapture类，参数指定打开的视频文件，也可以是摄像头
vs = cv2.VideoCapture('./input/test_1.mp4')
# 视频的宽度和高度，即帧尺寸
(W, H) = (None, None)
# 视频文件写对象
writer = None
try:
    # 确定获取视频帧数的方式
    prop = cv2.cv.CV_CAP_PROP_Frame_COUNT if imutils.is_cv2() else cv2.CAP_PROP_FRAME_COUNT
    # 获取视频的总帧数
    total = int(vs.get(prop))
    # 打印视频的帧数
    print("INFO:{} total Frame in video".format(total))
except:
    print("[INFO] could not determine in video")

# 遍历每一帧图像
while True:
    # 读取帧：grabbed是bool,表示是否成功捕获帧，frame是捕获的帧
    (grabed, frame) = vs.read()
    #读取完整个视频之后，grabed为False
    # 若未捕获帧，则退出循环
    if not grabed:
        break
    # 若W或H为空，则将第一帧画面的宽度和高度 即帧尺寸赋值给他
    if W is None or H is None:
        #获取图像的宽高
        (H, W) = frame.shape[:2]
    # 根据输入图像构造blob,利用OPenCV进行深度网路的计算时，一般将图像转换为blob形式，对图片进行预处理，包括缩放，减均值，通道交换等
    # 还可以设置尺寸，一般设置为在进行网络训练时的图像的大小
    # 将图像转换为blob，下一步可用于前向传播的输入数据
    blob = cv2.dnn.blobFromImage(frame, 1 / 255.0, (416, 416), swapRB=True, crop=False)
    # 将blob送入yoloV3前向网络中
    net.setInput(blob)
    start = time.time()
    # yoloV3网络 前向传播，进行预测，返回目标框边界和相应的概率
    layerOutputs = net.forward(ln)
    end = time.time()

    # 用于存放识别物体目标的检测框信息，包括框的左上角横坐标x和纵坐标y以及框的高h和宽w
    boxes = []
    # 置信度：此处存储的为最大概率的类别的预测概率值
    confidences = [] #表示识别目标是某种物体的可信度
    # 目标类别：此处存储的为 最大概率值的类别索引值
    classIDs = [] # 表示识别的目标归属于哪一类，['person', 'bicycle', 'car', 'motorbike'....]

    """
    输出layerOutsputs介绍：
        是YOLO算法在图片中检测到的bbx的信息
        由于YOLO v3有三个输出，也就是上面提到的['yolo_82', 'yolo_94', 'yolo_106']
        因此layerOutsputs是一个长度为3的列表
        其中，列表中每一个元素的维度是(num_detection, 85)
        num_detections表示该层输出检测到bbx的个数
        85：因为该模型在COCO数据集上训练，[5:]表示类别概率；[0:4]表示bbx的位置信息；[5]表示置信度
    下面对网络输出的bbx进行检查：
        判定每一个bbx的置信度是否足够的高，以及执行NMS算法去除冗余的bbx
    """
    # 遍历每个输出层[yolo-82, yolo-94, yolo-106]
    for output in layerOutputs:
        # 遍历某个输出层的检测框结果
        for detection in output:
            # detction检测框：1*85维度的向量。其中[5:]表示类别，[0:4]bbox的位置信息 [4]置信度
            scores = detection[5:] #80个类别的概率值。scores的大小应该是1*80，因为在训练yolo模型时是80类目标
            classID = np.argmax(scores) #获取最大概率值的类别索引值
            confidence = scores[classID] #根据最大概率值的类别索引值 获取出对应的类别
            #如果该最大概率的类别的预测概率值 大于 0.3
            if confidence > 0.3:
                """
                1.pw和ph分别为手动设定的锚框Anchor boxes宽和高，而网络最终计算的预测结果为(bx, by, bw, bh)，
                  因此需要把(tx, ty, tw, th)转换为(bx, by, bw, bh)。
                2.把(tx, ty, tw, th)转换为(bx, by, bw, bh)作为yolo输出层的最终输出：
                    σ读作sigma。Cx和Cy分别为当前单元格(grid cell)距离输入原图的左上角原点的边距离。
                    W和H为输入原图像的宽和高。分别除以W和H，目的是归一化。
                   
                        tx->bx：bx = (σ(tx) + Cx) / W
                        ty->by：by = (σ(ty) + Cy) / H
                        tw->bw：bw = (pw * e^tw) / W
                        th->bh：bh = (ph * e^th) / H
                       
                        σ(tx) + Cx：边界框的中心点在输入原图像中的x坐标，也即边界框的中心点离输入原图像原点的x方向长度
                        σ(ty) + Cy：边界框的中心点在输入原图像中的y坐标，也即边界框的中心点离输入原图像原点的y方向长度
                        pw * e^tw：边界框在输入原图像中的宽度
                        ph * e^th：边界框在输入原图像中的高度
                """
                # 将检测结果边界框的坐标还原至与原图片适配，YOLO返回的是边界框的中心坐标以及边界框的宽度和高度
                box = detection[0:4] * np.array([W, H, W, H])
                # 使用 astype("int") 对上述 array 进行强制类型转换
                # centerX：检测框的中心点横坐标， centerY：检测框的中心点纵坐标，width：检测框的宽度，height：检测框的高度
                (centerX, centerY, width, height) = box.astype("int")
                # 计算边界框的左上角的横坐标：检测框的中心点横坐标 - 检测框的宽度/2
                x = int(centerX - width / 2)
                # 计算边界框的左上角的纵坐标：检测框的中心点纵坐标 - 检测框的高度/2
                y = int(centerY - height / 2)
                # 更新检测到的目标框，置信度和类别ID
                # boxes：[边界框的左上角的横坐标, 边界框的左上角的纵坐标, 检测框的宽度, 检测框的高度]
                boxes.append([x, y, int(width), int(height)]) # 将边框的信息添加到列表boxes
                confidences.append(float(confidence)) # 将识别出是某种物体的置信度添加到列表confidences
                classIDs.append(classID) # 将识别物体归属于哪一类的信息添加到列表classIDs

    """
    上一步中已经得到yolo的检测框，但其中会存在冗余的bbox,即一个目标对应多个检测框，所以使用NMS去除重复的检测框。
    利用OpenCV内置的NMS DNN模块实现即可实现非最大值抑制 ，所需要的参数是边界框、置信度、以及置信度阈值和NMS阈值。
    第一个参数是存放边界框的列表，第二个参数是存放置信度的列表，第三个参数是自己设置的置信度，第四个参数是NMS阈值。
    返回的idxs是一个一维数组，数组中的元素是保留下来的检测框boxes的索引位置。
    dnn.NMSBoxes
        作用：根据给定的检测boxes和对应的scores进行NMS（非极大值抑制）处理
        原型：NMSBoxes(bboxes, scores, score_threshold, nms_threshold, eta=None, top_k=None)
        参数：
            boxes: 待处理的边界框 bounding boxes
            scores: 对于于待处理边界框的 scores
            score_threshold: 用于过滤 boxes 的 score 阈值
            nms_threshold: NMS 用到的阈值
            indices: NMS 处理后所保留的边界框的索引值
            eta: 自适应阈值公式中的相关系数：nms_threshold_i+1 = eta * nms_threshold_i
            top_k: 如果 top_k>0，则保留最多 top_k 个边界框索引值.
    """
    # 非极大值抑制
    # 此处的confidences使用的是 最大概率的类别的预测概率值。返回值idxs：保留下来的检测框boxes的索引位置的一维数组
    idxs = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.3)
    # 存放检测框的信息，包括左上角横坐标/纵坐标，右下角横坐标/纵坐标，以及检测到的物体的置信度(检测框的预测类别的概率值)，用于目标跟踪
    dets = []
    # 存在检测框的话（即检测框个数大于0）。idxs也即 保留下来的检测框boxes的索引位置的一维数组
    if len(idxs) > 0:
        #  循环检测出的每一个检测框boxes的索引位置
        for i in idxs.flatten():
            # yolo模型可以识别很多目标，因为我们在这里只是识别车，所以只有目标是车的我们进行检测，其他的忽略
            # classIDs[检测框boxes的索引位置]：根据检测框boxes的索引位置从classIDs列表中取出该检测框boxes对应的类别
            if LABELS[classIDs[i]] == "car":
                (x, y) = (boxes[i][0], boxes[i][1]) # 得到检测框的左上角的x/y坐标
                (w, h) = (boxes[i][2], boxes[i][3]) # 得到检测框的宽和高
                # cv2.rectangle(frame,(x,y),(x+w,y+h),(0,255,0),2)
                #检测框dets：[检测框的左上角的x/y坐标, 检测框的右下角的x/y坐标, 检测框的预测类别的概率值]
                dets.append([x, y, x + w, y + h, confidences[i]]) # 将检测框的信息的放入dets中

    # 类型设置
    # 设置数据类型，将整型数据转换为浮点数类型，且保留小数点后三位
    np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})
    # 将检测框数据转换为ndarray,其数据类型为浮点型
    dets = np.asarray(dets)

    # # 显示
    # plt.imshow(frame[:,:,::-1])
    # plt.show()

    #检测框为0
    if np.size(dets) == 0:
        continue
    else:
        #1.SORT目标跟踪：
        #       1.第一帧刚开始时：对第一帧所有的检测框生成对应的新跟踪框。
        #       2.第二帧开始到以后所有帧：
        #           上一帧成功跟踪并且保留下来的的跟踪框 在当前帧中 进行新一轮的预测新的跟踪框，
        #           并且针对所预测的新跟踪框和当前帧中的检测框进行iou计算和使用匈牙利算法对该两者进行关联匹配，
        #           通过上述操作后成功返回跟踪目标成功的跟踪框(即和当前帧中的目标检测框相匹配的跟踪框)，
        #           并且另外发现了新出现目标的检测框、跟踪目标失败的跟踪框(即目标离开了画面/两者匹配度IOU值小于iou阈值)，
        #           那么首先使用当前帧中的检测框对“成功关联匹配的跟踪框中的”状态向量进行更新，
        #           然后对新增目标的检测框生成对应新的跟踪框，最后把跟踪目标失败的跟踪框从跟踪器链列表中移除出去。
        #2.传入的检测框dets：[检测框的左上角的x/y坐标, 检测框的右下角的x/y坐标, 检测框的预测类别的概率值]
        #3.返回值tracks：
        #       当前帧中跟踪目标成功的跟踪框/预测框的集合，包含目标的跟踪的id(也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个)
        #       第一种返回值方案：[[左上角的x坐标, 左上角的y坐标, 右下角的x坐标, 右下角的y坐标, yolo识别目标是某种物体的可信度, trk.id] ...]
        #       第二种返回值方案(当前使用的为该种)：[[左上角的x坐标, 左上角的y坐标, 右下角的x坐标, 右下角的y坐标, trk.id] ...]
        #       d：[左上角的x坐标, 左上角的y坐标, 右下角的x坐标, 右下角的y坐标]
        #       trk.id：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个。
        tracks = tracker.update(dets)

    # 跟踪框
    boxes = []
    # indexIDs 也即 trk.id：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个
    indexIDs = []
    # 上一帧跟踪成功的跟踪框：把上一帧保留下来的跟踪成功的跟踪框 深拷贝 一份
    previous = memory.copy()
    # 创建新的集合 用于保存 当前帧跟踪成功的跟踪框(KalmanBoxTracker卡尔曼滤波对象)
    # key：跟踪框是创建出来的第几个的序号。value：跟踪框[左上角的x坐标, 左上角的y坐标, 右下角的x坐标, 右下角的y坐标]。
    memory = {}
    # 遍历 当前帧中跟踪目标成功的跟踪框
    for track in tracks:
        #当前帧中跟踪目标成功的跟踪框(即当前帧中检测框相匹配的跟踪框)：[左上角的x坐标, 左上角的y坐标, 右下角的x坐标, 右下角的y坐标]
        boxes.append([track[0], track[1], track[2], track[3]])
        # 即 trk.id：卡尔曼滤波器的个数/目标框的个数，也即该跟踪框(卡尔曼滤波实例对象)是创建出来的第几个
        indexIDs.append(int(track[4]))
        #key：跟踪框是创建出来的第几个的序号。value：跟踪框[左上角的x坐标, 左上角的y坐标, 右下角的x坐标, 右下角的y坐标]。
        memory[indexIDs[-1]] = boxes[-1]

    # 碰撞检测：虚拟线圈法检测的方法是 计算前后两帧图像的车辆检测框的中心点连线，若该连线与检测线相交，则计数加一，否则计数不变。
    if len(boxes) > 0:
        i = int(0)
        # 遍历跟踪框
        for box in boxes:
            (x, y) = (int(box[0]), int(box[1])) # 左上角的x坐标, 左上角的x坐标
            (w, h) = (int(box[2]), int(box[3])) # 右下角的x坐标, 右下角的y坐标
            color = [int(c) for c in COLORS[indexIDs[i] % len(COLORS)]]
            cv2.rectangle(frame, (x, y), (w, h), color, 2)

            # 根据在上一帧和当前帧的检测结果，利用虚拟线圈完成车辆计数
            #indexIDs[i]：跟踪框是创建出来的第几个的序号。previous：key：跟踪框是创建出来的第几个的序号。
            #判断的是 跟踪框是创建出来的第几个的序号 是否和 previous中有相同的 key
            if indexIDs[i] in previous:
                #previous[key：跟踪框是创建出来的第几个的序号] 获取出value 跟踪框[左上角的x坐标, 左上角的y坐标, 右下角的x坐标, 右下角的y坐标]
                previous_box = previous[indexIDs[i]]
                (x2, y2) = (int(previous_box[0]), int(previous_box[1])) # 左上角的x坐标, 左上角的y坐标
                (w2, h2) = (int(previous_box[2]), int(previous_box[3])) # 右下角的x坐标, 右下角的y坐标
                #上一帧中跟踪框的 中心点的x坐标：左上角的x坐标 + (右下角的x坐标 - 左上角的x坐标) / 2
                #上一帧中跟踪框的 中心点的y坐标：左上角的y坐标 + (右下角的y坐标 - 左上角的y坐标) / 2
                p1 = (int(x2 + (w2 - x2) / 2), int(y2 + (h2 - y2) / 2))
                #当前帧中跟踪框的 中心点的x坐标：左上角的x坐标 + (右下角的x坐标 - 左上角的x坐标) / 2
                #当前帧中跟踪框的 中心点的y坐标：左上角的y坐标 + (右下角的y坐标 - 左上角的y坐标) / 2
                p0 = (int(x + (w - x) / 2), int(y + (h - y) / 2))

                # 利用p0,p1与line进行碰撞检测
                # 检测AB和CD两条直线是否相交：p0和p1即为AB，line[0]和line[1]即为CD
                # 同一个目标的前后两帧的跟踪框的中心点构建为一条线段，即AB。检测线即CD。
                if intersect(p0, p1, line[0], line[1]):
                    counter += 1
                    # 判断行进方向
                    #上一帧中跟踪框的 左上角的y坐标 大于 当前帧中跟踪框的 左上角的y坐标
                    if y2 > y:
                        # 逆向车道的车辆数据
                        counter_down += 1
                    else:
                        # 正向车道的车辆数据
                        counter_up += 1
            i += 1

    # 将车辆计数的相关结果放在视频上
    cv2.line(frame, line[0], line[1], (0, 255, 0), 3)
    cv2.putText(frame, str(counter), (30, 80), cv2.FONT_HERSHEY_DUPLEX, 3.0, (255, 0, 0), 3)
    cv2.putText(frame, str(counter_up), (130, 80), cv2.FONT_HERSHEY_DUPLEX, 3.0, (0, 255, 0), 3)
    cv2.putText(frame, str(counter_down), (230, 80), cv2.FONT_HERSHEY_DUPLEX, 3.0, (0, 0, 255), 3)

    # 将检测结果保存在视频
    if writer is None:
        fourcc = cv2.VideoWriter_fourcc(*"mp4v")
        writer = cv2.VideoWriter("./output/output.mp4", fourcc, 30, (frame.shape[1], frame.shape[0]), True)
    writer.write(frame)
    cv2.imshow("", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

"释放资源"
writer.release()
vs.release()
cv2.destroyAllWindows()

from __future__ import print_function
# 对for循环有姮好的效果
from numba import jit
import numpy as np
# 用于线性分配，匈牙利匹配的实现
# from sklearn.utils.linear_assignment_ import linear_assignment
from scipy.optimize import linear_sum_assignment
# 使用卡尔曼滤波器
from filterpy.kalman import KalmanFilter


@jit
def iou(bb_test, bb_gt):
    """
    在两个box间计算IOU
    :param bb_test: box1 = [x1y1x2y2]
    :param bb_gt: box2 = [x1y1x2y2]
    :return: 交并比IOU
    """
    xx1 = np.maximum(bb_test[0], bb_gt[0])
    yy1 = np.maximum(bb_test[1], bb_gt[1])
    xx2 = np.minimum(bb_test[2], bb_gt[2])
    yy2 = np.minimum(bb_test[3], bb_gt[3])
    w = np.maximum(0., xx2 - xx1)
    h = np.maximum(0., yy2 - yy1)
    wh = w * h
    o = wh / ((bb_test[2] - bb_test[0]) * (bb_test[3] - bb_test[1]) + (bb_gt[2] - bb_gt[0]) * (
            bb_gt[3] - bb_gt[1]) - wh)
    return o


def convert_bbox_to_z(bbox):
    """
    将[x1,y1,x2,y2]形式的检测框转为滤波器的状态表示形式[x,y,s,r]。其中x，y是框的中心，s是比例/区域，r是宽高比
    :param bbox: [x1,y1,x2,y2] 分别是左上角坐标和右下角坐标
    :return: [ x, y, s, r ] 4行1列，其中x,y是box中心位置的坐标，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
    r = w / float(h)
    return np.array([x, y, s, r]).reshape((4, 1))


def convert_x_to_bbox(x, score=None):
    """
    将[cx，cy，s，r]的目标框表示转为[x_min，y_min，x_max，y_max]的形式
    :param x:[ x, y, s, r ],其中x,y是box中心位置的坐标，s是面积，r
    :param score: 置信度
    :return:[x1,y1,x2,y2],左上角坐标和右下角坐标
    """
    w = np.sqrt(x[2] * x[3])
    h = x[2] / w
    if score is 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))


"""
# 表示观测目标框bbox所对应的单个跟踪对像的内部状态
"""

class KalmanBoxTracker(object):
    count = 0

    def __init__(self, bbox):
        """
        初始化边界框和跟踪器
        :param bbox:
        """
        # 定义等速模型
        # 内部使用KalmanFilter，7个状态变量和4个观测输入
        self.kf = KalmanFilter(dim_x=7, dim_z=4)
        # F是状态变换模型
        self.kf.F = np.array(
            [[1, 0, 0, 0, 1, 0, 0], [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]])
        # H是观测函数
        self.kf.H = np.array(
            [[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, 0, 0, 0]])
        # R是观测函数
        self.kf.R[2:, 2:] *= 10.
        # P是协方差矩阵
        self.kf.P[4:, 4:] *= 1000.  # give high uncertainty to the unobservable initial velocities
        self.kf.P *= 10.
        # Q是过程噪声矩阵
        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 = []
        self.hits = 0
        self.hit_streak = 0
        self.age = 0

    def update(self, bbox):
        """
        使用观察到的目标框更新状态向量。filterpy.kalman.KalmanFilter.update 会根据观测修改内部状态估计self.kf.x。
        重置self.time_since_update，清空self.history。
        :param bbox:目标框
        :return:
        """
        self.time_since_update = 0
        self.history = []
        self.hits += 1
        self.hit_streak += 1
        self.kf.update(convert_bbox_to_z(bbox))

    def predict(self):
        """
        推进状态向量并返回预测的边界框估计。
        将预测结果追加到self.history。由于 get_state 直接访问 self.kf.x，所以self.history没有用到
        :return:
        """
        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:
            self.hit_streak = 0
        self.time_since_update += 1
        self.history.append(convert_x_to_bbox(self.kf.x))
        return self.history[-1]

    def get_state(self):
        """
        返回当前边界框估计值
        :return:
        """
        return convert_x_to_bbox(self.kf.x)


def associate_detections_to_trackers(detections, trackers, iou_threshold=0.3):
    """
    将检测框bbox与卡尔曼滤波器的跟踪框进行关联匹配
    :param detections:检测框
    :param trackers:跟踪框，即跟踪目标
    :param iou_threshold:IOU阈值
    :return:跟踪成功目标的矩阵：matchs
            新增目标的矩阵：unmatched_detections
            跟踪失败即离开画面的目标矩阵：unmatched_trackers
    """
    # 跟踪目标数量为0，直接构造结果
    if (len(trackers) == 0) or (len(detections) == 0):
        return np.empty((0, 2), dtype=int), np.arange(len(detections)), np.empty((0, 5), dtype=int)

    # iou 不支持数组计算。逐个计算两两间的交并比，调用 linear_assignment 进行匹配
    iou_matrix = np.zeros((len(detections), len(trackers)), dtype=np.float32)
    # 遍历目标检测的bbox集合，每个检测框的标识为d
    for d, det in enumerate(detections):
        # 遍历跟踪框（卡尔曼滤波器预测）bbox集合，每个跟踪框标识为t
        for t, trk in enumerate(trackers):
            iou_matrix[d, t] = iou(det, trk)
    # 通过匈牙利算法将跟踪框和检测框以[[d,t]...]的二维矩阵的形式存储在match_indices中
    # 为什么是负号：linear_assignment的输入是成本矩阵，IOU越大对应的分配代价应越小
    # matched_indices = linear_assignment(-iou_matrix)
    result = linear_sum_assignment(-iou_matrix)
    matched_indices = np.array(list(zip(*result)))

    # 记录未匹配的检测框及跟踪框
    # 未匹配的检测框放入unmatched_detections中，表示有新的目标进入画面，要新增跟踪器跟踪目标
    unmatched_detections = []
    for d, det in enumerate(detections):
        if d not in matched_indices[:, 0]:
            unmatched_detections.append(d)
    # 未匹配的跟踪框放入unmatched_trackers中，表示目标离开之前的画面，应删除对应的跟踪器
    unmatched_trackers = []
    for t, trk in enumerate(trackers):
        if t not in matched_indices[:, 1]:
            unmatched_trackers.append(t)
    # 将匹配成功的跟踪框放入matches中
    matches = []
    for m in matched_indices:
        # 过滤掉IOU低的匹配，将其放入到unmatched_detections和unmatched_trackers
        if iou_matrix[m[0], m[1]] < iou_threshold:
            unmatched_detections.append(m[0])
            unmatched_trackers.append(m[1])
        # 满足条件的以[[d,t]...]的形式放入matches中
        else:
            matches.append(m.reshape(1, 2))

    # 初始化matches,以np.array的形式返回
    if len(matches) == 0:
        matches = np.empty((0, 2), dtype=int)
    else:
        matches = np.concatenate(matches, axis=0)
    return matches, np.array(unmatched_detections), np.array(unmatched_trackers)


class Sort(object):
    """
    Sort 是一个多目标跟踪器，管理多个 KalmanBoxTracker 对象
    """
    def __init__(self, max_age=1, min_hits=3):
        """
        初始化：设置SORT算法的关键参数
        :param max_age: 最大检测数：目标未被检测到的帧数，超过之后会被删除
        :param min_hits:
        """
        self.max_age = max_age
        self.min_hits = min_hits
        self.trackers = []   # ？
        self.frame_count = 0  # ？

    def update(self, dets):

        """
        该方法实现了SORT算法，输入是当前帧中所有物体的检测框的集合，包括目标的score,
        输出是当前帧目标的跟踪框集合，包括目标的跟踪的id
        要求是即使检测框为空，也必须对每一帧调用此方法，返回一个类似的输出数组，最后一列是目标对像的id
        注意：返回的目标对象数量可能与检测框的数量不同
        :param dets:以[[x1,y1,x2,y2,score]，[x1,y1,x2,y2,score]，...]形式输入的numpy.array
        :return:
        """
        self.frame_count += 1
        # 在当前帧逐个预测轨迹位置，记录状态异常的跟踪器索引
        # 根据当前所有的卡尔曼跟踪器个数（即上一帧中跟踪的目标个数）创建二维数组：行号为卡尔曼滤波器的标识索引，列向量为跟踪框的位置和ID
        trks = np.zeros((len(self.trackers), 5))  # 存储跟踪器的预测
        to_del = []   # 存储要删除的目标框
        ret = []    # 存储要返回的追踪目标框
        # 循环遍历卡尔曼跟踪器列表
        for t, trk in enumerate(trks):
            # 使用卡尔曼跟踪器t产生对应目标的跟踪框
            pos = self.trackers[t].predict()[0]
            # 遍历完成后，trk中存储了上一帧中跟踪的目标的预测跟踪框
            trk[:] = [pos[0], pos[1], pos[2], pos[3], 0]
            # 如果跟踪框中包含空值则将该跟踪框添加到要删除的列表中
            if np.any(np.isnan(pos)):
                to_del.append(t)
        # numpy.ma.masked_invalid 屏蔽出现无效值的数组（NaN 或 inf）
        # numpy.ma.compress_rows 压缩包含掩码值的2-D 数组的整行，将包含掩码值的整行去除
        # trks中存储了上一帧中跟踪的目标并且在当前帧中的预测跟踪框
        trks = np.ma.compress_rows(np.ma.masked_invalid(trks))
        # 逆向删除异常的跟踪器，防止破坏索引
        for t in reversed(to_del):
            self.trackers.pop(t)
        # 将目标检测框与卡尔曼滤波器预测的跟踪框关联获取跟踪成功的目标，新增的目标，离开画面的目标
        matched, unmatched_dets, unmatched_trks = associate_detections_to_trackers(dets, trks)

        # 将跟踪成功的目标框更新到对应的卡尔曼滤波器
        for t, trk in enumerate(self.trackers):
            if t not in unmatched_trks:
                d = matched[np.where(matched[:, 1] == t)[0], 0]

                # 使用观测的边界框更新状态向量
                trk.update(dets[d, :][0])

        # 为新增的目标创建新的卡尔曼滤波器对象进行跟踪
        for i in unmatched_dets:
            trk = KalmanBoxTracker(dets[i, :])
            self.trackers.append(trk)

        # 自后向前遍历，仅返回在当前帧出现且命中周期大于self.min_hits（除非跟踪刚开始）的跟踪结果；如果未命中时间大于self.max_age则删除跟踪器。
        # hit_streak忽略目标初始的若干帧
        i = len(self.trackers)
        for trk in reversed(self.trackers):
            # 返回当前边界框的估计值
            d = trk.get_state()[0]
            # 跟踪成功目标的box与id放入ret列表中
            if (trk.time_since_update < 1) and (trk.hit_streak >= self.min_hits or self.frame_count <= self.min_hits):
                ret.append(np.concatenate((d, [trk.id + 1])).reshape(1, -1))  # +1 as MOT benchmark requires positive
            i -= 1
            # 跟踪失败或离开画面的目标从卡尔曼跟踪器中删除
            if trk.time_since_update > self.max_age:
                self.trackers.pop(i)
        # 返回当前画面中所有目标的box与id,以二维矩阵形式返回
        if len(ret) > 0:
            return np.concatenate(ret)
        return np.empty((0, 5))

from MultiCarDection.kalman import *
import imutils
import time
import cv2
import numpy as np
import matplotlib.pyplot as plt

line = [(0, 150), (2560, 150)]
# 车辆总数
counter = 0
# 正向车道的车辆数据
counter_up = 0
# 逆向车道的车辆数据
counter_down = 0

# 创建跟踪器对象
tracker = Sort()
memory = {}


# 线与线的碰撞检测：叉乘的方法判断两条线是否相交
# 计算叉乘符号
def ccw(A, B, C):
    return (C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0])


# 检测AB和CD两条直线是否相交
def intersect(A, B, C, D):
    return ccw(A, C, D) != ccw(B, C, D) and ccw(A, B, C) != ccw(A, B, D)


# 利用yoloV3模型进行目标检测
# 加载模型相关信息
# 加载可以检测的目标的类型
labelPath = "./yolo-coco/coco.names"
LABELS = open(labelPath).read().strip().split("\n")
# 生成多种不同的颜色
np.random.seed(42)
COLORS = np.random.randint(0, 255, size=(200, 3), dtype='uint8')
# 加载预训练的模型：权重 配置信息,进行恢复
weightsPath = "./yolo-coco/yoloV3.weights"
configPath = "./yolo-coco/yoloV3.cfg"
net = cv2.dnn.readNetFromDarknet(configPath, weightsPath)
# 获取yolo中每一层的名称
ln = net.getLayerNames()
# 获取输出层的名称: [yolo-82,yolo-94,yolo-106]
ln = [ln[i[0] - 1] for i in net.getUnconnectedOutLayers()]

# 读取图像
# frame = cv2.imread('./images/car2.jpg')
# (W,H) = (None,None)
# (H,W) = frame.shape[:2]
# 视频
vs = cv2.VideoCapture('./input/test_1.mp4')
(W, H) = (None, None)
writer = None
try:
    prop = cv2.cv.CV_CAP_PROP_Frame_COUNT if imutils.is_cv2() else cv2.CAP_PROP_FRAME_COUNT
    total = int(vs.get(prop))
    print("INFO:{} total Frame in video".format(total))
except:
    print("[INFO] could not determine in video")

# 遍历每一帧图像
while True:
    (grabed, frame) = vs.read()
    if not grabed:
        break
    if W is None or H is None:
        (H,W) = frame.shape[:2]
    # 将图像转换为blob,进行前向传播
    blob = cv2.dnn.blobFromImage(frame, 1 / 255.0, (416, 416), swapRB=True, crop=False)
    # 将blob送入网络
    net.setInput(blob)
    start = time.time()
    # 前向传播，进行预测，返回目标框边界和相应的概率
    layerOutputs = net.forward(ln)
    end = time.time()

    # 存放目标的检测框
    boxes = []
    # 置信度
    confidences = []
    # 目标类别
    classIDs = []

    # 遍历每个输出
    for output in layerOutputs:
        # 遍历检测结果
        for detection in output:
            # detction:1*85 [5:]表示类别，[0:4]bbox的位置信息 [4]置信度
            scores = detection[5:]
            classID = np.argmax(scores)
            confidence = scores[classID]

            if confidence > 0.3:
                # 将检测结果与原图片进行适配
                box = detection[0:4] * np.array([W, H, W, H])
                (centerX, centerY, width, height) = box.astype("int")
                # 左上角坐标
                x = int(centerX - width / 2)
                y = int(centerY - height / 2)
                # 更新目标框，置信度，类别
                boxes.append([x, y, int(width), int(height)])
                confidences.append(float(confidence))
                classIDs.append(classID)
    # 非极大值抑制
    idxs = cv2.dnn.NMSBoxes(boxes, confidences, 0.5, 0.3)
    # 检测框:左上角和右下角
    dets = []
    if len(idxs) > 0:
        for i in idxs.flatten():
            if LABELS[classIDs[i]] == "car":
                (x, y) = (boxes[i][0], boxes[i][1])
                (w, h) = (boxes[i][2], boxes[i][3])
                # cv2.rectangle(frame,(x,y),(x+w,y+h),(0,255,0),2)
                dets.append([x, y, x + w, y + h, confidences[i]])
    # 类型设置
    np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})
    dets = np.asarray(dets)

    # # 显示
    # plt.imshow(frame[:,:,::-1])
    # plt.show()

    # SORT目标跟踪
    if np.size(dets) == 0:
        continue
    else:
        tracks = tracker.update(dets)
    # 跟踪框
    boxes = []
    # 置信度
    indexIDs = []
    # 前一帧跟踪结果
    previous = memory.copy()
    memory = {}
    for track in tracks:
        boxes.append([track[0], track[1], track[2], track[3]])
        print(int(track[4]))
        indexIDs.append(int(track[4]))
        memory[indexIDs[-1]] = boxes[-1]
    print("indexIDs",indexIDs)
    # 碰撞检测
    if len(boxes) > 0:
        i = int(0)
        # 遍历跟踪框
        for box in boxes:
            (x, y) = (int(box[0]), int(box[1]))
            (w, h) = (int(box[2]), int(box[3]))
            color = [int(c) for c in COLORS[indexIDs[i] % len(COLORS)]]
            cv2.rectangle(frame, (x, y), (w, h), color, 2)

            # 根据在上一帧和当前帧的检测结果，利用虚拟线圈完成车辆计数
            if indexIDs[i] in previous:
                previous_box = previous[indexIDs[i]]
                (x2, y2) = (int(previous_box[0]), int(previous_box[1]))
                (w2, h2) = (int(previous_box[2]), int(previous_box[3]))
                p1 = (int(x2 + (w2 - x2) / 2), int(y2 + (h2 - y2) / 2))
                p0 = (int(x + (w - x) / 2), int(y + (h - y) / 2))

                # 利用p0,p1与line进行碰撞检测
                if intersect(p0, p1, line[0], line[1]):
                    counter += 1
                    # 判断行进方向
                    if y2 > y:
                        counter_down += 1
                    else:
                        counter_up += 1
            i += 1

    # 将车辆计数的相关结果放在视频上
    cv2.line(frame, line[0], line[1], (0, 255, 0), 3)
    cv2.putText(frame, str(counter), (30, 80), cv2.FONT_HERSHEY_DUPLEX, 3.0, (255, 0, 0), 3)
    cv2.putText(frame, str(counter_up), (130, 80), cv2.FONT_HERSHEY_DUPLEX, 3.0, (0, 255, 0), 3)
    cv2.putText(frame, str(counter_down), (230, 80), cv2.FONT_HERSHEY_DUPLEX, 3.0, (0, 0, 255), 3)

    # 将检测结果保存在视频
    if writer is None:
        fourcc = cv2.VideoWriter_fourcc(*"mp4v")
        writer = cv2.VideoWriter("./output/output.mp4", fourcc, 30, (frame.shape[1], frame.shape[0]), True)
    writer.write(frame)
    cv2.imshow("", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

"释放资源"
writer.release()
vs.release()
cv2.destroyAllWindows()