sort demo

hungarian.h

#ifndef HUNGARIAN_H
#define HUNGARIAN_H

#include <iostream>
#include <vector>

class hungarian
{
public:
    hungarian();
    ~hungarian();
    float Solve(std::vector<std::vector<float>>& DistMatrix, std::vector<int>& Assignment);
    private:
        void assignmentoptimal(int *assignment, float *cost, float *distMatrix, int nOfRows, int nOfColumns);
        void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns);
        void computeassignmentcost(int *assignment, float *cost, float *distMatrix, int nOfRows);
        void step2a(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
        void step2b(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
        void step3(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
        void step4(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col);
        void step5(int *assignment, float *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim);
};

#endif // HUNGARIAN_H

hungarian.cpp

#include "hungarian.h"
#include <stdlib.h>
#include <cfloat> // for DBL_MAX
#include <cmath>  // for fabs()

hungarian::hungarian(){}
hungarian::~hungarian(){}


//********************************************************//
// A single function wrapper for solving assignment problem.
//********************************************************//
float hungarian::Solve(std::vector<std::vector<float> >& DistMatrix, std::vector<int>& Assignment)
{
    unsigned int nRows = DistMatrix.size();
    unsigned int nCols = DistMatrix[0].size();

    float *distMatrixIn = new float[nRows * nCols];
    int *assignment = new int[nRows];
    float cost = 0.0;

    // Fill in the distMatrixIn. Mind the index is "i + nRows * j".
    // Here the cost matrix of size MxN is defined as a float precision array of N*M elements.
    // In the solving functions matrices are seen to be saved MATLAB-internally in row-order.
    // (i.e. the matrix [1 2; 3 4] will be stored as a vector [1 3 2 4], NOT [1 2 3 4]).
    for (unsigned int i = 0; i < nRows; i++)
        for (unsigned int j = 0; j < nCols; j++)
            distMatrixIn[i + nRows * j] = DistMatrix[i][j];

    // call solving function
    assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols);

    Assignment.clear();
    for (unsigned int r = 0; r < nRows; r++)
        Assignment.push_back(assignment[r]);

    delete[] distMatrixIn;
    delete[] assignment;
    return cost;
}


//********************************************************//
// Solve optimal solution for assignment problem using Munkres algorithm, also known as Hungarian Algorithm.
//********************************************************//
void hungarian::assignmentoptimal(int *assignment, float *cost, float *distMatrixIn, int nOfRows, int nOfColumns)
{
    float *distMatrix, *distMatrixTemp, *distMatrixEnd, *columnEnd, value, minValue;
    bool *coveredColumns, *coveredRows, *starMatrix, *newStarMatrix, *primeMatrix;
    int nOfElements, minDim, row, col;

    /* initialization */
    *cost = 0;
    for (row = 0; row<nOfRows; row++)
        assignment[row] = -1;

    /* generate working copy of distance Matrix */
    /* check if all matrix elements are positive */
    nOfElements = nOfRows * nOfColumns;
    distMatrix = (float *)malloc(nOfElements * sizeof(float));
    distMatrixEnd = distMatrix + nOfElements;

    for (row = 0; row<nOfElements; row++)
        distMatrix[row] = distMatrixIn[row];;

    /* memory allocation */
    coveredColumns = (bool *)calloc(nOfColumns, sizeof(bool));
    coveredRows = (bool *)calloc(nOfRows, sizeof(bool));
    starMatrix = (bool *)calloc(nOfElements, sizeof(bool));
    primeMatrix = (bool *)calloc(nOfElements, sizeof(bool));
    newStarMatrix = (bool *)calloc(nOfElements, sizeof(bool)); /* used in step4 */

    /* preliminary steps */
    if (nOfRows <= nOfColumns)
    {
        minDim = nOfRows;

        for (row = 0; row<nOfRows; row++)
        {
            /* find the smallest element in the row */
            distMatrixTemp = distMatrix + row;
            minValue = *distMatrixTemp;
            distMatrixTemp += nOfRows;
            while (distMatrixTemp < distMatrixEnd)
            {
                value = *distMatrixTemp;
                if (value < minValue)
                    minValue = value;
                distMatrixTemp += nOfRows;
            }

            /* subtract the smallest element from each element of the row */
            distMatrixTemp = distMatrix + row;
            while (distMatrixTemp < distMatrixEnd)
            {
                *distMatrixTemp -= minValue;
                distMatrixTemp += nOfRows;
            }
        }

        /* Steps 1 and 2a */
        for (row = 0; row<nOfRows; row++)
            for (col = 0; col<nOfColumns; col++)
                if (fabs(distMatrix[row + nOfRows*col]) < DBL_EPSILON)
                    if (!coveredColumns[col])
                    {
                        starMatrix[row + nOfRows*col] = true;
                        coveredColumns[col] = true;
                        break;
                    }
    }
    else /* if(nOfRows > nOfColumns) */
    {
        minDim = nOfColumns;

        for (col = 0; col<nOfColumns; col++)
        {
            /* find the smallest element in the column */
            distMatrixTemp = distMatrix + nOfRows*col;
            columnEnd = distMatrixTemp + nOfRows;

            minValue = *distMatrixTemp++;
            while (distMatrixTemp < columnEnd)
            {
                value = *distMatrixTemp++;
                if (value < minValue)
                    minValue = value;
            }

            /* subtract the smallest element from each element of the column */
            distMatrixTemp = distMatrix + nOfRows*col;
            while (distMatrixTemp < columnEnd)
                *distMatrixTemp++ -= minValue;
        }

        /* Steps 1 and 2a */
        for (col = 0; col<nOfColumns; col++)
            for (row = 0; row<nOfRows; row++)
                if (fabs(distMatrix[row + nOfRows*col]) < DBL_EPSILON)
                    if (!coveredRows[row])
                    {
                        starMatrix[row + nOfRows*col] = true;
                        coveredColumns[col] = true;
                        coveredRows[row] = true;
                        break;
                    }
        for (row = 0; row<nOfRows; row++)
            coveredRows[row] = false;

    }

    /* move to step 2b */
    step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);

    /* compute cost and remove invalid assignments */
    computeassignmentcost(assignment, cost, distMatrixIn, nOfRows);

    /* free allocated memory */
    free(distMatrix);
    free(coveredColumns);
    free(coveredRows);
    free(starMatrix);
    free(primeMatrix);
    free(newStarMatrix);

    return;
}

/********************************************************/
void hungarian::buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns)
{
    int row, col;

    for (row = 0; row<nOfRows; row++)
        for (col = 0; col<nOfColumns; col++)
            if (starMatrix[row + nOfRows*col])
            {
#ifdef ONE_INDEXING
                assignment[row] = col + 1; /* MATLAB-Indexing */
#else
                assignment[row] = col;
#endif
                break;
            }
}

/********************************************************/
void hungarian::computeassignmentcost(int *assignment, float *cost, float *distMatrix, int nOfRows)
{
    int row, col;

    for (row = 0; row<nOfRows; row++)
    {
        col = assignment[row];
        if (col >= 0)
            *cost += distMatrix[row + nOfRows*col];
    }
}

/********************************************************/
void hungarian::step2a(int *assignment, float *distMatrix, bool *starMatrix,
                       bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns,
                       bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
    bool *starMatrixTemp, *columnEnd;
    int col;

    /* cover every column containing a starred zero */
    for (col = 0; col<nOfColumns; col++)
    {
        starMatrixTemp = starMatrix + nOfRows*col;
        columnEnd = starMatrixTemp + nOfRows;
        while (starMatrixTemp < columnEnd){
            if (*starMatrixTemp++)
            {
                coveredColumns[col] = true;
                break;
            }
        }
    }

    /* move to step 3 */
    step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix,
           coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

/********************************************************/
void hungarian::step2b(int *assignment, float *distMatrix, bool *starMatrix,
                       bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns,
                       bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
    int col, nOfCoveredColumns;

    /* count covered columns */
    nOfCoveredColumns = 0;
    for (col = 0; col<nOfColumns; col++)
        if (coveredColumns[col])
            nOfCoveredColumns++;

    if (nOfCoveredColumns == minDim)
    {
        /* algorithm finished */
        buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns);
    }
    else
    {
        /* move to step 3 */
        step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
    }

}

/********************************************************/
void hungarian::step3(int *assignment, float *distMatrix, bool *starMatrix,
                      bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns,
                      bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
    bool zerosFound;
    int row, col, starCol;

    zerosFound = true;
    while (zerosFound)
    {
        zerosFound = false;
        for (col = 0; col<nOfColumns; col++)
            if (!coveredColumns[col])
                for (row = 0; row<nOfRows; row++)
                    if ((!coveredRows[row]) && (fabs(distMatrix[row + nOfRows*col]) < DBL_EPSILON))
                    {
                        /* prime zero */
                        primeMatrix[row + nOfRows*col] = true;

                        /* find starred zero in current row */
                        for (starCol = 0; starCol<nOfColumns; starCol++)
                            if (starMatrix[row + nOfRows*starCol])
                                break;

                        if (starCol == nOfColumns) /* no starred zero found */
                        {
                            /* move to step 4 */
                            step4(assignment, distMatrix, starMatrix, newStarMatrix,
                                  primeMatrix, coveredColumns, coveredRows, nOfRows,
                                  nOfColumns, minDim, row, col);
                            return;
                        }
                        else
                        {
                            coveredRows[row] = true;
                            coveredColumns[starCol] = false;
                            zerosFound = true;
                            break;
                        }
                    }
    }

    /* move to step 5 */
    step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

/********************************************************/
void hungarian::step4(int *assignment, float *distMatrix, bool *starMatrix,
                      bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns,
                      bool *coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col)
{
    int n, starRow, starCol, primeRow, primeCol;
    int nOfElements = nOfRows*nOfColumns;

    /* generate temporary copy of starMatrix */
    for (n = 0; n<nOfElements; n++)
        newStarMatrix[n] = starMatrix[n];

    /* star current zero */
    newStarMatrix[row + nOfRows*col] = true;

    /* find starred zero in current column */
    starCol = col;
    for (starRow = 0; starRow<nOfRows; starRow++)
        if (starMatrix[starRow + nOfRows*starCol])
            break;

    while (starRow<nOfRows)
    {
        /* unstar the starred zero */
        newStarMatrix[starRow + nOfRows*starCol] = false;

        /* find primed zero in current row */
        primeRow = starRow;
        for (primeCol = 0; primeCol<nOfColumns; primeCol++)
            if (primeMatrix[primeRow + nOfRows*primeCol])
                break;

        /* star the primed zero */
        newStarMatrix[primeRow + nOfRows*primeCol] = true;

        /* find starred zero in current column */
        starCol = primeCol;
        for (starRow = 0; starRow<nOfRows; starRow++)
            if (starMatrix[starRow + nOfRows*starCol])
                break;
    }

    /* use temporary copy as new starMatrix */
    /* delete all primes, uncover all rows */
    for (n = 0; n<nOfElements; n++)
    {
        primeMatrix[n] = false;
        starMatrix[n] = newStarMatrix[n];
    }
    for (n = 0; n<nOfRows; n++)
        coveredRows[n] = false;

    /* move to step 2a */
    step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

/********************************************************/
void hungarian::step5(int *assignment, float *distMatrix, bool *starMatrix,
                      bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns,
                      bool *coveredRows, int nOfRows, int nOfColumns, int minDim)
{
    float h, value;
    int row, col;

    /* find smallest uncovered element h */
    h = DBL_MAX;
    for (row = 0; row<nOfRows; row++)
        if (!coveredRows[row])
            for (col = 0; col<nOfColumns; col++)
                if (!coveredColumns[col])
                {
                    value = distMatrix[row + nOfRows*col];
                    if (value < h)
                        h = value;
                }

    /* add h to each covered row */
    for (row = 0; row<nOfRows; row++)
        if (coveredRows[row])
            for (col = 0; col<nOfColumns; col++)
                distMatrix[row + nOfRows*col] += h;

    /* subtract h from each uncovered column */
    for (col = 0; col<nOfColumns; col++)
        if (!coveredColumns[col])
            for (row = 0; row<nOfRows; row++)
                distMatrix[row + nOfRows*col] -= h;

    /* move to step 3 */
    step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix,
          coveredColumns, coveredRows, nOfRows, nOfColumns, minDim);
}

kalmanboxtracker.h

#ifndef KALMANBOXTRACKER_H
#define KALMANBOXTRACKER_H

#include <iostream>
#include <opencv2/opencv.hpp>

class kalmanboxtracker
{
public:
    kalmanboxtracker(std::vector<int> bbox);
    void update(std::vector<int> bbox);
    std::vector<float> predict();
    std::vector<float> get_state();

public:

    cv::KalmanFilter* m_kf;
    int m_time_since_update;
    static int count;
    int m_id;
    std::vector<std::vector<float>> m_history;
    int m_hits;
    int m_hit_streak;
    int m_age;
};

#endif // KALMANBOXTRACKER_H

kalmanboxtracker.cpp

#include "kalmanboxtracker.h"
#include "utils.h"

int kalmanboxtracker::count = 0;

kalmanboxtracker::kalmanboxtracker(std::vector<int> bbox)
{
    m_kf = new cv::KalmanFilter(7, 4);

        m_kf->transitionMatrix = cv::Mat::eye(7, 7, CV_32F);
        m_kf->transitionMatrix.at<float>(0, 4) = 1;
        m_kf->transitionMatrix.at<float>(1, 5) = 1;
        m_kf->transitionMatrix.at<float>(2, 6) = 1;

        m_kf->measurementMatrix = cv::Mat::eye(4, 7, CV_32F);

        m_kf->measurementNoiseCov = cv::Mat::eye(4, 4, CV_32F);
        m_kf->measurementNoiseCov.at<float>(2, 2) *= 10;
        m_kf->measurementNoiseCov.at<float>(3, 3) *= 10;

        m_kf->errorCovPost = cv::Mat::eye(7, 7, CV_32F);
        m_kf->errorCovPost.at<float>(4, 4) *= 1000;
        m_kf->errorCovPost.at<float>(5, 5) *= 1000;
        m_kf->errorCovPost.at<float>(6, 6) *= 1000;
        m_kf->errorCovPost *= 10;

        m_kf->processNoiseCov = cv::Mat::eye(7, 7, CV_32F);
        m_kf->processNoiseCov.at<float>(4, 4) *= 0.01;
        m_kf->processNoiseCov.at<float>(5, 5) *= 0.01;
        m_kf->processNoiseCov.at<float>(6, 6) *= 0.001;

        m_kf->statePost = cv::Mat::eye(7, 1, CV_32F);
        m_kf->statePost.at<float>(0, 0) = convert_bbox_to_z(bbox)[0];
        m_kf->statePost.at<float>(1, 0) = convert_bbox_to_z(bbox)[1];
        m_kf->statePost.at<float>(2, 0) = convert_bbox_to_z(bbox)[2];
        m_kf->statePost.at<float>(3, 0) = convert_bbox_to_z(bbox)[3];

        m_time_since_update = 0;

        m_id = count;
        count++;

        m_history = {};

        m_hits = 0;

        m_hit_streak = 0;

        m_age = 0;

        //std::cout << m_kf->transitionMatrix << std::endl;
        //std::cout << m_kf->measurementMatrix << std::endl;
        //std::cout << m_kf->measurementNoiseCov << std::endl;
        //std::cout << m_kf->errorCovPost << std::endl;
        //std::cout << m_kf->processNoiseCov << std::endl;
        //std::cout << m_kf->statePost << std::endl;

}


void kalmanboxtracker::update(std::vector<int> bbox)
{
    m_time_since_update = 0;
    m_history = {};
    m_hits++;
    m_hit_streak++;
    cv::Mat measurement(4, 1, CV_32F);
    for (size_t i = 0; i < 4; i++)
        measurement.at<float>(i) = convert_bbox_to_z(bbox)[i];
    //std::cout << measurement << std::endl;
    m_kf->correct(measurement);
}


std::vector<float> kalmanboxtracker::predict()
{
    //std::cout << m_kf->statePost << std::endl;
    if (m_kf->statePost.at<float>(2, 0) + m_kf->statePost.at<float>(6, 0) <= 0)
        m_kf->statePost.at<float>(6, 0) = 0;
    m_kf->predict();
    m_age++;
    if (m_time_since_update > 0)
        m_hit_streak = 0;
    m_time_since_update++;
    m_history.push_back(convert_x_to_bbox(m_kf->statePost));
    return m_history.back();
}


std::vector<float> kalmanboxtracker::get_state()
{
    //std::cout << m_kf->statePost << std::endl;
    return convert_x_to_bbox(m_kf->statePost);
}

utils.h

#ifndef UTILS_H
#define UTILS_H

#include <iostream>
#include <numeric>
#include <opencv2/opencv.hpp>


/**
 * @brief draw_label 画检测框
 * @param input_image 输入图像
 * @param label 标签名称
 * @param left 标签距图像左侧距离
 * @param top 标签距图像上侧距离
 */
void draw_label(cv::Mat& input_image, std::string label, int left, int top);


/**
 * @brief get_center 计算检测框中心
 * @param detections 输入检测框
 */
std::vector<cv::Point> get_center(std::vector<cv::Rect> detections);


/**
 * @brief get_center 计算两点间距离
 * @param p1 点1
 * @param p2 点2
 */
float get_distance(cv::Point p1, cv::Point p2);


/**
 * @brief get_center 计算两检测框中心
 * @param p1 检测框1
 * @param p2 检测框2
 */
float get_center_distance(std::vector<float> bbox1, std::vector<float> bbox2);


/**
 * @brief convert_bbox_to_z 将检测框由[x1,y1,x2,y2]的形式转化为[x,y,s,r]的形式
 * @param bbox				检测框
 */
std::vector<float> convert_bbox_to_z(std::vector<int> bbox);


/**
 * @brief convert_x_to_bbox 将检测框由[x,y,s,r]的形式转化为[x1,y1,x2,y2]的形式
 * @param x					检测框
 */
std::vector<float> convert_x_to_bbox(std::vector<float> x);


/**
 * @brief iou	计算两检测框的iou
 * @param box1	检测框1
 * @param box2	检测框2
 */
float iou(std::vector<float> box1, std::vector<float> box2);


/**
 * @brief associate_detections_to_tracks	将检测结果和跟踪结果关联
 * @param detections						检测结果
 * @param trackers							跟踪结果
 * @param iou_threshold						iou矩阵
 */
std::tuple<std::vector<std::pair<int, int>>, std::vector<int>, std::vector<int>>

associate_detections_to_tracks(std::vector<cv::Rect> detections,
                               std::vector<std::vector<int>> trackers, float iou_threshold = 0.3);

#endif // UTILS_H

utils.cpp

#include "utils.h"
#include "hungarian.h"

void draw_label(cv::Mat& input_image, std::string label, int left, int top)
{
    int baseLine;
    cv::Size label_size = cv::getTextSize(label, 1, 1, 2, &baseLine);
    top = std::max(top, label_size.height);
    cv::Point tlc = cv::Point(left, top);
    cv::Point brc = cv::Point(left, top + label_size.height + baseLine);
    cv::putText(input_image, label, cv::Point(left, top + label_size.height), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 255), 1);
}


std::vector<cv::Point> get_center(std::vector<cv::Rect> detections)
{
    std::vector<cv::Point> detections_center(detections.size());
    for (size_t i = 0; i < detections.size(); i++)
    {
        detections_center[i] = cv::Point(detections[i].x + detections[i].width / 2, detections[i].y + detections[i].height / 2);
    }

    return detections_center;
}


float get_distance(cv::Point p1, cv::Point p2)
{
    return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}


float get_center_distance(std::vector<float> bbox1, std::vector<float> bbox2)
{
    float x1 = bbox1[0], x2 = bbox2[0];
    float y1 = bbox1[1], y2 = bbox2[1];
    float w1 = bbox1[2] - bbox1[0], w2 = bbox2[2] - bbox2[0];
    float h1 = bbox1[3] - bbox1[1], h2 = bbox2[3] - bbox2[1];
    cv::Point p1(x1 + w1 / 2, y1 + h1 / 2), p2(x2 + w2 / 2, y2 + h2 / 2);
    return get_distance(p1, p2);
}


std::vector<float> convert_bbox_to_z(std::vector<int> bbox)
{
    float w = bbox[2] - bbox[0];
    float h = bbox[3] - bbox[1];
    float x = bbox[0] + w / 2;
    float y = bbox[1] + h / 2;
    float s = w * h;
    float r = w / h;

    return { x, y, s, r };
}


std::vector<float> convert_x_to_bbox(std::vector<float> x)
{
    float w = sqrt(x[2] * x[3]);
    float h = x[2] / w;
    return { x[0] - w / 2, x[1] - h / 2, x[0] + w / 2, x[1] + h / 2 };
}


float iou(std::vector<int> box1, std::vector<int> box2)
{
    int x1 = std::max(box1[0], box2[0]);
    int y1 = std::max(box1[1], box2[1]);
    int x2 = std::min(box1[2], box2[2]);
    int y2 = std::min(box1[3], box2[3]);
    int w = std::max(0, x2 - x1);
    int h = std::max(0, y2 - y1);
    int inter_area = w * h;
    float iou = inter_area * 1.0 / ((box1[2] - box1[0]) * (box1[3] - box1[1]) + (box2[2] - box2[0]) * (box2[3] - box2[1]) - inter_area);

    return iou;
}


template <typename T>
std::vector<size_t> sort_indices(const std::vector<T>& v)
{
    std::vector<size_t> idx(v.size());
    std::iota(idx.begin(), idx.end(), 0);
    std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) { return v[i1] < v[i2]; });
    return idx;
}


std::vector<std::vector<int>> linear_assignment(cv::Mat iou_matrix)
{
    std::vector<std::vector<float>> costMatrix(iou_matrix.cols, std::vector<float>(iou_matrix.rows));
    for (size_t i = 0; i < iou_matrix.cols; i++)
        for (size_t j = 0; j < iou_matrix.rows; j++)
            costMatrix[i][j] = iou_matrix.at<float>(j, i);

    hungarian HungAlgo;
    std::vector<int> assignment;
    HungAlgo.Solve(costMatrix, assignment);

    std::vector<std::vector<int>> tmp(2);
    for (size_t i = 0; i < assignment.size(); i++)
    {
        if (assignment[i] >= 0)
        {
            tmp[0].push_back(assignment[i]);
            tmp[1].push_back(i);
        }
    }

    std::vector<size_t> indices = sort_indices(tmp[0]);
    std::sort(tmp[0].begin(), tmp[0].end());

    std::vector<std::vector<int>> ret(2);
    ret[0] = tmp[0];
    for (size_t i = 0; i < ret[0].size(); i++)
        ret[1].push_back(tmp[1][indices[i]]);

    return ret;
}


std::tuple<std::vector<std::pair<int, int>>, std::vector<int>, std::vector<int>>
associate_detections_to_tracks(std::vector<cv::Rect> detections, std::vector<std::vector<int>> trackers, float iou_threshold)
{
    std::vector<int> unmatched_trackers;
    std::vector<std::pair<int, int>> matches;

    if (trackers.size() == 0)
    {
        std::vector<int> unmatched_detections(detections.size());
        std::iota(unmatched_detections.begin(), unmatched_detections.end(), 0);
        //return { {}, unmatched_detections, {} };
        return {matches, unmatched_detections, unmatched_trackers};
    }

    cv::Mat iou_matrix(detections.size(), trackers.size(), CV_32F);
    for (size_t i = 0; i < iou_matrix.rows; i++)
    {
        for (size_t j = 0; j < iou_matrix.cols; j++)
        {
            std::vector<int> detection{ detections[i].x, detections[i].y, detections[i].x + detections[i].width, detections[i].y + detections[i].height };
            std::vector<int> tracker = trackers[j];
            iou_matrix.at<float>(i, j) = iou(detection, tracker);
        }
    }
    //std::cout << iou_matrix << std::endl;

    std::vector<std::vector<int>> matched_indices(2);
    if (std::min(iou_matrix.rows, iou_matrix.cols) > 0)
    {
        cv::Mat a(iou_matrix.rows, iou_matrix.cols, CV_32F, cv::Scalar(0));
        for (size_t i = 0; i < a.rows; i++)
        {
            for (size_t j = 0; j < a.cols; j++)
            {
                if (iou_matrix.at<float>(i, j) > iou_threshold)
                    a.at<float>(i, j) = 1;
            }
        }
        //std::cout << a << std::endl;

        cv::Mat a_sum0(iou_matrix.cols, 1, CV_32F, cv::Scalar(0));
        cv::reduce(a, a_sum0, 0, cv::REDUCE_SUM);
        std::vector<float> sum0(iou_matrix.cols);
        for (size_t i = 0; i < sum0.size(); i++)
            sum0[i] = a_sum0.at<float>(0, i);
        float a_sum0_max = *std::max_element(sum0.begin(), sum0.end());

        cv::Mat a_sum1(1, iou_matrix.rows, CV_32F, cv::Scalar(0));
        cv::reduce(a, a_sum1, 1, cv::REDUCE_SUM);
        std::vector<float> sum1(iou_matrix.rows);
        for (size_t i = 0; i < sum1.size(); i++)
            sum1[i] = a_sum1.at<float>(i, 0);
        float a_sum1_max = *std::max_element(sum1.begin(), sum1.end());

        if (int(a_sum0_max) == 1 && int(a_sum1_max) == 1)
        {
            std::vector<cv::Point> nonZeroCoordinates;
            cv::findNonZero(a, nonZeroCoordinates);
            std::sort(nonZeroCoordinates.begin(), nonZeroCoordinates.end(), [](cv::Point p1, cv::Point p2) {return p1.y < p2.y; });
            for (int i = 0; i < nonZeroCoordinates.size(); i++)
            {
                matched_indices[0].push_back(nonZeroCoordinates[i].y);
                matched_indices[1].push_back(nonZeroCoordinates[i].x);
            }
        }
        else
        {
            matched_indices = linear_assignment(-iou_matrix);
        }
    }

    std::vector<int> unmatched_detections;
    for (size_t i = 0; i < detections.size(); i++)
    {
        if (std::find(matched_indices[0].begin(), matched_indices[0].end(), i) == matched_indices[0].end())
            unmatched_detections.push_back(i);
    }

    //std::vector<int> unmatched_trackers;
    for (size_t i = 0; i < trackers.size(); i++)
    {
        if (std::find(matched_indices[1].begin(), matched_indices[1].end(), i) == matched_indices[1].end())
            unmatched_trackers.push_back(i);
    }

    //std::vector<std::pair<int, int>> matches;
    for (size_t i = 0; i < matched_indices[0].size(); i++)
    {
        //std::cout << matched_indices[0][i] << " " << matched_indices[1][i] << std::endl;
        if (iou_matrix.at<float>(matched_indices[0][i], matched_indices[1][i]) < iou_threshold)
        {
            unmatched_detections.push_back(matched_indices[0][i]);
            unmatched_trackers.push_back(matched_indices[1][i]);
        }
        else
            matches.push_back({ matched_indices[0][i], matched_indices[1][i] });
    }

    return { matches, unmatched_detections, unmatched_trackers };
}

sort.h

#ifndef SORT_H
#define SORT_H


#include <iostream>
#include <opencv2/opencv.hpp>

#include "utils.h"
#include "kalmanboxtracker.h"

class sort
{
public:
    /**
     * @brief Sort 			构造函数
     * @param max_age		未命中时间上限
     * @param min_hits		未命中时间下限
     * @param iou_threshold iou阈值
     */
    sort(int max_age, int min_hits, float iou_threshold);

    /**
     * @brief update	更新检测框
     * @param dets		检测框
     */
    std::vector<std::vector<float>> update(std::vector<cv::Rect> dets);

private:
    /**
     * @param m_max_age		未命中时间上限
     */
    int m_max_age;

    /**
     * @param m_min_hits	未命中时间下限
     */
    int m_min_hits;

    /**
     * @param m_iou_threshold	iou阈值
     */
    float m_iou_threshold;

    /**
     * @param m_trackers	卡尔曼跟踪器的集合
     */
    std::vector<kalmanboxtracker> m_trackers;

    /**
     * @param m_frame_count	帧数
     */
    int m_frame_count;
};

#endif // SORT_H

sort.cpp

#include "sort.h"
#include <iostream>
#include <opencv2/opencv.hpp>

#include "utils.h"
#include "kalmanboxtracker.h"
#include "sort.h"

/*
max_age：每个track可以存活的帧数
min_hits：最小击中帧数
frame_count：总的帧数，也就是第几帧*/
sort::sort(int max_age = 1, int min_hits = 3, float iou_threshold = 0.3)
{
    m_max_age = max_age;
    m_min_hits = min_hits;
    m_iou_threshold = iou_threshold;
    m_trackers = {};
    m_frame_count = 0;
}


std::vector<std::vector<float>> sort::update(std::vector<cv::Rect> dets)
{
    m_frame_count++;
    std::vector<std::vector<int>> trks(m_trackers.size(), std::vector<int>(5, 0));
    std::vector<int> to_del;
    std::vector<std::vector<float>> ret;

    for (size_t i = 0; i < trks.size(); i++)
    {
        std::vector<float> pos = m_trackers[i].predict();
        std::vector<int> trk{ (int)pos[0],  (int)pos[1], (int)pos[2], (int)pos[3], 0 };
        trks[i] = trk;
    }

    for (int i = to_del.size() - 1; i >= 0; i--)
        m_trackers.erase(m_trackers.begin() + i);

    auto [matched, unmatched_dets, unmatched_trks] = associate_detections_to_tracks(dets, trks, m_iou_threshold);

    for (size_t i = 0; i < matched.size(); i++)
    {
        int id = matched[i].first;
        std::vector<int> vec{ dets[id].x, dets[id].y, dets[id].x + dets[id].width, dets[id].y + dets[id].height };
        m_trackers[matched[i].second].update(vec);
    }

    for (auto i : unmatched_dets)
    {
        std::vector<int> vec{ dets[i].x, dets[i].y, dets[i].x + dets[i].width, dets[i].y + dets[i].height };
        kalmanboxtracker trk(vec);
        m_trackers.push_back(trk);
    }
    int n = m_trackers.size();

    for (int i = m_trackers.size() - 1; i >= 0; i--)
    {
        auto trk = m_trackers[i];
        //std::cout << KalmanBoxTracker::count << std::endl;
        std::vector<float> d = trk.get_state();
        if ((trk.m_time_since_update < 1) && (trk.m_hit_streak >= m_min_hits || m_frame_count <= m_min_hits))
        {
            std::vector<float> tmp = d;
            tmp.push_back(trk.m_id + 1);
            ret.push_back(tmp);
        }
        n--;
        if (trk.m_time_since_update > m_max_age)
            m_trackers.erase(m_trackers.begin() + n);
    }

    if (ret.size() > 0)
        return ret;

    return {};
}

sort *mot_tracker;
std::vector<cv::Vec3b> colors;

mot_tracker = new sort(3, 3, 0.25);


myyolo->Infer(tempmat.cols, tempmat.rows, tempmat.channels(), tempmat.data, Boxes, ClassIndexs, BboxNum,Scores);


std::vector<cv::Rect> detections;

       for (int i =0; i< newBboxNum[0]; i++){

           cv::Rect rect(newBoxes[i * 4], newBoxes[i * 4 + 1], newBoxes[i * 4 + 2], newBoxes[i * 4 + 3]);
           //cv::rectangle(tempmat, rect, cv::Scalar(0x27, 0xC1, 0x36), 2);
           detections.push_back(rect);
       }

       std::vector<std::vector<float>> trackers = mot_tracker->update(detections);
       //qDebug()<<trackers;

       if(!trackers.empty()){
            //std::vector<cv::Vec3b> colors(32);
            //srand(time(0));

            //int size_row = trackers.size();
            //int size_col = trackers[0].size();
            for (auto tracker : trackers)
            {
                cv::rectangle(tempmat, cv::Rect(tracker[0], tracker[1],
                            tracker[2]- tracker[0], tracker[3]- tracker[1]),
                            colors[int(tracker[4])%100], 2);
                cv::putText(tempmat,std::to_string(int(tracker[4])),
                cv::Point(tracker[0], tracker[1]),
                cv::FONT_HERSHEY_PLAIN,1.5,
                colors[int(tracker[4])%100],
                2);
            }
       }


       QImage outimg = Mat2QImage(tempmat);