sort demo

hungarian.h

#ifndef HUNGARIAN_H
#define HUNGARIAN_H

#include 
#include 

class hungarian
{
public:
    hungarian();
    ~hungarian();
    float Solve(std::vector>& DistMatrix, std::vector& 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 
#include  // for DBL_MAX
#include   // for fabs()

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


//********************************************************//
// A single function wrapper for solving assignment problem.
//********************************************************//
float hungarian::Solve(std::vector >& DistMatrix, std::vector& 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 nOfColumns) */
    {
        minDim = nOfColumns;

        for (col = 0; 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

kalmanboxtracker.h

#ifndef KALMANBOXTRACKER_H
#define KALMANBOXTRACKER_H

#include 
#include 

class kalmanboxtracker
{
public:
    kalmanboxtracker(std::vector bbox);
    void update(std::vector bbox);
    std::vector predict();
    std::vector get_state();

public:

    cv::KalmanFilter* m_kf;
    int m_time_since_update;
    static int count;
    int m_id;
    std::vector> 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 bbox)
{
    m_kf = new cv::KalmanFilter(7, 4);

        m_kf->transitionMatrix = cv::Mat::eye(7, 7, CV_32F);
        m_kf->transitionMatrix.at(0, 4) = 1;
        m_kf->transitionMatrix.at(1, 5) = 1;
        m_kf->transitionMatrix.at(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(2, 2) *= 10;
        m_kf->measurementNoiseCov.at(3, 3) *= 10;

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

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

        m_kf->statePost = cv::Mat::eye(7, 1, CV_32F);
        m_kf->statePost.at(0, 0) = convert_bbox_to_z(bbox)[0];
        m_kf->statePost.at(1, 0) = convert_bbox_to_z(bbox)[1];
        m_kf->statePost.at(2, 0) = convert_bbox_to_z(bbox)[2];
        m_kf->statePost.at(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 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(i) = convert_bbox_to_z(bbox)[i];
    //std::cout << measurement << std::endl;
    m_kf->correct(measurement);
}


std::vector kalmanboxtracker::predict()
{
    //std::cout << m_kf->statePost << std::endl;
    if (m_kf->statePost.at(2, 0) + m_kf->statePost.at(6, 0) <= 0)
        m_kf->statePost.at(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 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 
#include 
#include 


/**
 * @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 get_center(std::vector 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 bbox1, std::vector bbox2);


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


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


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


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

associate_detections_to_tracks(std::vector detections,
                               std::vector> 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 get_center(std::vector detections)
{
    std::vector 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 bbox1, std::vector 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 convert_bbox_to_z(std::vector 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 convert_x_to_bbox(std::vector 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 box1, std::vector 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 
std::vector sort_indices(const std::vector& v)
{
    std::vector 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> linear_assignment(cv::Mat iou_matrix)
{
    std::vector> costMatrix(iou_matrix.cols, std::vector(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(j, i);

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

    std::vector> 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 indices = sort_indices(tmp[0]);
    std::sort(tmp[0].begin(), tmp[0].end());

    std::vector> 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::vector>
associate_detections_to_tracks(std::vector detections, std::vector> trackers, float iou_threshold)
{
    std::vector unmatched_trackers;
    std::vector> matches;

    if (trackers.size() == 0)
    {
        std::vector 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 detection{ detections[i].x, detections[i].y, detections[i].x + detections[i].width, detections[i].y + detections[i].height };
            std::vector tracker = trackers[j];
            iou_matrix.at(i, j) = iou(detection, tracker);
        }
    }
    //std::cout << iou_matrix << std::endl;

    std::vector> 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(i, j) > iou_threshold)
                    a.at(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 sum0(iou_matrix.cols);
        for (size_t i = 0; i < sum0.size(); i++)
            sum0[i] = a_sum0.at(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 sum1(iou_matrix.rows);
        for (size_t i = 0; i < sum1.size(); i++)
            sum1[i] = a_sum1.at(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 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 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 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> 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(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 
#include 

#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> update(std::vector 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 m_trackers;

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

#endif // SORT_H

sort.cpp

#include "sort.h"
#include 
#include 

#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> sort::update(std::vector dets)
{
    m_frame_count++;
    std::vector> trks(m_trackers.size(), std::vector(5, 0));
    std::vector to_del;
    std::vector> ret;

    for (size_t i = 0; i < trks.size(); i++)
    {
        std::vector pos = m_trackers[i].predict();
        std::vector 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 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 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 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 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 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 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> trackers = mot_tracker->update(detections);
       //qDebug()< 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);