From fbc8a9b47bca84382bc0d0d5cb843647ecc950b0 Mon Sep 17 00:00:00 2001 From: Frank Dana Date: Tue, 13 Apr 2021 12:19:50 +0000 Subject: [PATCH] Normalize repo to LF (Unix) line-endings, enforce (#654) - A top-level .gitattributes sets the attribute 'text=auto' for all files in the repo, enabling line-ending normalization - A `git add --renormalize` run converted any existing CRLF files to the canonical LF format. - 'text=auto' permits Git on Windows to convert files to CRLF automatically on checkout, for convenience. Any commits added will be converted back to the repo format for checkin. --- .gitattributes | 2 + src/sort_filter/Hungarian.cpp | 908 +++++++++++++++--------------- src/sort_filter/Hungarian.h | 68 +-- src/sort_filter/KalmanTracker.cpp | 236 ++++---- src/sort_filter/KalmanTracker.h | 122 ++-- src/sort_filter/sort.cpp | 422 +++++++------- src/sort_filter/sort.hpp | 122 ++-- 7 files changed, 941 insertions(+), 939 deletions(-) create mode 100644 .gitattributes diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 00000000..032c1bd1 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,2 @@ +* text=auto + diff --git a/src/sort_filter/Hungarian.cpp b/src/sort_filter/Hungarian.cpp index 6442d2da..9eef6139 100644 --- a/src/sort_filter/Hungarian.cpp +++ b/src/sort_filter/Hungarian.cpp @@ -1,454 +1,454 @@ -/////////////////////////////////////////////////////////////////////////////// -// Hungarian.cpp: Implementation file for Class HungarianAlgorithm. -// -// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren. -// The original implementation is a few mex-functions for use in MATLAB, found here: -// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem -// -// Both this code and the orignal code are published under the BSD license. -// by Cong Ma, 2016 -// - -#include "Hungarian.h" - -using namespace std; - -HungarianAlgorithm::HungarianAlgorithm() {} -HungarianAlgorithm::~HungarianAlgorithm() {} - -//********************************************************// -// A single function wrapper for solving assignment problem. -//********************************************************// -double HungarianAlgorithm::Solve( - vector> &DistMatrix, - vector &Assignment) -{ - unsigned int nRows = DistMatrix.size(); - unsigned int nCols = DistMatrix[0].size(); - - double *distMatrixIn = new double[nRows * nCols]; - int *assignment = new int[nRows]; - double 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 double 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 HungarianAlgorithm::assignmentoptimal( - int *assignment, - double *cost, - double *distMatrixIn, - int nOfRows, - int nOfColumns) -{ - double *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 = (double *)malloc(nOfElements * sizeof(double)); - distMatrixEnd = distMatrix + nOfElements; - - for (row = 0; row < nOfElements; row++) - { - value = distMatrixIn[row]; - if (value < 0) - cerr << "All matrix elements have to be non-negative." << endl; - distMatrix[row] = value; - } - - /* 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 HungarianAlgorithm::buildassignmentvector( - int *assignment, - bool *starMatrix, - int nOfRows, - int nOfColumns) -{ - for (int row = 0; row < nOfRows; row++) - for (int 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 HungarianAlgorithm::computeassignmentcost( - int *assignment, - double *cost, - double *distMatrix, - int nOfRows) -{ - int row, col; - - for (row = 0; row < nOfRows; row++) - { - col = assignment[row]; - if (col >= 0) - *cost += distMatrix[row + nOfRows * col]; - } -} - -/********************************************************/ -void HungarianAlgorithm::step2a( - int *assignment, - double *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 HungarianAlgorithm::step2b( - int *assignment, - double *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 HungarianAlgorithm::step3( - int *assignment, - double *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 HungarianAlgorithm::step4( - int *assignment, - double *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 HungarianAlgorithm::step5( - int *assignment, - double *distMatrix, - bool *starMatrix, - bool *newStarMatrix, - bool *primeMatrix, - bool *coveredColumns, - bool *coveredRows, - int nOfRows, - int nOfColumns, - int minDim) -{ - double 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); -} +/////////////////////////////////////////////////////////////////////////////// +// Hungarian.cpp: Implementation file for Class HungarianAlgorithm. +// +// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren. +// The original implementation is a few mex-functions for use in MATLAB, found here: +// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem +// +// Both this code and the orignal code are published under the BSD license. +// by Cong Ma, 2016 +// + +#include "Hungarian.h" + +using namespace std; + +HungarianAlgorithm::HungarianAlgorithm() {} +HungarianAlgorithm::~HungarianAlgorithm() {} + +//********************************************************// +// A single function wrapper for solving assignment problem. +//********************************************************// +double HungarianAlgorithm::Solve( + vector> &DistMatrix, + vector &Assignment) +{ + unsigned int nRows = DistMatrix.size(); + unsigned int nCols = DistMatrix[0].size(); + + double *distMatrixIn = new double[nRows * nCols]; + int *assignment = new int[nRows]; + double 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 double 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 HungarianAlgorithm::assignmentoptimal( + int *assignment, + double *cost, + double *distMatrixIn, + int nOfRows, + int nOfColumns) +{ + double *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 = (double *)malloc(nOfElements * sizeof(double)); + distMatrixEnd = distMatrix + nOfElements; + + for (row = 0; row < nOfElements; row++) + { + value = distMatrixIn[row]; + if (value < 0) + cerr << "All matrix elements have to be non-negative." << endl; + distMatrix[row] = value; + } + + /* 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 HungarianAlgorithm::buildassignmentvector( + int *assignment, + bool *starMatrix, + int nOfRows, + int nOfColumns) +{ + for (int row = 0; row < nOfRows; row++) + for (int 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 HungarianAlgorithm::computeassignmentcost( + int *assignment, + double *cost, + double *distMatrix, + int nOfRows) +{ + int row, col; + + for (row = 0; row < nOfRows; row++) + { + col = assignment[row]; + if (col >= 0) + *cost += distMatrix[row + nOfRows * col]; + } +} + +/********************************************************/ +void HungarianAlgorithm::step2a( + int *assignment, + double *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 HungarianAlgorithm::step2b( + int *assignment, + double *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 HungarianAlgorithm::step3( + int *assignment, + double *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 HungarianAlgorithm::step4( + int *assignment, + double *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 HungarianAlgorithm::step5( + int *assignment, + double *distMatrix, + bool *starMatrix, + bool *newStarMatrix, + bool *primeMatrix, + bool *coveredColumns, + bool *coveredRows, + int nOfRows, + int nOfColumns, + int minDim) +{ + double 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); +} diff --git a/src/sort_filter/Hungarian.h b/src/sort_filter/Hungarian.h index 3bd834e7..ae59d06e 100644 --- a/src/sort_filter/Hungarian.h +++ b/src/sort_filter/Hungarian.h @@ -1,34 +1,34 @@ -/////////////////////////////////////////////////////////////////////////////// -// Hungarian.h: Header file for Class HungarianAlgorithm. -// -// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren. -// The original implementation is a few mex-functions for use in MATLAB, found here: -// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem -// -// Both this code and the orignal code are published under the BSD license. -// by Cong Ma, 2016 -// - -#include -#include -#include -#include -#include - -class HungarianAlgorithm -{ -public: - HungarianAlgorithm(); - ~HungarianAlgorithm(); - double Solve(std::vector> &DistMatrix, std::vector &Assignment); - -private: - void assignmentoptimal(int *assignment, double *cost, double *distMatrix, int nOfRows, int nOfColumns); - void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns); - void computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows); - void step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); - void step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); - void step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); - void step4(int *assignment, double *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, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); -}; +/////////////////////////////////////////////////////////////////////////////// +// Hungarian.h: Header file for Class HungarianAlgorithm. +// +// This is a C++ wrapper with slight modification of a hungarian algorithm implementation by Markus Buehren. +// The original implementation is a few mex-functions for use in MATLAB, found here: +// http://www.mathworks.com/matlabcentral/fileexchange/6543-functions-for-the-rectangular-assignment-problem +// +// Both this code and the orignal code are published under the BSD license. +// by Cong Ma, 2016 +// + +#include +#include +#include +#include +#include + +class HungarianAlgorithm +{ +public: + HungarianAlgorithm(); + ~HungarianAlgorithm(); + double Solve(std::vector> &DistMatrix, std::vector &Assignment); + +private: + void assignmentoptimal(int *assignment, double *cost, double *distMatrix, int nOfRows, int nOfColumns); + void buildassignmentvector(int *assignment, bool *starMatrix, int nOfRows, int nOfColumns); + void computeassignmentcost(int *assignment, double *cost, double *distMatrix, int nOfRows); + void step2a(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); + void step2b(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); + void step3(int *assignment, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); + void step4(int *assignment, double *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, double *distMatrix, bool *starMatrix, bool *newStarMatrix, bool *primeMatrix, bool *coveredColumns, bool *coveredRows, int nOfRows, int nOfColumns, int minDim); +}; diff --git a/src/sort_filter/KalmanTracker.cpp b/src/sort_filter/KalmanTracker.cpp index bb8519c2..01923962 100644 --- a/src/sort_filter/KalmanTracker.cpp +++ b/src/sort_filter/KalmanTracker.cpp @@ -1,118 +1,118 @@ -/////////////////////////////////////////////////////////////////////////////// -// KalmanTracker.cpp: KalmanTracker Class Implementation Declaration - -#include "KalmanTracker.h" -#include - -using namespace std; -using namespace cv; - -int KalmanTracker::kf_count = 0; - -// initialize Kalman filter -void KalmanTracker::init_kf( - StateType stateMat) -{ - int stateNum = 7; - int measureNum = 4; - kf = KalmanFilter(stateNum, measureNum, 0); - - measurement = Mat::zeros(measureNum, 1, CV_32F); - - kf.transitionMatrix = (Mat_(7, 7) << 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); - - setIdentity(kf.measurementMatrix); - setIdentity(kf.processNoiseCov, Scalar::all(1e-1)); - setIdentity(kf.measurementNoiseCov, Scalar::all(1e-4)); - setIdentity(kf.errorCovPost, Scalar::all(1e-2)); - - // initialize state vector with bounding box in [cx,cy,s,r] style - kf.statePost.at(0, 0) = stateMat.x + stateMat.width / 2; - kf.statePost.at(1, 0) = stateMat.y + stateMat.height / 2; - kf.statePost.at(2, 0) = stateMat.area(); - kf.statePost.at(3, 0) = stateMat.width / stateMat.height; -} - -// Predict the estimated bounding box. -StateType KalmanTracker::predict() -{ - // predict - Mat p = kf.predict(); - m_age += 1; - - if (m_time_since_update > 0) - m_hit_streak = 0; - m_time_since_update += 1; - - StateType predictBox = get_rect_xysr(p.at(0, 0), p.at(1, 0), p.at(2, 0), p.at(3, 0)); - - m_history.push_back(predictBox); - return m_history.back(); -} - -StateType KalmanTracker::predict2() -{ - // predict - Mat p = kf.predict(); - - StateType predictBox = get_rect_xysr(p.at(0, 0), p.at(1, 0), p.at(2, 0), p.at(3, 0)); - - return predictBox; -} - -// Update the state vector with observed bounding box. -void KalmanTracker::update( - StateType stateMat) -{ - m_time_since_update = 0; - m_history.clear(); - m_hits += 1; - m_hit_streak += 1; - - // measurement - measurement.at(0, 0) = stateMat.x + stateMat.width / 2; - measurement.at(1, 0) = stateMat.y + stateMat.height / 2; - measurement.at(2, 0) = stateMat.area(); - measurement.at(3, 0) = stateMat.width / stateMat.height; - - // update - kf.correct(measurement); - // time_t now = time(0); - // convert now to string form - - // detect_times.push_back(dt); -} - -// Return the current state vector -StateType KalmanTracker::get_state() -{ - Mat s = kf.statePost; - return get_rect_xysr(s.at(0, 0), s.at(1, 0), s.at(2, 0), s.at(3, 0)); -} - -// Convert bounding box from [cx,cy,s,r] to [x,y,w,h] style. -StateType KalmanTracker::get_rect_xysr( - float cx, - float cy, - float s, - float r) -{ - float w = sqrt(s * r); - float h = s / w; - float x = (cx - w / 2); - float y = (cy - h / 2); - - if (x < 0 && cx > 0) - x = 0; - if (y < 0 && cy > 0) - y = 0; - - return StateType(x, y, w, h); -} +/////////////////////////////////////////////////////////////////////////////// +// KalmanTracker.cpp: KalmanTracker Class Implementation Declaration + +#include "KalmanTracker.h" +#include + +using namespace std; +using namespace cv; + +int KalmanTracker::kf_count = 0; + +// initialize Kalman filter +void KalmanTracker::init_kf( + StateType stateMat) +{ + int stateNum = 7; + int measureNum = 4; + kf = KalmanFilter(stateNum, measureNum, 0); + + measurement = Mat::zeros(measureNum, 1, CV_32F); + + kf.transitionMatrix = (Mat_(7, 7) << 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); + + setIdentity(kf.measurementMatrix); + setIdentity(kf.processNoiseCov, Scalar::all(1e-1)); + setIdentity(kf.measurementNoiseCov, Scalar::all(1e-4)); + setIdentity(kf.errorCovPost, Scalar::all(1e-2)); + + // initialize state vector with bounding box in [cx,cy,s,r] style + kf.statePost.at(0, 0) = stateMat.x + stateMat.width / 2; + kf.statePost.at(1, 0) = stateMat.y + stateMat.height / 2; + kf.statePost.at(2, 0) = stateMat.area(); + kf.statePost.at(3, 0) = stateMat.width / stateMat.height; +} + +// Predict the estimated bounding box. +StateType KalmanTracker::predict() +{ + // predict + Mat p = kf.predict(); + m_age += 1; + + if (m_time_since_update > 0) + m_hit_streak = 0; + m_time_since_update += 1; + + StateType predictBox = get_rect_xysr(p.at(0, 0), p.at(1, 0), p.at(2, 0), p.at(3, 0)); + + m_history.push_back(predictBox); + return m_history.back(); +} + +StateType KalmanTracker::predict2() +{ + // predict + Mat p = kf.predict(); + + StateType predictBox = get_rect_xysr(p.at(0, 0), p.at(1, 0), p.at(2, 0), p.at(3, 0)); + + return predictBox; +} + +// Update the state vector with observed bounding box. +void KalmanTracker::update( + StateType stateMat) +{ + m_time_since_update = 0; + m_history.clear(); + m_hits += 1; + m_hit_streak += 1; + + // measurement + measurement.at(0, 0) = stateMat.x + stateMat.width / 2; + measurement.at(1, 0) = stateMat.y + stateMat.height / 2; + measurement.at(2, 0) = stateMat.area(); + measurement.at(3, 0) = stateMat.width / stateMat.height; + + // update + kf.correct(measurement); + // time_t now = time(0); + // convert now to string form + + // detect_times.push_back(dt); +} + +// Return the current state vector +StateType KalmanTracker::get_state() +{ + Mat s = kf.statePost; + return get_rect_xysr(s.at(0, 0), s.at(1, 0), s.at(2, 0), s.at(3, 0)); +} + +// Convert bounding box from [cx,cy,s,r] to [x,y,w,h] style. +StateType KalmanTracker::get_rect_xysr( + float cx, + float cy, + float s, + float r) +{ + float w = sqrt(s * r); + float h = s / w; + float x = (cx - w / 2); + float y = (cy - h / 2); + + if (x < 0 && cx > 0) + x = 0; + if (y < 0 && cy > 0) + y = 0; + + return StateType(x, y, w, h); +} diff --git a/src/sort_filter/KalmanTracker.h b/src/sort_filter/KalmanTracker.h index 2894fe47..f08683df 100644 --- a/src/sort_filter/KalmanTracker.h +++ b/src/sort_filter/KalmanTracker.h @@ -1,12 +1,12 @@ -/////////////////////////////////////////////////////////////////////////////// -// KalmanTracker.h: KalmanTracker Class Declaration - -#ifndef KALMAN_H -#define KALMAN_H 2 - -#include "opencv2/video/tracking.hpp" -#include "opencv2/highgui/highgui.hpp" - +/////////////////////////////////////////////////////////////////////////////// +// KalmanTracker.h: KalmanTracker Class Declaration + +#ifndef KALMAN_H +#define KALMAN_H 2 + +#include "opencv2/video/tracking.hpp" +#include "opencv2/highgui/highgui.hpp" + #define StateType cv::Rect_ @@ -14,56 +14,56 @@ class KalmanTracker { public: - KalmanTracker() - { - init_kf(StateType()); - m_time_since_update = 0; - m_hits = 0; - m_hit_streak = 0; - m_age = 0; - m_id = kf_count; - //kf_count++; - } - KalmanTracker(StateType initRect, float confidence, int classId) : confidence(confidence), classId(classId) - { - init_kf(initRect); - m_time_since_update = 0; - m_hits = 0; - m_hit_streak = 0; - m_age = 0; - m_id = kf_count; - kf_count++; - } - - ~KalmanTracker() - { - m_history.clear(); - } - - StateType predict(); - StateType predict2(); - void update(StateType stateMat); - - StateType get_state(); - StateType get_rect_xysr(float cx, float cy, float s, float r); - - static int kf_count; - - int m_time_since_update; - int m_hits; - int m_hit_streak; - int m_age; - int m_id; - float confidence; - int classId; - -private: - void init_kf(StateType stateMat); - - cv::KalmanFilter kf; - cv::Mat measurement; - - std::vector m_history; -}; - + KalmanTracker() + { + init_kf(StateType()); + m_time_since_update = 0; + m_hits = 0; + m_hit_streak = 0; + m_age = 0; + m_id = kf_count; + //kf_count++; + } + KalmanTracker(StateType initRect, float confidence, int classId) : confidence(confidence), classId(classId) + { + init_kf(initRect); + m_time_since_update = 0; + m_hits = 0; + m_hit_streak = 0; + m_age = 0; + m_id = kf_count; + kf_count++; + } + + ~KalmanTracker() + { + m_history.clear(); + } + + StateType predict(); + StateType predict2(); + void update(StateType stateMat); + + StateType get_state(); + StateType get_rect_xysr(float cx, float cy, float s, float r); + + static int kf_count; + + int m_time_since_update; + int m_hits; + int m_hit_streak; + int m_age; + int m_id; + float confidence; + int classId; + +private: + void init_kf(StateType stateMat); + + cv::KalmanFilter kf; + cv::Mat measurement; + + std::vector m_history; +}; + #endif \ No newline at end of file diff --git a/src/sort_filter/sort.cpp b/src/sort_filter/sort.cpp index 186a2825..b65d01a4 100644 --- a/src/sort_filter/sort.cpp +++ b/src/sort_filter/sort.cpp @@ -1,211 +1,211 @@ -#include "sort.hpp" - -using namespace std; - -// Constructor -SortTracker::SortTracker(int max_age, int min_hits) -{ - _min_hits = min_hits; - _max_age = max_age; - alive_tracker = true; -} - -// Computes IOU between two bounding boxes -double SortTracker::GetIOU(cv::Rect_ bb_test, cv::Rect_ bb_gt) -{ - float in = (bb_test & bb_gt).area(); - float un = bb_test.area() + bb_gt.area() - in; - - if (un < DBL_EPSILON) - return 0; - - return (double)(in / un); -} - -// Computes centroid distance between two bounding boxes -double SortTracker::GetCentroidsDistance( - cv::Rect_ bb_test, - cv::Rect_ bb_gt) -{ - float bb_test_centroid_x = (bb_test.x + bb_test.width / 2); - float bb_test_centroid_y = (bb_test.y + bb_test.height / 2); - - float bb_gt_centroid_x = (bb_gt.x + bb_gt.width / 2); - float bb_gt_centroid_y = (bb_gt.y + bb_gt.height / 2); - - double distance = (double)sqrt(pow(bb_gt_centroid_x - bb_test_centroid_x, 2) + pow(bb_gt_centroid_y - bb_test_centroid_y, 2)); - - return distance; -} - -void SortTracker::update(vector detections_cv, int frame_count, double image_diagonal, std::vector confidences, std::vector classIds) -{ - vector detections; - if (trackers.size() == 0) // the first frame met - { - alive_tracker = false; - // initialize kalman trackers using first detections. - for (unsigned int i = 0; i < detections_cv.size(); i++) - { - TrackingBox tb; - - tb.box = cv::Rect_(detections_cv[i]); - tb.classId = classIds[i]; - tb.confidence = confidences[i]; - detections.push_back(tb); - - KalmanTracker trk = KalmanTracker(detections[i].box, detections[i].confidence, detections[i].classId); - trackers.push_back(trk); - } - return; - } - else - { - for (unsigned int i = 0; i < detections_cv.size(); i++) - { - TrackingBox tb; - tb.box = cv::Rect_(detections_cv[i]); - tb.classId = classIds[i]; - tb.confidence = confidences[i]; - detections.push_back(tb); - } - for (auto it = frameTrackingResult.begin(); it != frameTrackingResult.end(); it++) - { - int frame_age = frame_count - it->frame; - if (frame_age >= _max_age || frame_age < 0) - { - dead_trackers_id.push_back(it->id); - } - } - } - - /////////////////////////////////////// - // 3.1. get predicted locations from existing trackers. - predictedBoxes.clear(); - for (unsigned int i = 0; i < trackers.size();) - { - cv::Rect_ pBox = trackers[i].predict(); - if (pBox.x >= 0 && pBox.y >= 0) - { - predictedBoxes.push_back(pBox); - i++; - continue; - } - trackers.erase(trackers.begin() + i); - } - - trkNum = predictedBoxes.size(); - detNum = detections.size(); - - centroid_dist_matrix.clear(); - centroid_dist_matrix.resize(trkNum, vector(detNum, 0)); - - for (unsigned int i = 0; i < trkNum; i++) // compute iou matrix as a distance matrix - { - for (unsigned int j = 0; j < detNum; j++) - { - // use 1-iou because the hungarian algorithm computes a minimum-cost assignment. - double distance = SortTracker::GetCentroidsDistance(predictedBoxes[i], detections[j].box) / image_diagonal; - centroid_dist_matrix[i][j] = distance; - } - } - - HungarianAlgorithm HungAlgo; - assignment.clear(); - HungAlgo.Solve(centroid_dist_matrix, assignment); - // find matches, unmatched_detections and unmatched_predictions - unmatchedTrajectories.clear(); - unmatchedDetections.clear(); - allItems.clear(); - matchedItems.clear(); - - if (detNum > trkNum) // there are unmatched detections - { - for (unsigned int n = 0; n < detNum; n++) - allItems.insert(n); - - for (unsigned int i = 0; i < trkNum; ++i) - matchedItems.insert(assignment[i]); - - set_difference(allItems.begin(), allItems.end(), - matchedItems.begin(), matchedItems.end(), - insert_iterator>(unmatchedDetections, unmatchedDetections.begin())); - } - else if (detNum < trkNum) // there are unmatched trajectory/predictions - { - for (unsigned int i = 0; i < trkNum; ++i) - if (assignment[i] == -1) // unassigned label will be set as -1 in the assignment algorithm - unmatchedTrajectories.insert(i); - } - else - ; - - // filter out matched with low IOU - matchedPairs.clear(); - for (unsigned int i = 0; i < trkNum; ++i) - { - if (assignment[i] == -1) // pass over invalid values - continue; - if (centroid_dist_matrix[i][assignment[i]] > max_centroid_dist_norm) - { - unmatchedTrajectories.insert(i); - unmatchedDetections.insert(assignment[i]); - } - else - matchedPairs.push_back(cv::Point(i, assignment[i])); - } - - for (unsigned int i = 0; i < matchedPairs.size(); i++) - { - int trkIdx = matchedPairs[i].x; - int detIdx = matchedPairs[i].y; - trackers[trkIdx].update(detections[detIdx].box); - trackers[trkIdx].classId = detections[detIdx].classId; - trackers[trkIdx].confidence = detections[detIdx].confidence; - } - - // create and initialise new trackers for unmatched detections - for (auto umd : unmatchedDetections) - { - KalmanTracker tracker = KalmanTracker(detections[umd].box, detections[umd].confidence, detections[umd].classId); - trackers.push_back(tracker); - } - - for (auto it2 = dead_trackers_id.begin(); it2 != dead_trackers_id.end(); it2++) - { - for (unsigned int i = 0; i < trackers.size();) - { - if (trackers[i].m_id == (*it2)) - { - trackers.erase(trackers.begin() + i); - continue; - } - i++; - } - } - - // get trackers' output - frameTrackingResult.clear(); - for (unsigned int i = 0; i < trackers.size();) - { - if ((trackers[i].m_time_since_update < 1 && trackers[i].m_hit_streak >= _min_hits) || frame_count <= _min_hits) - { - alive_tracker = true; - TrackingBox res; - res.box = trackers[i].get_state(); - res.id = trackers[i].m_id; - res.frame = frame_count; - res.classId = trackers[i].classId; - res.confidence = trackers[i].confidence; - frameTrackingResult.push_back(res); - } - - // remove dead tracklet - if (trackers[i].m_time_since_update >= _max_age) - { - trackers.erase(trackers.begin() + i); - continue; - } - i++; - } -} +#include "sort.hpp" + +using namespace std; + +// Constructor +SortTracker::SortTracker(int max_age, int min_hits) +{ + _min_hits = min_hits; + _max_age = max_age; + alive_tracker = true; +} + +// Computes IOU between two bounding boxes +double SortTracker::GetIOU(cv::Rect_ bb_test, cv::Rect_ bb_gt) +{ + float in = (bb_test & bb_gt).area(); + float un = bb_test.area() + bb_gt.area() - in; + + if (un < DBL_EPSILON) + return 0; + + return (double)(in / un); +} + +// Computes centroid distance between two bounding boxes +double SortTracker::GetCentroidsDistance( + cv::Rect_ bb_test, + cv::Rect_ bb_gt) +{ + float bb_test_centroid_x = (bb_test.x + bb_test.width / 2); + float bb_test_centroid_y = (bb_test.y + bb_test.height / 2); + + float bb_gt_centroid_x = (bb_gt.x + bb_gt.width / 2); + float bb_gt_centroid_y = (bb_gt.y + bb_gt.height / 2); + + double distance = (double)sqrt(pow(bb_gt_centroid_x - bb_test_centroid_x, 2) + pow(bb_gt_centroid_y - bb_test_centroid_y, 2)); + + return distance; +} + +void SortTracker::update(vector detections_cv, int frame_count, double image_diagonal, std::vector confidences, std::vector classIds) +{ + vector detections; + if (trackers.size() == 0) // the first frame met + { + alive_tracker = false; + // initialize kalman trackers using first detections. + for (unsigned int i = 0; i < detections_cv.size(); i++) + { + TrackingBox tb; + + tb.box = cv::Rect_(detections_cv[i]); + tb.classId = classIds[i]; + tb.confidence = confidences[i]; + detections.push_back(tb); + + KalmanTracker trk = KalmanTracker(detections[i].box, detections[i].confidence, detections[i].classId); + trackers.push_back(trk); + } + return; + } + else + { + for (unsigned int i = 0; i < detections_cv.size(); i++) + { + TrackingBox tb; + tb.box = cv::Rect_(detections_cv[i]); + tb.classId = classIds[i]; + tb.confidence = confidences[i]; + detections.push_back(tb); + } + for (auto it = frameTrackingResult.begin(); it != frameTrackingResult.end(); it++) + { + int frame_age = frame_count - it->frame; + if (frame_age >= _max_age || frame_age < 0) + { + dead_trackers_id.push_back(it->id); + } + } + } + + /////////////////////////////////////// + // 3.1. get predicted locations from existing trackers. + predictedBoxes.clear(); + for (unsigned int i = 0; i < trackers.size();) + { + cv::Rect_ pBox = trackers[i].predict(); + if (pBox.x >= 0 && pBox.y >= 0) + { + predictedBoxes.push_back(pBox); + i++; + continue; + } + trackers.erase(trackers.begin() + i); + } + + trkNum = predictedBoxes.size(); + detNum = detections.size(); + + centroid_dist_matrix.clear(); + centroid_dist_matrix.resize(trkNum, vector(detNum, 0)); + + for (unsigned int i = 0; i < trkNum; i++) // compute iou matrix as a distance matrix + { + for (unsigned int j = 0; j < detNum; j++) + { + // use 1-iou because the hungarian algorithm computes a minimum-cost assignment. + double distance = SortTracker::GetCentroidsDistance(predictedBoxes[i], detections[j].box) / image_diagonal; + centroid_dist_matrix[i][j] = distance; + } + } + + HungarianAlgorithm HungAlgo; + assignment.clear(); + HungAlgo.Solve(centroid_dist_matrix, assignment); + // find matches, unmatched_detections and unmatched_predictions + unmatchedTrajectories.clear(); + unmatchedDetections.clear(); + allItems.clear(); + matchedItems.clear(); + + if (detNum > trkNum) // there are unmatched detections + { + for (unsigned int n = 0; n < detNum; n++) + allItems.insert(n); + + for (unsigned int i = 0; i < trkNum; ++i) + matchedItems.insert(assignment[i]); + + set_difference(allItems.begin(), allItems.end(), + matchedItems.begin(), matchedItems.end(), + insert_iterator>(unmatchedDetections, unmatchedDetections.begin())); + } + else if (detNum < trkNum) // there are unmatched trajectory/predictions + { + for (unsigned int i = 0; i < trkNum; ++i) + if (assignment[i] == -1) // unassigned label will be set as -1 in the assignment algorithm + unmatchedTrajectories.insert(i); + } + else + ; + + // filter out matched with low IOU + matchedPairs.clear(); + for (unsigned int i = 0; i < trkNum; ++i) + { + if (assignment[i] == -1) // pass over invalid values + continue; + if (centroid_dist_matrix[i][assignment[i]] > max_centroid_dist_norm) + { + unmatchedTrajectories.insert(i); + unmatchedDetections.insert(assignment[i]); + } + else + matchedPairs.push_back(cv::Point(i, assignment[i])); + } + + for (unsigned int i = 0; i < matchedPairs.size(); i++) + { + int trkIdx = matchedPairs[i].x; + int detIdx = matchedPairs[i].y; + trackers[trkIdx].update(detections[detIdx].box); + trackers[trkIdx].classId = detections[detIdx].classId; + trackers[trkIdx].confidence = detections[detIdx].confidence; + } + + // create and initialise new trackers for unmatched detections + for (auto umd : unmatchedDetections) + { + KalmanTracker tracker = KalmanTracker(detections[umd].box, detections[umd].confidence, detections[umd].classId); + trackers.push_back(tracker); + } + + for (auto it2 = dead_trackers_id.begin(); it2 != dead_trackers_id.end(); it2++) + { + for (unsigned int i = 0; i < trackers.size();) + { + if (trackers[i].m_id == (*it2)) + { + trackers.erase(trackers.begin() + i); + continue; + } + i++; + } + } + + // get trackers' output + frameTrackingResult.clear(); + for (unsigned int i = 0; i < trackers.size();) + { + if ((trackers[i].m_time_since_update < 1 && trackers[i].m_hit_streak >= _min_hits) || frame_count <= _min_hits) + { + alive_tracker = true; + TrackingBox res; + res.box = trackers[i].get_state(); + res.id = trackers[i].m_id; + res.frame = frame_count; + res.classId = trackers[i].classId; + res.confidence = trackers[i].confidence; + frameTrackingResult.push_back(res); + } + + // remove dead tracklet + if (trackers[i].m_time_since_update >= _max_age) + { + trackers.erase(trackers.begin() + i); + continue; + } + i++; + } +} diff --git a/src/sort_filter/sort.hpp b/src/sort_filter/sort.hpp index 1482a28a..9161ada9 100644 --- a/src/sort_filter/sort.hpp +++ b/src/sort_filter/sort.hpp @@ -1,61 +1,61 @@ - -#include "KalmanTracker.h" -#include "Hungarian.h" - -#include -#include -#include // to format image names using setw() and setfill() -#include - -#include "opencv2/video/tracking.hpp" -#include "opencv2/highgui/highgui.hpp" - -#ifndef _OPENCV_KCFTRACKER_HPP_ -#define _OPENCV_KCFTRACKER_HPP_ -#endif -#pragma once - -typedef struct TrackingBox -{ - int frame = 0; - float confidence = 0; - int classId = 0; - int id = 0; - cv::Rect_ box = cv::Rect_(0.0, 0.0, 0.0, 0.0); - TrackingBox() {} - TrackingBox(int _frame, float _confidence, int _classId, int _id) : frame(_frame), confidence(_confidence), classId(_classId), id(_id) {} -} TrackingBox; - -class SortTracker -{ -public: - // Constructor - SortTracker(int max_age = 7, int min_hits = 2); - // Initialize tracker - - // Update position based on the new frame - void update(std::vector detection, int frame_count, double image_diagonal, std::vector confidences, std::vector classIds); - double GetIOU(cv::Rect_ bb_test, cv::Rect_ bb_gt); - double GetCentroidsDistance(cv::Rect_ bb_test, cv::Rect_ bb_gt); - std::vector trackers; - - double max_centroid_dist_norm = 0.15; - - std::vector> predictedBoxes; - std::vector> centroid_dist_matrix; - std::vector assignment; - std::set unmatchedDetections; - std::set unmatchedTrajectories; - std::set allItems; - std::set matchedItems; - std::vector matchedPairs; - - std::vector frameTrackingResult; - std::vector dead_trackers_id; - - unsigned int trkNum = 0; - unsigned int detNum = 0; - int _min_hits; - int _max_age; - bool alive_tracker; -}; + +#include "KalmanTracker.h" +#include "Hungarian.h" + +#include +#include +#include // to format image names using setw() and setfill() +#include + +#include "opencv2/video/tracking.hpp" +#include "opencv2/highgui/highgui.hpp" + +#ifndef _OPENCV_KCFTRACKER_HPP_ +#define _OPENCV_KCFTRACKER_HPP_ +#endif +#pragma once + +typedef struct TrackingBox +{ + int frame = 0; + float confidence = 0; + int classId = 0; + int id = 0; + cv::Rect_ box = cv::Rect_(0.0, 0.0, 0.0, 0.0); + TrackingBox() {} + TrackingBox(int _frame, float _confidence, int _classId, int _id) : frame(_frame), confidence(_confidence), classId(_classId), id(_id) {} +} TrackingBox; + +class SortTracker +{ +public: + // Constructor + SortTracker(int max_age = 7, int min_hits = 2); + // Initialize tracker + + // Update position based on the new frame + void update(std::vector detection, int frame_count, double image_diagonal, std::vector confidences, std::vector classIds); + double GetIOU(cv::Rect_ bb_test, cv::Rect_ bb_gt); + double GetCentroidsDistance(cv::Rect_ bb_test, cv::Rect_ bb_gt); + std::vector trackers; + + double max_centroid_dist_norm = 0.15; + + std::vector> predictedBoxes; + std::vector> centroid_dist_matrix; + std::vector assignment; + std::set unmatchedDetections; + std::set unmatchedTrajectories; + std::set allItems; + std::set matchedItems; + std::vector matchedPairs; + + std::vector frameTrackingResult; + std::vector dead_trackers_id; + + unsigned int trkNum = 0; + unsigned int detNum = 0; + int _min_hits; + int _max_age; + bool alive_tracker; +};