upbot_sort
FengHua0208
parent
db4da8fc6e
commit
bededf80b9
|
|
@ -0,0 +1,40 @@
|
|||
///
|
||||
// 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
|
||||
//
|
||||
///
|
||||
|
||||
#pragma once
|
||||
#ifndef HUNGARIAN_H
|
||||
#define HUNGARIAN_H
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
|
||||
class HungarianAlgorithm
|
||||
{
|
||||
public:
|
||||
HungarianAlgorithm();
|
||||
~HungarianAlgorithm();
|
||||
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
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
#pragma once
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
|
||||
/**
|
||||
* @brief KalmanBoxTracker 卡尔曼跟踪器
|
||||
*/
|
||||
class KalmanBoxTracker
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief KalmanBoxTracker 卡尔曼跟踪器类构造函数
|
||||
* @param bbox 检测框
|
||||
*/
|
||||
KalmanBoxTracker(std::vector<int> bbox);
|
||||
|
||||
/**
|
||||
* @brief update 通过观测的检测框更新系统状态
|
||||
* @param bbox 检测框
|
||||
*/
|
||||
void update(std::vector<int> bbox);
|
||||
|
||||
/**
|
||||
* @brief predict 估计预测的边界框
|
||||
*/
|
||||
std::vector<float> predict();
|
||||
|
||||
/**
|
||||
* @brief get_state 返回当前检测框状态
|
||||
*/
|
||||
std::vector<float> get_state();
|
||||
|
||||
public:
|
||||
/**
|
||||
* @param m_kf 卡尔曼滤波器
|
||||
*/
|
||||
cv::KalmanFilter* m_kf;
|
||||
|
||||
/**
|
||||
* @param m_time_since_update 从更新开始的时间（帧数）
|
||||
*/
|
||||
int m_time_since_update;
|
||||
|
||||
/**
|
||||
* @param count 卡尔曼跟踪器的个数
|
||||
*/
|
||||
static int count;
|
||||
|
||||
/**
|
||||
* @param m_id 卡尔曼跟踪器的id
|
||||
*/
|
||||
int m_id;
|
||||
|
||||
/**
|
||||
* @param m_history 历史检测框的储存
|
||||
*/
|
||||
std::vector<std::vector<float>> m_history;
|
||||
|
||||
/**
|
||||
* @param m_hits
|
||||
*/
|
||||
int m_hits;
|
||||
|
||||
/**
|
||||
* @param m_hit_streak 忽略目标初始的若干帧
|
||||
*/
|
||||
int m_hit_streak;
|
||||
|
||||
/**
|
||||
* @param m_age predict被调用的次数计数
|
||||
*/
|
||||
int m_age;
|
||||
};
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
|
||||
#pragma once
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include "utils.h"
|
||||
#include "kalmanboxtracker.h"
|
||||
|
||||
|
||||
/**
|
||||
* @brief Sort Sort<EFBFBD>㷨
|
||||
*/
|
||||
class Sort
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Sort <EFBFBD><EFBFBD><EFBFBD>캯<EFBFBD><EFBFBD>
|
||||
* @param max_age δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param min_hits δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param iou_threshold iou<EFBFBD><EFBFBD>ֵ
|
||||
*/
|
||||
Sort(int max_age, int min_hits, float iou_threshold);
|
||||
|
||||
/**
|
||||
* @brief update <EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param dets <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
std::vector<std::vector<float>> update(std::vector<cv::Rect> dets);
|
||||
|
||||
/**
|
||||
* @brief detect_update <EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param detect_result_group <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
_detect_result_group_t detect_update(_detect_result_group_t detect_result_group);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @param m_max_age δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
int m_max_age;
|
||||
|
||||
/**
|
||||
* @param m_min_hits δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
int m_min_hits;
|
||||
|
||||
/**
|
||||
* @param m_iou_threshold iou<EFBFBD><EFBFBD>ֵ
|
||||
*/
|
||||
float m_iou_threshold;
|
||||
|
||||
/**
|
||||
* @param m_trackers <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
std::vector<KalmanBoxTracker> m_trackers;
|
||||
|
||||
/**
|
||||
* @param m_frame_count ֡<EFBFBD><EFBFBD>
|
||||
*/
|
||||
int m_frame_count;
|
||||
};
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
#pragma once
|
||||
#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);
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
#pragma once
|
||||
#include<iostream>
|
||||
#include<opencv2/opencv.hpp>
|
||||
|
||||
#define YOLO_P6 false //是否使用P6模型
|
||||
|
||||
struct Output {
|
||||
int id; //结果类别id
|
||||
float confidence; //结果置信度
|
||||
cv::Rect box; //矩形框
|
||||
};
|
||||
|
||||
class Yolov5 {
|
||||
public:
|
||||
Yolov5() {
|
||||
}
|
||||
~Yolov5() {}
|
||||
bool readModel(cv::dnn::Net& net, std::string& netPath, bool isCuda);
|
||||
bool Detect(cv::Mat& SrcImg, cv::dnn::Net& net, std::vector<Output>& output);
|
||||
void drawPred(cv::Mat& img, std::vector<Output> result, std::vector<cv::Scalar> color);
|
||||
|
||||
private:
|
||||
|
||||
void LetterBox(const cv::Mat& image, cv::Mat& outImage,
|
||||
cv::Vec4d& params, //[ratio_x,ratio_y,dw,dh]
|
||||
const cv::Size& newShape = cv::Size(640, 640),
|
||||
bool autoShape = false,
|
||||
bool scaleFill = false,
|
||||
bool scaleUp = true,
|
||||
int stride = 32,
|
||||
const cv::Scalar& color = cv::Scalar(114, 114, 114));
|
||||
|
||||
#if(defined YOLO_P6 && YOLO_P6==true)
|
||||
const int _netWidth = 640; //ONNX图片输入宽度
|
||||
const int _netHeight = 640; //ONNX图片输入高度
|
||||
#else
|
||||
|
||||
const int _netWidth = 640; //ONNX图片输入宽度
|
||||
const int _netHeight = 640; //ONNX图片输入高度
|
||||
#endif // YOLO_P6
|
||||
|
||||
float _classThreshold = 0.25;
|
||||
float _nmsThreshold = 0.45;
|
||||
public:
|
||||
std::vector<std::string> _className = { "person", "bicycle", "car", "motorcycle", "airplane", "bus", "train", "truck", "boat", "traffic light",
|
||||
"fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow",
|
||||
"elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", "suitcase", "frisbee",
|
||||
"skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", "skateboard", "surfboard",
|
||||
"tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana", "apple",
|
||||
"sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", "chair", "couch",
|
||||
"potted plant", "bed", "dining table", "toilet", "tv", "laptop", "mouse", "remote", "keyboard", "cell phone",
|
||||
"microwave", "oven", "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", "teddy bear",
|
||||
"hair drier", "toothbrush" };
|
||||
};
|
||||
|
|
@ -0,0 +1,389 @@
|
|||
///
|
||||
// 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 <stdlib.h>
|
||||
#include <cfloat> // for DBL_MAX
|
||||
#include <cmath> // for fabs()
|
||||
#include "Hungarian.h"
|
||||
|
||||
|
||||
HungarianAlgorithm::HungarianAlgorithm() {}
|
||||
HungarianAlgorithm::~HungarianAlgorithm() {}
|
||||
|
||||
|
||||
//********************************************************//
|
||||
// A single function wrapper for solving assignment problem.
|
||||
//********************************************************//
|
||||
float HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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);
|
||||
}
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
#include "utils.h"
|
||||
#include "kalmanboxtracker.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);
|
||||
}
|
||||
|
|
@ -0,0 +1,266 @@
|
|||
#include "yolo.h"
|
||||
#include "kalmanboxtracker.h"
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <vector>
|
||||
#include <time.h>
|
||||
#include "sort.h"
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
using namespace cv::dnn;
|
||||
|
||||
int main()
|
||||
{
|
||||
string img_path = "D:/VS/yoloSORT/images/test.mp4";
|
||||
string model_path = "D:/VS/yoloSORT/models/yolov5s.onnx";
|
||||
|
||||
// 创建Yolov5模型和跟踪器
|
||||
Yolov5 yolo;
|
||||
Net net;
|
||||
Sort mot_tracker = Sort(1, 3, 0.3); // 创建Sort跟踪器
|
||||
|
||||
// 读取Yolo模型
|
||||
if (!yolo.readModel(net, model_path, true)) {
|
||||
cout << "无法加载Yolo模型" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 生成颜色表，每个类别对应一个颜色
|
||||
vector<Scalar> colors;
|
||||
srand(42); // 固定随机种子，确保每次运行颜色一致
|
||||
for (int i = 0; i < 80; i++) { // 假设最多80个类别
|
||||
int b = rand() % 256;
|
||||
int g = rand() % 256;
|
||||
int r = rand() % 256;
|
||||
colors.push_back(Scalar(b, g, r));
|
||||
}
|
||||
|
||||
// 读取视频
|
||||
VideoCapture cap(img_path);
|
||||
if (!cap.isOpened()) {
|
||||
cout << "无法打开视频文件" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
VideoWriter writer;
|
||||
writer = VideoWriter("D:/VS/yoloSORT/images/detect.mp4", CAP_OPENCV_MJPEG, 20, Size(2560, 1440), true);
|
||||
|
||||
clock_t start, end; //计时器
|
||||
start = clock();
|
||||
int num = 0;
|
||||
|
||||
Mat frame;
|
||||
int frame_counter = 0;
|
||||
int detect_interval = 3; // 每隔多少帧重新调用一次YOLO进行检测
|
||||
vector<Rect> detection_rects;
|
||||
|
||||
while (cap.read(frame)) {
|
||||
cap >> frame;
|
||||
|
||||
if (frame.empty()) {
|
||||
cout << "视频已结束" << endl;
|
||||
break;
|
||||
}
|
||||
|
||||
vector<Output> result;
|
||||
|
||||
// 每隔 detect_interval 帧进行一次 YOLO 检测
|
||||
if (frame_counter % detect_interval == 0) {
|
||||
detection_rects.clear(); // 清空之前的检测框
|
||||
|
||||
// 进行目标检测
|
||||
if (yolo.Detect(frame, net, result)) {
|
||||
for (int i = 0; i < result.size(); i++) {
|
||||
int x = result[i].box.x;
|
||||
int y = result[i].box.y;
|
||||
int w = result[i].box.width;
|
||||
int h = result[i].box.height;
|
||||
Rect rect(x, y, w, h);
|
||||
detection_rects.push_back(rect);
|
||||
}
|
||||
}
|
||||
|
||||
// 将检测结果传递给 SORT 进行初始化或更新
|
||||
vector<vector<float>> trackers = mot_tracker.update(detection_rects);
|
||||
// 绘制跟踪结果
|
||||
for (int i = 0; i < trackers.size(); i++) {
|
||||
Rect rect(trackers[i][0], trackers[i][1], trackers[i][2] - trackers[i][0], trackers[i][3] - trackers[i][1]);
|
||||
|
||||
// 获取检测的类别和置信度
|
||||
// 在这里，我们假设检测的类别和置信度没有变化
|
||||
int class_id = 0; // 假设分类 ID 为 0，仅用于示例
|
||||
float confidence = 0.8f; // 假设置信度为 0.8，仅用于示例
|
||||
|
||||
// 使用类别索引从颜色表中获取颜色
|
||||
Scalar color = colors[class_id % colors.size()]; // 使用颜色表中的颜色
|
||||
|
||||
// 构建标签内容：类别名称 + 置信度
|
||||
string label = "box";
|
||||
|
||||
// 绘制矩形框和类别标签
|
||||
rectangle(frame, rect, color, 2); // 绘制矩形框
|
||||
putText(frame, label, Point(rect.x, rect.y), FONT_HERSHEY_SIMPLEX, 1, color, 2); // 显示类别名称和置信度
|
||||
}
|
||||
}
|
||||
else {
|
||||
// 如果不是检测帧，使用 SORT 进行目标预测和跟踪
|
||||
vector<vector<float>> trackers = mot_tracker.update(detection_rects);
|
||||
|
||||
// 绘制跟踪结果
|
||||
for (int i = 0; i < trackers.size(); i++) {
|
||||
Rect rect(trackers[i][0], trackers[i][1], trackers[i][2] - trackers[i][0], trackers[i][3] - trackers[i][1]);
|
||||
|
||||
// 获取检测的类别和置信度
|
||||
// 在这里，我们假设检测的类别和置信度没有变化
|
||||
int class_id = 0; // 假设分类 ID 为 0，仅用于示例
|
||||
float confidence = 0.8f; // 假设置信度为 0.8，仅用于示例
|
||||
|
||||
// 使用类别索引从颜色表中获取颜色
|
||||
Scalar color = colors[class_id % colors.size()]; // 使用颜色表中的颜色
|
||||
|
||||
// 构建标签内容：类别名称 + 置信度
|
||||
string label = "box";
|
||||
|
||||
// 绘制矩形框和类别标签
|
||||
rectangle(frame, rect, color, 2); // 绘制矩形框
|
||||
putText(frame, label, Point(rect.x, rect.y), FONT_HERSHEY_SIMPLEX, 1, color, 2); // 显示类别名称和置信度
|
||||
}
|
||||
}
|
||||
|
||||
// 显示并保存帧
|
||||
imshow("img", frame);
|
||||
writer << frame;
|
||||
|
||||
if (waitKey(10) == 27) {
|
||||
break; // 退出循环
|
||||
}
|
||||
|
||||
frame_counter++;
|
||||
num++;
|
||||
}
|
||||
|
||||
end = clock();
|
||||
cout << "time = " << double(end - start) / CLOCKS_PER_SEC << "s" << endl; // 输出时间（单位：s）
|
||||
cout << "frame num: " << num << endl;
|
||||
cout << "Speed = " << double(end - start) * 1000 / CLOCKS_PER_SEC / num << "ms" << endl; // 输出时间（单位：ms）
|
||||
|
||||
cap.release();
|
||||
destroyAllWindows();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
#include "yolo.h"
|
||||
#include "kalmanboxtracker.h"
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <vector>
|
||||
#include <time.h>
|
||||
#include "sort.h"
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
using namespace cv::dnn;
|
||||
|
||||
int main()
|
||||
{
|
||||
string img_path = "D:/VS/yoloTest/images/1.mp4";
|
||||
string model_path = "D:/VS/yoloTest/models/yolov5s.onnx";
|
||||
|
||||
// 创建Yolov5模型和跟踪器
|
||||
Yolov5 yolo;
|
||||
Net net;
|
||||
Sort mot_tracker = Sort(1, 3, 0.3); // 创建Sort跟踪器
|
||||
|
||||
// 读取Yolo模型
|
||||
if (!yolo.readModel(net, model_path, true)) {
|
||||
cout << "无法加载Yolo模型" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 生成随机颜色
|
||||
srand(time(0));
|
||||
vector<Scalar> colors;
|
||||
for (int i = 0; i < 80; i++) {
|
||||
int b = rand() % 256;
|
||||
int g = rand() % 256;
|
||||
int r = rand() % 256;
|
||||
colors.push_back(Scalar(b, g, r));
|
||||
}
|
||||
|
||||
// 读取视频
|
||||
VideoCapture cap(img_path);
|
||||
if (!cap.isOpened()) {
|
||||
cout << "无法打开视频文件" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
VideoWriter writer;
|
||||
writer = VideoWriter("D:/VS/yoloTest/images/test.mp4", CAP_OPENCV_MJPEG, 20, Size(2560, 1440), true);
|
||||
|
||||
clock_t start, end; //计时器
|
||||
start = clock();
|
||||
int num = 0;
|
||||
|
||||
Mat frame;
|
||||
while (cap.read(frame)) {
|
||||
cap >> frame;
|
||||
|
||||
if (frame.empty()) {
|
||||
cout << "视频已结束" << endl;
|
||||
break;
|
||||
}
|
||||
// 进行目标检测
|
||||
vector<Output> result;
|
||||
if (yolo.Detect(frame, net, result)) {
|
||||
// 跟踪已检测到的目标
|
||||
vector<Rect> detection_rects; // 存储检测结果的矩形框
|
||||
for (int i = 0; i < result.size(); i++) {
|
||||
int x = result[i].box.x;
|
||||
int y = result[i].box.y;
|
||||
int w = result[i].box.width;
|
||||
int h = result[i].box.height;
|
||||
Rect rect(x, y, w, h);
|
||||
detection_rects.push_back(rect);
|
||||
}
|
||||
// 更新目标跟踪器
|
||||
vector<vector<float>> trackers = mot_tracker.update(detection_rects);//x,y,w,h,id
|
||||
// 绘制跟踪结果
|
||||
for (int i = 0; i < trackers.size(); i++) {
|
||||
Rect rect(trackers[i][0], trackers[i][1], trackers[i][2] - trackers[i][0], trackers[i][3] - trackers[i][1]);
|
||||
//cout << "0:" << trackers[i][0] << endl; //跟踪目标的 x 坐标（左上角的 x 坐标）。
|
||||
//cout << "1:" << trackers[i][1] << endl; //跟踪目标的 y 坐标（左上角的 y 坐标）。
|
||||
//cout << "2:" << trackers[i][2] << endl; //跟踪目标的宽度。
|
||||
//cout << "3:" << trackers[i][3] << endl; //跟踪目标的高度。
|
||||
//cout << "4:" << trackers[i][4] << endl; //跟踪目标的 ID。
|
||||
int id = static_cast<int>(trackers[i][4]);
|
||||
//cout << "id:" << id << endl;
|
||||
//string label = yolo.className[result[id].id] + ":" + to_string(result[id].confidence);
|
||||
rectangle(frame, rect, Scalar(0, 255, 0), 2);
|
||||
cout << trackers[i][5] << endl;
|
||||
putText(frame, "id:" + to_string(int(trackers[i][4])), Point(rect.x, rect.y), FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 255, 0), 2);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
imshow("img", frame);
|
||||
writer << frame;
|
||||
if (waitKey(10) == 27) {
|
||||
break; // 退出循环
|
||||
}
|
||||
|
||||
num++;
|
||||
}
|
||||
|
||||
end = clock();
|
||||
cout << "time = " << double(end - start) / CLOCKS_PER_SEC << "s" << endl; //输出时间（单位：ｓ）
|
||||
cout << "frame num: " << num << endl;
|
||||
cout << "Speed = " << double(end - start) * 1000 / CLOCKS_PER_SEC / num << "ms" << endl; //输出时间（单位：mｓ）
|
||||
|
||||
cap.release();
|
||||
destroyAllWindows();
|
||||
|
||||
return 0;
|
||||
}
|
||||
*/
|
||||
|
|
@ -0,0 +1,140 @@
|
|||
#include "sort.h"
|
||||
|
||||
|
||||
|
||||
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 {};
|
||||
}
|
||||
|
||||
_detect_result_group_t Sort::detect_update(_detect_result_group_t detect_result_group);
|
||||
{
|
||||
std::vector<cv::Rect> dets;
|
||||
for(int i=0; i<detect_result_group.results.size();i++){
|
||||
detect_result_t item = result.results[i];
|
||||
cv::Rect temp(item.box.left, item.box.top, item.box.right-item.box.left, item.box.bottom-item.box.top);
|
||||
dets.push_back(temp);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
for(int i=0; i<detect_result_group.results.size(); i++){
|
||||
// cv::Rect rect(trackers[i][0], trackers[i][1], trackers[i][2]-trackers[i][0], trackers[i][3]-trackers[i][1]);
|
||||
detect_result_group.result[i].box.left = ret[i][0];
|
||||
detect_result_group.result[i].box.top = ret[i][1];
|
||||
detect_result_group.result[i].box.right = ret[i][2] + ret[i][0];
|
||||
detect_result_group.result[i].box.bottom = ret[i][3] + ret[i][1];
|
||||
}
|
||||
|
||||
if (ret.size() > 0)
|
||||
return detect_result_group;
|
||||
|
||||
return {};
|
||||
}
|
||||
|
|
@ -0,0 +1,219 @@
|
|||
#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);
|
||||
|
||||
HungarianAlgorithm 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)
|
||||
{
|
||||
if (trackers.size() == 0)
|
||||
{
|
||||
std::vector<int> unmatched_detections(detections.size());
|
||||
std::iota(unmatched_detections.begin(), unmatched_detections.end(), 0);
|
||||
return { {}, unmatched_detections, {} };
|
||||
}
|
||||
|
||||
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 };
|
||||
}
|
||||
|
|
@ -0,0 +1,183 @@
|
|||
#include"yolo.h"
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
using namespace cv::dnn;
|
||||
|
||||
void Yolov5::LetterBox(const cv::Mat& image, cv::Mat& outImage, cv::Vec4d& params, const cv::Size& newShape,
|
||||
bool autoShape, bool scaleFill, bool scaleUp, int stride, const cv::Scalar& color)
|
||||
{
|
||||
if (false) {
|
||||
int maxLen = MAX(image.rows, image.cols);
|
||||
outImage = Mat::zeros(Size(maxLen, maxLen), CV_8UC3);
|
||||
image.copyTo(outImage(Rect(0, 0, image.cols, image.rows)));
|
||||
params[0] = 1;
|
||||
params[1] = 1;
|
||||
params[3] = 0;
|
||||
params[2] = 0;
|
||||
}
|
||||
|
||||
cv::Size shape = image.size();
|
||||
float r = std::min((float)newShape.height / (float)shape.height,
|
||||
(float)newShape.width / (float)shape.width);
|
||||
if (!scaleUp)
|
||||
r = std::min(r, 1.0f);
|
||||
|
||||
float ratio[2]{ r, r };
|
||||
int new_un_pad[2] = { (int)std::round((float)shape.width * r),(int)std::round((float)shape.height * r) };
|
||||
|
||||
auto dw = (float)(newShape.width - new_un_pad[0]);
|
||||
auto dh = (float)(newShape.height - new_un_pad[1]);
|
||||
|
||||
if (autoShape)
|
||||
{
|
||||
dw = (float)((int)dw % stride);
|
||||
dh = (float)((int)dh % stride);
|
||||
}
|
||||
else if (scaleFill)
|
||||
{
|
||||
dw = 0.0f;
|
||||
dh = 0.0f;
|
||||
new_un_pad[0] = newShape.width;
|
||||
new_un_pad[1] = newShape.height;
|
||||
ratio[0] = (float)newShape.width / (float)shape.width;
|
||||
ratio[1] = (float)newShape.height / (float)shape.height;
|
||||
}
|
||||
|
||||
dw /= 2.0f;
|
||||
dh /= 2.0f;
|
||||
|
||||
if (shape.width != new_un_pad[0] && shape.height != new_un_pad[1])
|
||||
{
|
||||
cv::resize(image, outImage, cv::Size(new_un_pad[0], new_un_pad[1]));
|
||||
}
|
||||
else {
|
||||
outImage = image.clone();
|
||||
}
|
||||
|
||||
int top = int(std::round(dh - 0.1f));
|
||||
int bottom = int(std::round(dh + 0.1f));
|
||||
int left = int(std::round(dw - 0.1f));
|
||||
int right = int(std::round(dw + 0.1f));
|
||||
params[0] = ratio[0];
|
||||
params[1] = ratio[1];
|
||||
params[2] = left;
|
||||
params[3] = top;
|
||||
cv::copyMakeBorder(outImage, outImage, top, bottom, left, right, cv::BORDER_CONSTANT, color);
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool Yolov5::readModel(Net& net, string& netPath, bool isCuda = false) {
|
||||
try {
|
||||
|
||||
net = readNet(netPath);
|
||||
#if CV_VERSION_MAJOR==4 &&CV_VERSION_MINOR==7&&CV_VERSION_REVISION==0
|
||||
net.enableWinograd(false); //bug of opencv4.7.x in AVX only platform ,https://github.com/opencv/opencv/pull/23112 and https://github.com/opencv/opencv/issues/23080
|
||||
//net.enableWinograd(true); //If your CPU supports AVX2, you can set it true to speed up
|
||||
#endif
|
||||
}
|
||||
catch (const std::exception&) {
|
||||
return false;
|
||||
}
|
||||
//cuda
|
||||
if (isCuda) {
|
||||
net.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA);
|
||||
net.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA);
|
||||
}
|
||||
//cpu
|
||||
else {
|
||||
net.setPreferableBackend(cv::dnn::DNN_BACKEND_DEFAULT);
|
||||
net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool Yolov5::Detect(Mat& SrcImg, Net& net, vector<Output>& output) {
|
||||
Mat blob;
|
||||
int col = SrcImg.cols;
|
||||
int row = SrcImg.rows;
|
||||
int maxLen = MAX(col, row);
|
||||
Mat netInputImg = SrcImg.clone();
|
||||
Vec4d params;
|
||||
LetterBox(SrcImg, netInputImg, params, cv::Size(_netWidth, _netHeight));
|
||||
|
||||
blobFromImage(netInputImg, blob, 1 / 255.0, cv::Size(_netWidth, _netHeight), cv::Scalar(0, 0, 0), true, false);
|
||||
//如果在其他设置没有问题的情况下但是结果偏差很大，可以尝试下用下面两句语句
|
||||
//blobFromImage(netInputImg, blob, 1 / 255.0, cv::Size(_netWidth, _netHeight), cv::Scalar(104, 117, 123), true, false);
|
||||
//blobFromImage(netInputImg, blob, 1 / 255.0, cv::Size(_netWidth, _netHeight), cv::Scalar(114, 114,114), true, false);
|
||||
net.setInput(blob);
|
||||
std::vector<cv::Mat> netOutputImg;
|
||||
vector<string> outputLayerName{"345","403", "461","output" };
|
||||
net.forward(netOutputImg, outputLayerName[3]); //获取output的输出
|
||||
//net.forward(netOutputImg, net.getUnconnectedOutLayersNames());
|
||||
std::vector<int> classIds;//结果id数组
|
||||
std::vector<float> confidences;//结果每个id对应置信度数组
|
||||
std::vector<cv::Rect> boxes;//每个id矩形框
|
||||
int net_width = _className.size() + 5; //输出的网络宽度是类别数+5
|
||||
int net_out_width = netOutputImg[0].size[2];
|
||||
assert(net_out_width == net_width, "Error Wrong number of _className"); //模型类别数目不对
|
||||
float* pdata = (float*)netOutputImg[0].data;
|
||||
int net_height = netOutputImg[0].size[1];
|
||||
for (int r = 0; r < net_height; ++r) {
|
||||
float box_score = pdata[4]; ;//获取每一行的box框中含有某个物体的概率
|
||||
if (box_score >= _classThreshold) {
|
||||
cv::Mat scores(1, _className.size(), CV_32FC1, pdata + 5);
|
||||
Point classIdPoint;
|
||||
double max_class_socre;
|
||||
minMaxLoc(scores, 0, &max_class_socre, 0, &classIdPoint);
|
||||
max_class_socre = max_class_socre * box_score;
|
||||
if (max_class_socre >= _classThreshold) {
|
||||
//rect [x,y,w,h]
|
||||
float x = (pdata[0] - params[2]) / params[0];
|
||||
float y = (pdata[1] - params[3]) / params[1];
|
||||
float w = pdata[2] / params[0];
|
||||
float h = pdata[3] / params[1];
|
||||
int left = MAX(round(x - 0.5 * w + 0.5), 0);
|
||||
int top = MAX(round(y - 0.5 * h + 0.5), 0);
|
||||
classIds.push_back(classIdPoint.x);
|
||||
confidences.push_back(max_class_socre);
|
||||
boxes.push_back(Rect(left, top, int(w + 0.5), int(h + 0.5)));
|
||||
}
|
||||
}
|
||||
pdata += net_width;//下一行
|
||||
|
||||
}
|
||||
|
||||
//执行非极大值抑制（NMS）以消除具有较低置信度的冗余重叠框
|
||||
vector<int> nms_result;
|
||||
NMSBoxes(boxes, confidences, _classThreshold, _nmsThreshold, nms_result);
|
||||
for (int i = 0; i < nms_result.size(); i++) {
|
||||
int idx = nms_result[i];
|
||||
Output result;
|
||||
result.id = classIds[idx];
|
||||
result.confidence = confidences[idx];
|
||||
result.box = boxes[idx];
|
||||
output.push_back(result);
|
||||
}
|
||||
if (output.size())
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
void Yolov5::drawPred(Mat& img, vector<Output> result, vector<Scalar> color) {
|
||||
for (int i = 0; i < result.size(); i++) {
|
||||
int left, top;
|
||||
left = result[i].box.x;
|
||||
top = result[i].box.y;
|
||||
int color_num = i;
|
||||
rectangle(img, result[i].box, color[result[i].id], 2, 8);
|
||||
|
||||
string label = _className[result[i].id] + ":" + to_string(result[i].confidence);
|
||||
|
||||
int baseLine;
|
||||
Size labelSize = getTextSize(label, FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);
|
||||
top = max(top, labelSize.height);
|
||||
//rectangle(frame, Point(left, top - int(1.5 * labelSize.height)), Point(left + int(1.5 * labelSize.width), top + baseLine), Scalar(0, 255, 0), FILLED);
|
||||
putText(img, label, Point(left, top), FONT_HERSHEY_SIMPLEX, 1, color[result[i].id], 2);
|
||||
}
|
||||
//imshow("1", img);
|
||||
//imwrite("out.bmp", img);
|
||||
//waitKey();
|
||||
//destroyAllWindows();
|
||||
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
## Yolov5s+SORT(卡尔曼+匈牙利)
|
||||
|
||||
利用目标跟踪对检测框进行预测的技术，对目标检测任务进行加速，在yolov5检测的帧之间穿插使用卡尔曼滤波进行目标检测框的预测，减少对模型的调用，实现目标检测帧率提升的目的。
|
||||
|
||||
#### 代码运行环境：
|
||||
|
||||
| 开发环境 | rk3588 |
|
||||
| -------- | -------------------- |
|
||||
| OpenCV | opencv-3.4.3-windows |
|
||||
| 调用模型 | YOLOv5s.rknn |
|
||||
|
||||
|
||||
|
||||
#### 主要代码设置：
|
||||
|
||||
环境配置：
|
||||
|
||||
```
|
||||
进入到ROS环境主目录中
|
||||
catkin_make #编译程序
|
||||
source devel/setup.bash #更新ROS工作环境变量
|
||||
rosrun sort_test sortmain #运行SORT加速检测代码
|
||||
sudo cat /sys/kernel/debug/rknpu/load #查看npu占用
|
||||
```
|
||||
|
||||
#### 消耗时间分析
|
||||
|
||||
对于摄像头显示的画面（双目摄像头）
|
||||
|
||||
读取摄像头画面需要花费1~3ms，60~100ms，（两种极端，忽快忽慢）
|
||||
|
||||
检测需要花费39.6ms（40ms上下幅度，±不超过5ms）
|
||||
|
||||
标记检测框需要花费0.05ms（多个putText的操作，消耗时间可忽略不计）
|
||||
|
||||
双目匹配花费50~140ms（出现波动）
|
||||
|
||||
帧率显示大约花费0.03ms（单个putText的操作，消耗时间可忽略不计）
|
||||
|
||||
imgshow大约花费3.5ms（单帧图像imgshow的操作）
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,33 @@
|
|||
/**
|
||||
* @file Timer.h
|
||||
* @author hcy
|
||||
* @brief 用于计时
|
||||
* @version 1
|
||||
* @date 2023-04-10
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
#include <iostream>
|
||||
#include <chrono>
|
||||
|
||||
class Timer
|
||||
{
|
||||
public:
|
||||
Timer()
|
||||
{
|
||||
start = std::chrono::high_resolution_clock::now();
|
||||
};
|
||||
~Timer()
|
||||
{
|
||||
end = std::chrono::high_resolution_clock::now();
|
||||
duration = end - start;
|
||||
double ms = duration.count() * 1000.f;
|
||||
std::cout << "Timer took " << ms << "ms" << std::endl;
|
||||
};
|
||||
|
||||
private:
|
||||
std::chrono::time_point<std::chrono::system_clock> start;
|
||||
std::chrono::time_point<std::chrono::system_clock> end;
|
||||
std::chrono::duration<double> duration;
|
||||
};
|
||||
|
|
@ -0,0 +1,48 @@
|
|||
#include <dlfcn.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#define _BASETSD_H
|
||||
|
||||
#include "rknn_api.h"
|
||||
#include "sort_test/preprocess.h"
|
||||
#include "sort_test/postprocess.h"
|
||||
|
||||
|
||||
#define PERF_WITH_POST 1
|
||||
|
||||
class Detection
|
||||
{
|
||||
private:
|
||||
|
||||
int ret;
|
||||
rknn_context ctx;
|
||||
size_t actual_size = 0;
|
||||
int img_width = 0;
|
||||
int img_height = 0;
|
||||
int img_channel = 0;
|
||||
const float nms_threshold = NMS_THRESH; // 默认的NMS阈值
|
||||
const float box_conf_threshold = BOX_THRESH; // 默认的置信度阈值
|
||||
char* model_name = "/home/firefly/pibot_ros/ros_ws/src/sort_test/model/yolov5s-640-640.rknn";
|
||||
|
||||
cv::Mat orig_img;
|
||||
rknn_input_output_num io_num;
|
||||
|
||||
int channel = 3;
|
||||
int width = 0;
|
||||
int height = 0;
|
||||
|
||||
rknn_input inputs[1];
|
||||
|
||||
std::vector<float> out_scales;
|
||||
std::vector<int32_t> out_zps;
|
||||
|
||||
public:
|
||||
Detection();
|
||||
static unsigned char *load_data(FILE *fp, size_t ofst, size_t sz);
|
||||
static unsigned char *load_model(const char *filename, int *model_size);
|
||||
detect_result_group_t outputParse(cv::Mat netInputImg);
|
||||
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,40 @@
|
|||
///
|
||||
// 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
|
||||
//
|
||||
///
|
||||
|
||||
#pragma once
|
||||
#ifndef HUNGARIAN_H
|
||||
#define HUNGARIAN_H
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
|
||||
class HungarianAlgorithm
|
||||
{
|
||||
public:
|
||||
HungarianAlgorithm();
|
||||
~HungarianAlgorithm();
|
||||
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
|
||||
|
|
@ -0,0 +1,74 @@
|
|||
#pragma once
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
|
||||
/**
|
||||
* @brief KalmanBoxTracker 卡尔曼跟踪器
|
||||
*/
|
||||
class KalmanBoxTracker
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief KalmanBoxTracker 卡尔曼跟踪器类构造函数
|
||||
* @param bbox 检测框
|
||||
*/
|
||||
KalmanBoxTracker(std::vector<int> bbox);
|
||||
|
||||
/**
|
||||
* @brief update 通过观测的检测框更新系统状态
|
||||
* @param bbox 检测框
|
||||
*/
|
||||
void update(std::vector<int> bbox);
|
||||
|
||||
/**
|
||||
* @brief predict 估计预测的边界框
|
||||
*/
|
||||
std::vector<float> predict();
|
||||
|
||||
/**
|
||||
* @brief get_state 返回当前检测框状态
|
||||
*/
|
||||
std::vector<float> get_state();
|
||||
|
||||
public:
|
||||
/**
|
||||
* @param m_kf 卡尔曼滤波器
|
||||
*/
|
||||
cv::KalmanFilter* m_kf;
|
||||
|
||||
/**
|
||||
* @param m_time_since_update 从更新开始的时间（帧数）
|
||||
*/
|
||||
int m_time_since_update;
|
||||
|
||||
/**
|
||||
* @param count 卡尔曼跟踪器的个数
|
||||
*/
|
||||
static int count;
|
||||
|
||||
/**
|
||||
* @param m_id 卡尔曼跟踪器的id
|
||||
*/
|
||||
int m_id;
|
||||
|
||||
/**
|
||||
* @param m_history 历史检测框的储存
|
||||
*/
|
||||
std::vector<std::vector<float>> m_history;
|
||||
|
||||
/**
|
||||
* @param m_hits
|
||||
*/
|
||||
int m_hits;
|
||||
|
||||
/**
|
||||
* @param m_hit_streak 忽略目标初始的若干帧
|
||||
*/
|
||||
int m_hit_streak;
|
||||
|
||||
/**
|
||||
* @param m_age predict被调用的次数计数
|
||||
*/
|
||||
int m_age;
|
||||
};
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#ifndef _upbot_vision_POSTPROCESS_H_
|
||||
#define _upbot_vision_POSTPROCESS_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <vector>
|
||||
|
||||
#define OBJ_NAME_MAX_SIZE 16
|
||||
#define OBJ_NUMB_MAX_SIZE 64
|
||||
#define OBJ_CLASS_NUM 80
|
||||
#define NMS_THRESH 0.45
|
||||
#define BOX_THRESH 0.25
|
||||
#define PROP_BOX_SIZE (5 + OBJ_CLASS_NUM)
|
||||
|
||||
typedef struct _BOX_RECT
|
||||
{
|
||||
int left;
|
||||
int right;
|
||||
int top;
|
||||
int bottom;
|
||||
} BOX_RECT;
|
||||
|
||||
typedef struct __detect_result_t
|
||||
{
|
||||
char name[OBJ_NAME_MAX_SIZE];
|
||||
BOX_RECT box;
|
||||
float prop;
|
||||
} detect_result_t;
|
||||
|
||||
typedef struct _detect_result_group_t
|
||||
{
|
||||
int id;
|
||||
int count;
|
||||
detect_result_t results[OBJ_NUMB_MAX_SIZE];
|
||||
} detect_result_group_t;
|
||||
|
||||
int post_process(int8_t *input0, int8_t *input1, int8_t *input2, int model_in_h, int model_in_w,
|
||||
float conf_threshold, float nms_threshold, BOX_RECT pads, float scale_w, float scale_h,
|
||||
std::vector<int32_t> &qnt_zps, std::vector<float> &qnt_scales,
|
||||
detect_result_group_t *group);
|
||||
|
||||
void deinitPostProcess();
|
||||
#endif //_upbot_vision_POSTPROCESS_H_
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
#ifndef _upbot_vision_PREPROCESS_H_
|
||||
#define _upbot_vision_PREPROCESS_H_
|
||||
|
||||
#include <stdio.h>
|
||||
#include "im2d.h"
|
||||
#include "rga.h"
|
||||
#include "opencv2/core/core.hpp"
|
||||
#include "opencv2/imgcodecs.hpp"
|
||||
#include "opencv2/imgproc.hpp"
|
||||
#include "sort_test/postprocess.h"
|
||||
|
||||
void letterbox(const cv::Mat &image, cv::Mat &padded_image, BOX_RECT &pads, const float scale, const cv::Size &target_size, const cv::Scalar &pad_color = cv::Scalar(128, 128, 128));
|
||||
|
||||
int resize_rga(rga_buffer_t &src, rga_buffer_t &dst, const cv::Mat &image, cv::Mat &resized_image, const cv::Size &target_size);
|
||||
|
||||
#endif //_upbot_vision_PREPROCESS_H_
|
||||
|
|
@ -0,0 +1,102 @@
|
|||
#pragma once
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/calib3d.hpp>
|
||||
|
||||
#include <cstdlib>
|
||||
#include <vector>
|
||||
#include "sort_test/detection.h"
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
using namespace cv;
|
||||
|
||||
class Ranging
|
||||
{
|
||||
private:
|
||||
// rknn_handle hdx;
|
||||
cv::VideoCapture vcapture;
|
||||
Detection yolov5s;
|
||||
/* 2-stereo
|
||||
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||
4.791153893499601e+02, 0, 0,
|
||||
0,4.798785696902847e+02, 0,
|
||||
3.195422157433843e+02, 2.355081129251360e+02, 1);
|
||||
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||
4.851946576927952e+02, 0, 0,
|
||||
0,4.848412081674973e+02, 0,
|
||||
3.139237321746737e+02, 2.316610217516937e+02, 1);
|
||||
Mat distortion_l = (Mat_<double>(1, 5) <<0.091215339806518,-0.088601421082219, 0,
|
||||
0, 0);
|
||||
|
||||
Mat distortion_r = (Mat_<double>(1, 5) <<0.086266675232625,-0.070869167707634, 0,
|
||||
0, 0);
|
||||
Mat rotate = (Mat_<double>(3, 3) <<
|
||||
0.999999240684794, -5.243648990073547e-05, -0.001231210888060,
|
||||
5.272610271745578e-05, 0.999999970951700, -0.002653487630467,
|
||||
0.001231198519510, -2.352594617066257e-04,0.999999214401287);
|
||||
Mat trans = (Mat_<double>(3, 1) <<
|
||||
-61.636981845981540, -1.107000282904877, -1.084419989523733);
|
||||
*/
|
||||
/*
|
||||
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||
4.869084743604942e+02, 0, 0,
|
||||
0,4.859921620798602e+02, 0,
|
||||
2.813183643903652e+02, 2.267657091677993e+02, 1);
|
||||
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||
4.859133331805883e+02, 0, 0,
|
||||
0,4.850356748771951e+02, 0,
|
||||
2.970046483040089e+02, 2.324763397214774e+02, 1);
|
||||
Mat distortion_l = (Mat_<double>(1, 5) <<0.121235284781974,-0.161097849662596, 0,
|
||||
0, 0);
|
||||
|
||||
Mat distortion_r = (Mat_<double>(1, 5) <<0.105479235005840,-0.120347246815955, 0,
|
||||
0, 0);
|
||||
Mat rotate = (Mat_<double>(3, 3) <<
|
||||
0.999921984818601, 3.753847738839353e-04, -0.012485325894835,
|
||||
-4.085449515452996e-04, 0.999996396040715, -0.002653487630467,
|
||||
0.012484284819374, 0.002658381435011,0.999918534502034);
|
||||
Mat trans = (Mat_<double>(3, 1) <<
|
||||
-60.319997608188590, -0.019664800882533, -0.638993708428792);
|
||||
*/
|
||||
// 3_stereo:
|
||||
Mat cam_matrix_left = (Mat_<double>(3, 3) <<
|
||||
725.8008655, 0, 606.83544339,
|
||||
0, 725.71074058, 355.60009442,
|
||||
0, 0, 1);
|
||||
Mat cam_matrix_right = (Mat_<double>(3, 3) <<
|
||||
720.47514477, 0, 611.91679085,
|
||||
0, 720.93965216, 328.55158146,
|
||||
0, 0, 1);
|
||||
Mat distortion_l = (Mat_<double>(1, 5) <<
|
||||
0.0794014, -0.07158048, -0.00233537, -0.00538731, -0.07631366);
|
||||
|
||||
Mat distortion_r = (Mat_<double>(1, 5) <<
|
||||
0.10602397, -0.12121708, -0.0039814, -0.00316623, -0.06940564);
|
||||
Mat rotate = (Mat_<double>(3, 3) <<
|
||||
9.99952249e-01, -3.99415019e-04, -9.76426160e-03,
|
||||
4.05975519e-04, 9.99999693e-01, 6.69916194e-04,
|
||||
9.76399103e-03, -6.73848256e-04, 9.99952104e-01);
|
||||
Mat trans = (Mat_<double>(3, 1) <<
|
||||
-45.23755445, -0.59995078, -3.05666663);
|
||||
cv::Mat mapX1, mapX2, mapY1, mapY2, q, Z;
|
||||
|
||||
int imgw, imgh;
|
||||
detect_result_group_t detect_result_group;
|
||||
cv::Mat info;
|
||||
cv::Mat empty;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
Ranging(){};
|
||||
Ranging(int imgw, int imgh);
|
||||
void rectifyImage(cv::Mat &oriImgL, cv::Mat &oriImgR);
|
||||
void getInfo(cv::Mat &imgl, cv::Mat &imgr, cv::Mat &detBoxes, cv::Mat &info);
|
||||
std::vector<float> pic2cam(int u, int v);
|
||||
std::vector<int> muban(cv::Mat &left_image, cv::Mat &right_image, const int *coordinates);
|
||||
std::vector<cv::Mat> get_range(int frame_num, int detect_interval);
|
||||
void horizon_estimate(cv::Mat& img, cv::Mat& bboxs,int k);
|
||||
|
||||
};
|
||||
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
|
||||
#pragma once
|
||||
#include <iostream>
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include "utils.h"
|
||||
#include "kalmanboxtracker.h"
|
||||
#include "sort_test/postprocess.h"
|
||||
|
||||
|
||||
/**
|
||||
* @brief Sort Sort<EFBFBD>㷨
|
||||
*/
|
||||
class Sort
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Sort <EFBFBD><EFBFBD><EFBFBD>캯<EFBFBD><EFBFBD>
|
||||
* @param max_age δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param min_hits δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param iou_threshold iou<EFBFBD><EFBFBD>ֵ
|
||||
*/
|
||||
Sort(int max_age, int min_hits, float iou_threshold);
|
||||
|
||||
/**
|
||||
* @brief update <EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param dets <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
std::vector<std::vector<float>> update(std::vector<cv::Rect> dets);
|
||||
|
||||
/**
|
||||
* @brief detect_update <EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD><EFBFBD><EFBFBD>
|
||||
* @param detect_result_group <EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
_detect_result_group_t detect_update(_detect_result_group_t detect_result_group);
|
||||
|
||||
private:
|
||||
/**
|
||||
* @param m_max_age δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
int m_max_age;
|
||||
|
||||
/**
|
||||
* @param m_min_hits δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
int m_min_hits;
|
||||
|
||||
/**
|
||||
* @param m_iou_threshold iou<EFBFBD><EFBFBD>ֵ
|
||||
*/
|
||||
float m_iou_threshold;
|
||||
|
||||
/**
|
||||
* @param m_trackers <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD>
|
||||
*/
|
||||
std::vector<KalmanBoxTracker> m_trackers;
|
||||
|
||||
/**
|
||||
* @param m_frame_count ֡<EFBFBD><EFBFBD>
|
||||
*/
|
||||
int m_frame_count;
|
||||
};
|
||||
|
|
@ -0,0 +1,37 @@
|
|||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/Range.h>
|
||||
#include <boost/asio.hpp>
|
||||
#include <boost/bind.hpp>
|
||||
#include <boost/thread.hpp>
|
||||
#include "pibot_msgs/avoid.h"
|
||||
#include <libusb-1.0/libusb.h>
|
||||
|
||||
|
||||
|
||||
class UltrasonicNode {
|
||||
public:
|
||||
ros::NodeHandle nh;
|
||||
ros::Publisher ultrasonic_pub;
|
||||
libusb_device_handle *dev_handle;
|
||||
libusb_device **devs;
|
||||
libusb_context *ctx = NULL;
|
||||
int r;
|
||||
ssize_t cnt;
|
||||
float range;
|
||||
// boost::asio::io_service io;
|
||||
// boost::asio::serial_port serial;
|
||||
|
||||
public:
|
||||
// UltrasonicNode() : nh("~"), serial(io), ultrasonic_pub(nh.advertise<sensor_msgs::Range>("ultrasonic", 1)) {
|
||||
// std::string port_name;
|
||||
// nh.param<std::string>("port", port_name, "/dev/ttyUSB0");
|
||||
// serial.open(port_name);
|
||||
// serial.set_option(boost::asio::serial_port_base::baud_rate(9600));
|
||||
// }
|
||||
UltrasonicNode();
|
||||
~UltrasonicNode();
|
||||
|
||||
|
||||
void readUltrasonic();
|
||||
};
|
||||
|
||||
|
|
@ -0,0 +1,69 @@
|
|||
#pragma once
|
||||
#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);
|
||||
Binary file not shown.
|
|
@ -0,0 +1,201 @@
|
|||
#include "sort_test/detection.h"
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
|
||||
unsigned char * Detection::load_data(FILE *fp, size_t ofst, size_t sz)
|
||||
{
|
||||
unsigned char *data;
|
||||
int ret;
|
||||
|
||||
data = NULL;
|
||||
|
||||
if (NULL == fp)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
ret = fseek(fp, ofst, SEEK_SET);
|
||||
if (ret != 0)
|
||||
{
|
||||
printf("blob seek failure.\n");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
data = (unsigned char *)malloc(sz);
|
||||
if (data == NULL)
|
||||
{
|
||||
printf("buffer malloc failure.\n");
|
||||
return NULL;
|
||||
}
|
||||
ret = fread(data, 1, sz, fp);
|
||||
return data;
|
||||
}
|
||||
|
||||
unsigned char * Detection::load_model(const char *filename, int *model_size)
|
||||
{
|
||||
FILE *fp;
|
||||
unsigned char *data;
|
||||
|
||||
fp = fopen(filename, "rb");
|
||||
if (NULL == fp)
|
||||
{
|
||||
printf("Open file %s failed.\n", filename);
|
||||
return NULL;
|
||||
}
|
||||
|
||||
fseek(fp, 0, SEEK_END);
|
||||
int size = ftell(fp);
|
||||
|
||||
data = load_data(fp, 0, size);
|
||||
|
||||
fclose(fp);
|
||||
|
||||
*model_size = size;
|
||||
return data;
|
||||
}
|
||||
|
||||
Detection::Detection()
|
||||
{
|
||||
|
||||
/* Create the neural network */
|
||||
printf("Loading mode...\n");
|
||||
int model_data_size = 0;
|
||||
unsigned char *model_data = load_model(model_name, &model_data_size);
|
||||
ret = rknn_init(&ctx, model_data, model_data_size, 0, NULL);
|
||||
if (ret < 0)
|
||||
{
|
||||
printf("rknn_init error ret=%d\n", ret);
|
||||
}
|
||||
|
||||
rknn_sdk_version version;
|
||||
ret = rknn_query(ctx, RKNN_QUERY_SDK_VERSION, &version, sizeof(rknn_sdk_version));
|
||||
if (ret < 0)
|
||||
{
|
||||
printf("rknn_init error ret=%d\n", ret);
|
||||
}
|
||||
printf("sdk version: %s driver version: %s\n", version.api_version, version.drv_version);
|
||||
|
||||
ret = rknn_query(ctx, RKNN_QUERY_IN_OUT_NUM, &io_num, sizeof(io_num));
|
||||
if (ret < 0)
|
||||
{
|
||||
printf("rknn_init error ret=%d\n", ret);
|
||||
}
|
||||
printf("model input num: %d, output num: %d\n", io_num.n_input, io_num.n_output);
|
||||
|
||||
rknn_tensor_attr input_attrs[io_num.n_input];
|
||||
memset(input_attrs, 0, sizeof(input_attrs));
|
||||
for (int i = 0; i < io_num.n_input; i++)
|
||||
{
|
||||
input_attrs[i].index = i;
|
||||
ret = rknn_query(ctx, RKNN_QUERY_INPUT_ATTR, &(input_attrs[i]), sizeof(rknn_tensor_attr));
|
||||
if (ret < 0)
|
||||
{
|
||||
printf("rknn_init error ret=%d\n", ret);
|
||||
}
|
||||
// dump_tensor_attr(&(input_attrs[i]));
|
||||
}
|
||||
|
||||
rknn_tensor_attr output_attrs[io_num.n_output];
|
||||
memset(output_attrs, 0, sizeof(output_attrs));
|
||||
for (int i = 0; i < io_num.n_output; i++)
|
||||
{
|
||||
output_attrs[i].index = i;
|
||||
ret = rknn_query(ctx, RKNN_QUERY_OUTPUT_ATTR, &(output_attrs[i]), sizeof(rknn_tensor_attr));
|
||||
// dump_tensor_attr(&(output_attrs[i]));
|
||||
}
|
||||
|
||||
for (int i = 0; i < io_num.n_output; ++i)
|
||||
{
|
||||
out_scales.push_back(output_attrs[i].scale);
|
||||
out_zps.push_back(output_attrs[i].zp);
|
||||
}
|
||||
|
||||
if (input_attrs[0].fmt == RKNN_TENSOR_NCHW)
|
||||
{
|
||||
printf("model is NCHW input fmt\n");
|
||||
channel = input_attrs[0].dims[1];
|
||||
height = input_attrs[0].dims[2];
|
||||
width = input_attrs[0].dims[3];
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("model is NHWC input fmt\n");
|
||||
height = input_attrs[0].dims[1];
|
||||
width = input_attrs[0].dims[2];
|
||||
channel = input_attrs[0].dims[3];
|
||||
}
|
||||
|
||||
printf("model input height=%d, width=%d, channel=%d\n", height, width, channel);
|
||||
|
||||
|
||||
memset(inputs, 0, sizeof(inputs));
|
||||
inputs[0].index = 0;
|
||||
inputs[0].type = RKNN_TENSOR_UINT8;
|
||||
inputs[0].size = width * height * channel;
|
||||
inputs[0].fmt = RKNN_TENSOR_NHWC;
|
||||
inputs[0].pass_through = 0;
|
||||
|
||||
}
|
||||
|
||||
detect_result_group_t Detection::outputParse(cv::Mat netInputImg)
|
||||
{
|
||||
|
||||
orig_img = netInputImg.clone();
|
||||
|
||||
if (!orig_img.data)
|
||||
{
|
||||
printf("no image\n");
|
||||
}
|
||||
cv::Mat img;
|
||||
cv::cvtColor(orig_img, img, cv::COLOR_BGR2RGB);
|
||||
img_width = img.cols;
|
||||
img_height = img.rows;
|
||||
// printf("img width = %d, img height = %d\n", img_width, img_height);
|
||||
|
||||
// 指定目标大小和预处理方式,默认使用LetterBox的预处理
|
||||
BOX_RECT pads;
|
||||
memset(&pads, 0, sizeof(BOX_RECT));
|
||||
cv::Size target_size(width, height);
|
||||
cv::Mat resized_img(target_size.height, target_size.width, CV_8UC3);
|
||||
// 计算缩放比例
|
||||
float scale_w = (float)target_size.width / img.cols;
|
||||
float scale_h = (float)target_size.height / img.rows;
|
||||
|
||||
if (img_width != width || img_height != height)
|
||||
{
|
||||
// printf("resize image with letterbox\n");
|
||||
float min_scale = std::min(scale_w, scale_h);
|
||||
scale_w = min_scale;
|
||||
scale_h = min_scale;
|
||||
letterbox(img, resized_img, pads, min_scale, target_size);
|
||||
// 保存预处理图片
|
||||
// cv::imwrite("letterbox_input.jpg", resized_img);
|
||||
inputs[0].buf = resized_img.data;
|
||||
}
|
||||
else
|
||||
{
|
||||
inputs[0].buf = img.data;
|
||||
}
|
||||
|
||||
rknn_inputs_set(ctx, io_num.n_input, inputs);
|
||||
|
||||
rknn_output outputs[io_num.n_output];
|
||||
memset(outputs, 0, sizeof(outputs));
|
||||
for (int i = 0; i < io_num.n_output; i++)
|
||||
{
|
||||
outputs[i].want_float = 0;
|
||||
}
|
||||
|
||||
// 执行推理
|
||||
ret = rknn_run(ctx, NULL);
|
||||
ret = rknn_outputs_get(ctx, io_num.n_output, outputs, NULL);
|
||||
|
||||
// 后处理
|
||||
detect_result_group_t detect_result_group;
|
||||
|
||||
post_process((int8_t *)outputs[0].buf, (int8_t *)outputs[1].buf, (int8_t *)outputs[2].buf, height, width,
|
||||
box_conf_threshold, nms_threshold, pads, scale_w, scale_h, out_zps, out_scales, &detect_result_group);
|
||||
|
||||
rknn_outputs_release(ctx, io_num.n_output,outputs);
|
||||
return detect_result_group;
|
||||
}
|
||||
|
|
@ -0,0 +1,389 @@
|
|||
//
|
||||
// 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 <stdlib.h>
|
||||
#include <cfloat> // for DBL_MAX
|
||||
#include <cmath> // for fabs()
|
||||
#include "sort_test/hungarian.h"
|
||||
|
||||
|
||||
HungarianAlgorithm::HungarianAlgorithm() {}
|
||||
HungarianAlgorithm::~HungarianAlgorithm() {}
|
||||
|
||||
|
||||
//********************************************************//
|
||||
// A single function wrapper for solving assignment problem.
|
||||
//********************************************************//
|
||||
float HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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 HungarianAlgorithm::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);
|
||||
}
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
#include "sort_test/utils.h"
|
||||
#include "sort_test/kalmanboxtracker.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);
|
||||
}
|
||||
|
|
@ -0,0 +1,152 @@
|
|||
#include <iostream>
|
||||
#include <stdio.h>
|
||||
#include <ros/ros.h>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include "sort_test/ranging.h"
|
||||
#include <string>
|
||||
#include <pthread.h>
|
||||
#include <atomic>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <sys/time.h>
|
||||
#include "opencv2/imgcodecs/imgcodecs.hpp"
|
||||
#include <ctime>
|
||||
#include <queue>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
#include <cstdlib>
|
||||
#include "pibot_msgs/dis_info.h"
|
||||
#include "pibot_msgs/dis_info_array.h"
|
||||
|
||||
#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
|
||||
|
||||
Ranging r(640, 480);
|
||||
std::atomic<bool> running(true);
|
||||
std::queue<std::vector<cv::Mat>> frameInfo;
|
||||
std::mutex g_mutex;
|
||||
int detect_interval = 1;
|
||||
|
||||
void *ranging(void *args) // ranging线程
|
||||
{
|
||||
int64 t = getTickCount();
|
||||
ros::Publisher dis_pub_;
|
||||
ros::NodeHandle nh;
|
||||
// <>内为自定义ros消息类型 存储障碍物信息包括与障碍物的距离、障碍物宽度、高度和与障碍物之间的夹角
|
||||
dis_pub_ = nh.advertise<pibot_msgs::dis_info_array>("ceju_info",10);
|
||||
// std::ofstream distance_file("distance_data.txt", std::ios::out);
|
||||
// if (!distance_file.is_open())
|
||||
// {
|
||||
// std::cerr << "Error opening distance_data.txt" << std::endl;
|
||||
// return nullptr;
|
||||
// }
|
||||
int frame_num = 0;
|
||||
while (true)
|
||||
{
|
||||
/*进入测距并发布障碍物信息*/;
|
||||
std::vector<cv::Mat> result = r.get_range(frame_num, detect_interval);
|
||||
cv ::Mat info = result[2];
|
||||
pibot_msgs::dis_info_array dis_array;
|
||||
if(!info.empty())
|
||||
{
|
||||
for(int i=0;i<info.rows;i++)
|
||||
{
|
||||
// std::cout<<"1"<<std::endl;
|
||||
if(info.at<float>(i,0)>0&&info.at<float>(i,0)<200)
|
||||
{
|
||||
pibot_msgs::dis_info data;
|
||||
data.distance = info.at<float>(i,0);
|
||||
data.width = info.at<float>(i,1);
|
||||
data.height = info.at<float>(i,2);
|
||||
data.angle = info.at<float>(i,3);
|
||||
dis_array.dis.push_back(data);
|
||||
// std::cout<<data.distance<<std::endl;
|
||||
// distance_file << "Distance: " << data.distance << ", Width: " << data.width
|
||||
// << ", Height: " << data.height << ", Angle: " << data.angle << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
dis_pub_.publish(dis_array);
|
||||
}
|
||||
g_mutex.lock();
|
||||
for (uchar i = 0; i < frameInfo.size(); i++) // 只保存当前最新的图片帧信息
|
||||
frameInfo.pop();
|
||||
frameInfo.push(result);
|
||||
g_mutex.unlock();
|
||||
frame_num++;
|
||||
|
||||
if(frame_num%100==0){
|
||||
int64 tt = getTickCount() - t;
|
||||
std::cout << frame_num << " frames cost time:"<< tt/cv::getTickFrequency()<< "s" << std::endl;
|
||||
}
|
||||
}
|
||||
t = getTickCount() - t;
|
||||
std::cout << "all frames cost time:"<< t/cv::getTickFrequency() << std::endl;
|
||||
// distance_file.close();
|
||||
}
|
||||
|
||||
int kbhit(void) {
|
||||
struct termios oldt, newt;
|
||||
int ch;
|
||||
int oldf;
|
||||
|
||||
tcgetattr(STDIN_FILENO, &oldt);
|
||||
newt = oldt;
|
||||
newt.c_lflag &= ~(ICANON | ECHO);
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
||||
oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
|
||||
fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
|
||||
|
||||
ch = getchar();
|
||||
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
|
||||
fcntl(STDIN_FILENO, F_SETFL, oldf);
|
||||
|
||||
if (ch != EOF) {
|
||||
ungetc(ch, stdin);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void createDirectory(const char* path) {
|
||||
if (mkdir(path, 0777) == -1) {
|
||||
std::cerr << "Error : " << strerror(errno) << std::endl;
|
||||
} else {
|
||||
std::cout << "Directory created: " << path << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
createDirectory("imgGrayL");
|
||||
createDirectory("imgGrayR");
|
||||
createDirectory("coordinates");
|
||||
ros::init(argc, argv, "stereo");
|
||||
ros::NodeHandle nh;
|
||||
pthread_t tids[1]; // 执行ranging线程
|
||||
int ret = pthread_create(&tids[0], NULL, ranging, NULL);
|
||||
std::cout << "Press 'q' to stop the thread." << std::endl;
|
||||
|
||||
// 主线程检测按键输入
|
||||
while (running) {
|
||||
if (kbhit()) {
|
||||
char ch = getchar();
|
||||
if (ch == 'q') {
|
||||
std::cout << "Stopping thread..." << std::endl;
|
||||
running = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
pthread_join(tids[0],NULL);
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,373 @@
|
|||
// Copyright (c) 2021 by Rockchip Electronics Co., Ltd. All Rights Reserved.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "sort_test/postprocess.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <sys/time.h>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
#define LABEL_NALE_TXT_PATH "/home/firefly/pibot_ros/ros_ws/src/upbot_vision/model/coco_80_labels_list.txt"
|
||||
|
||||
static char *labels[OBJ_CLASS_NUM];
|
||||
|
||||
const int anchor0[6] = {10, 13, 16, 30, 33, 23};
|
||||
const int anchor1[6] = {30, 61, 62, 45, 59, 119};
|
||||
const int anchor2[6] = {116, 90, 156, 198, 373, 326};
|
||||
|
||||
inline static int clamp(float val, int min, int max) { return val > min ? (val < max ? val : max) : min; }
|
||||
|
||||
char *readLine(FILE *fp, char *buffer, int *len)
|
||||
{
|
||||
int ch;
|
||||
int i = 0;
|
||||
size_t buff_len = 0;
|
||||
|
||||
buffer = (char *)malloc(buff_len + 1);
|
||||
if (!buffer)
|
||||
return NULL; // Out of memory
|
||||
|
||||
while ((ch = fgetc(fp)) != '\n' && ch != EOF)
|
||||
{
|
||||
buff_len++;
|
||||
void *tmp = realloc(buffer, buff_len + 1);
|
||||
if (tmp == NULL)
|
||||
{
|
||||
free(buffer);
|
||||
return NULL; // Out of memory
|
||||
}
|
||||
buffer = (char *)tmp;
|
||||
|
||||
buffer[i] = (char)ch;
|
||||
i++;
|
||||
}
|
||||
buffer[i] = '\0';
|
||||
|
||||
*len = buff_len;
|
||||
|
||||
// Detect end
|
||||
if (ch == EOF && (i == 0 || ferror(fp)))
|
||||
{
|
||||
free(buffer);
|
||||
return NULL;
|
||||
}
|
||||
return buffer;
|
||||
}
|
||||
|
||||
int readLines(const char *fileName, char *lines[], int max_line)
|
||||
{
|
||||
FILE *file = fopen(fileName, "r");
|
||||
char *s;
|
||||
int i = 0;
|
||||
int n = 0;
|
||||
|
||||
if (file == NULL)
|
||||
{
|
||||
printf("Open %s fail!\n", fileName);
|
||||
return -1;
|
||||
}
|
||||
|
||||
while ((s = readLine(file, s, &n)) != NULL)
|
||||
{
|
||||
lines[i++] = s;
|
||||
if (i >= max_line)
|
||||
break;
|
||||
}
|
||||
fclose(file);
|
||||
return i;
|
||||
}
|
||||
|
||||
int loadLabelName(const char *locationFilename, char *label[])
|
||||
{
|
||||
printf("loadLabelName %s\n", locationFilename);
|
||||
readLines(locationFilename, label, OBJ_CLASS_NUM);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static float CalculateOverlap(float xmin0, float ymin0, float xmax0, float ymax0, float xmin1, float ymin1, float xmax1,
|
||||
float ymax1)
|
||||
{
|
||||
float w = fmax(0.f, fmin(xmax0, xmax1) - fmax(xmin0, xmin1) + 1.0);
|
||||
float h = fmax(0.f, fmin(ymax0, ymax1) - fmax(ymin0, ymin1) + 1.0);
|
||||
float i = w * h;
|
||||
float u = (xmax0 - xmin0 + 1.0) * (ymax0 - ymin0 + 1.0) + (xmax1 - xmin1 + 1.0) * (ymax1 - ymin1 + 1.0) - i;
|
||||
return u <= 0.f ? 0.f : (i / u);
|
||||
}
|
||||
|
||||
static int nms(int validCount, std::vector<float> &outputLocations, std::vector<int> classIds, std::vector<int> &order,
|
||||
int filterId, float threshold)
|
||||
{
|
||||
for (int i = 0; i < validCount; ++i)
|
||||
{
|
||||
if (order[i] == -1 || classIds[i] != filterId)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
int n = order[i];
|
||||
for (int j = i + 1; j < validCount; ++j)
|
||||
{
|
||||
int m = order[j];
|
||||
if (m == -1 || classIds[i] != filterId)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
float xmin0 = outputLocations[n * 4 + 0];
|
||||
float ymin0 = outputLocations[n * 4 + 1];
|
||||
float xmax0 = outputLocations[n * 4 + 0] + outputLocations[n * 4 + 2];
|
||||
float ymax0 = outputLocations[n * 4 + 1] + outputLocations[n * 4 + 3];
|
||||
|
||||
float xmin1 = outputLocations[m * 4 + 0];
|
||||
float ymin1 = outputLocations[m * 4 + 1];
|
||||
float xmax1 = outputLocations[m * 4 + 0] + outputLocations[m * 4 + 2];
|
||||
float ymax1 = outputLocations[m * 4 + 1] + outputLocations[m * 4 + 3];
|
||||
|
||||
float iou = CalculateOverlap(xmin0, ymin0, xmax0, ymax0, xmin1, ymin1, xmax1, ymax1);
|
||||
|
||||
if (iou > threshold)
|
||||
{
|
||||
order[j] = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int quick_sort_indice_inverse(std::vector<float> &input, int left, int right, std::vector<int> &indices)
|
||||
{
|
||||
float key;
|
||||
int key_index;
|
||||
int low = left;
|
||||
int high = right;
|
||||
if (left < right)
|
||||
{
|
||||
key_index = indices[left];
|
||||
key = input[left];
|
||||
while (low < high)
|
||||
{
|
||||
while (low < high && input[high] <= key)
|
||||
{
|
||||
high--;
|
||||
}
|
||||
input[low] = input[high];
|
||||
indices[low] = indices[high];
|
||||
while (low < high && input[low] >= key)
|
||||
{
|
||||
low++;
|
||||
}
|
||||
input[high] = input[low];
|
||||
indices[high] = indices[low];
|
||||
}
|
||||
input[low] = key;
|
||||
indices[low] = key_index;
|
||||
quick_sort_indice_inverse(input, left, low - 1, indices);
|
||||
quick_sort_indice_inverse(input, low + 1, right, indices);
|
||||
}
|
||||
return low;
|
||||
}
|
||||
|
||||
static float sigmoid(float x) { return 1.0 / (1.0 + expf(-x)); }
|
||||
|
||||
static float unsigmoid(float y) { return -1.0 * logf((1.0 / y) - 1.0); }
|
||||
|
||||
inline static int32_t __clip(float val, float min, float max)
|
||||
{
|
||||
float f = val <= min ? min : (val >= max ? max : val);
|
||||
return f;
|
||||
}
|
||||
|
||||
static int8_t qnt_f32_to_affine(float f32, int32_t zp, float scale)
|
||||
{
|
||||
float dst_val = (f32 / scale) + zp;
|
||||
int8_t res = (int8_t)__clip(dst_val, -128, 127);
|
||||
return res;
|
||||
}
|
||||
|
||||
static float deqnt_affine_to_f32(int8_t qnt, int32_t zp, float scale) { return ((float)qnt - (float)zp) * scale; }
|
||||
|
||||
static int process(int8_t *input, int *anchor, int grid_h, int grid_w, int height, int width, int stride,
|
||||
std::vector<float> &boxes, std::vector<float> &objProbs, std::vector<int> &classId, float threshold,
|
||||
int32_t zp, float scale)
|
||||
{
|
||||
int validCount = 0;
|
||||
int grid_len = grid_h * grid_w;
|
||||
int8_t thres_i8 = qnt_f32_to_affine(threshold, zp, scale);
|
||||
for (int a = 0; a < 3; a++)
|
||||
{
|
||||
for (int i = 0; i < grid_h; i++)
|
||||
{
|
||||
for (int j = 0; j < grid_w; j++)
|
||||
{
|
||||
int8_t box_confidence = input[(PROP_BOX_SIZE * a + 4) * grid_len + i * grid_w + j];
|
||||
if (box_confidence >= thres_i8)
|
||||
{
|
||||
int offset = (PROP_BOX_SIZE * a) * grid_len + i * grid_w + j;
|
||||
int8_t *in_ptr = input + offset;
|
||||
float box_x = (deqnt_affine_to_f32(*in_ptr, zp, scale)) * 2.0 - 0.5;
|
||||
float box_y = (deqnt_affine_to_f32(in_ptr[grid_len], zp, scale)) * 2.0 - 0.5;
|
||||
float box_w = (deqnt_affine_to_f32(in_ptr[2 * grid_len], zp, scale)) * 2.0;
|
||||
float box_h = (deqnt_affine_to_f32(in_ptr[3 * grid_len], zp, scale)) * 2.0;
|
||||
box_x = (box_x + j) * (float)stride;
|
||||
box_y = (box_y + i) * (float)stride;
|
||||
box_w = box_w * box_w * (float)anchor[a * 2];
|
||||
box_h = box_h * box_h * (float)anchor[a * 2 + 1];
|
||||
box_x -= (box_w / 2.0);
|
||||
box_y -= (box_h / 2.0);
|
||||
|
||||
int8_t maxClassProbs = in_ptr[5 * grid_len];
|
||||
int maxClassId = 0;
|
||||
for (int k = 1; k < OBJ_CLASS_NUM; ++k)
|
||||
{
|
||||
int8_t prob = in_ptr[(5 + k) * grid_len];
|
||||
if (prob > maxClassProbs)
|
||||
{
|
||||
maxClassId = k;
|
||||
maxClassProbs = prob;
|
||||
}
|
||||
}
|
||||
if (maxClassProbs > thres_i8)
|
||||
{
|
||||
objProbs.push_back((deqnt_affine_to_f32(maxClassProbs, zp, scale)) * (deqnt_affine_to_f32(box_confidence, zp, scale)));
|
||||
classId.push_back(maxClassId);
|
||||
validCount++;
|
||||
boxes.push_back(box_x);
|
||||
boxes.push_back(box_y);
|
||||
boxes.push_back(box_w);
|
||||
boxes.push_back(box_h);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return validCount;
|
||||
}
|
||||
|
||||
int post_process(int8_t *input0, int8_t *input1, int8_t *input2, int model_in_h, int model_in_w, float conf_threshold,
|
||||
float nms_threshold, BOX_RECT pads, float scale_w, float scale_h, std::vector<int32_t> &qnt_zps,
|
||||
std::vector<float> &qnt_scales, detect_result_group_t *group)
|
||||
{
|
||||
static int init = -1;
|
||||
if (init == -1)
|
||||
{
|
||||
int ret = 0;
|
||||
ret = loadLabelName(LABEL_NALE_TXT_PATH, labels);
|
||||
if (ret < 0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
init = 0;
|
||||
}
|
||||
memset(group, 0, sizeof(detect_result_group_t));
|
||||
|
||||
std::vector<float> filterBoxes;
|
||||
std::vector<float> objProbs;
|
||||
std::vector<int> classId;
|
||||
|
||||
// stride 8
|
||||
int stride0 = 8;
|
||||
int grid_h0 = model_in_h / stride0;
|
||||
int grid_w0 = model_in_w / stride0;
|
||||
int validCount0 = 0;
|
||||
validCount0 = process(input0, (int *)anchor0, grid_h0, grid_w0, model_in_h, model_in_w, stride0, filterBoxes, objProbs,
|
||||
classId, conf_threshold, qnt_zps[0], qnt_scales[0]);
|
||||
|
||||
// stride 16
|
||||
int stride1 = 16;
|
||||
int grid_h1 = model_in_h / stride1;
|
||||
int grid_w1 = model_in_w / stride1;
|
||||
int validCount1 = 0;
|
||||
validCount1 = process(input1, (int *)anchor1, grid_h1, grid_w1, model_in_h, model_in_w, stride1, filterBoxes, objProbs,
|
||||
classId, conf_threshold, qnt_zps[1], qnt_scales[1]);
|
||||
|
||||
// stride 32
|
||||
int stride2 = 32;
|
||||
int grid_h2 = model_in_h / stride2;
|
||||
int grid_w2 = model_in_w / stride2;
|
||||
int validCount2 = 0;
|
||||
validCount2 = process(input2, (int *)anchor2, grid_h2, grid_w2, model_in_h, model_in_w, stride2, filterBoxes, objProbs,
|
||||
classId, conf_threshold, qnt_zps[2], qnt_scales[2]);
|
||||
|
||||
int validCount = validCount0 + validCount1 + validCount2;
|
||||
// no object detect
|
||||
if (validCount <= 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::vector<int> indexArray;
|
||||
for (int i = 0; i < validCount; ++i)
|
||||
{
|
||||
indexArray.push_back(i);
|
||||
}
|
||||
|
||||
quick_sort_indice_inverse(objProbs, 0, validCount - 1, indexArray);
|
||||
|
||||
std::set<int> class_set(std::begin(classId), std::end(classId));
|
||||
|
||||
for (auto c : class_set)
|
||||
{
|
||||
nms(validCount, filterBoxes, classId, indexArray, c, nms_threshold);
|
||||
}
|
||||
|
||||
int last_count = 0;
|
||||
group->count = 0;
|
||||
/* box valid detect target */
|
||||
for (int i = 0; i < validCount; ++i)
|
||||
{
|
||||
if (indexArray[i] == -1 || last_count >= OBJ_NUMB_MAX_SIZE)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
int n = indexArray[i];
|
||||
|
||||
float x1 = filterBoxes[n * 4 + 0] - pads.left;
|
||||
float y1 = filterBoxes[n * 4 + 1] - pads.top;
|
||||
float x2 = x1 + filterBoxes[n * 4 + 2];
|
||||
float y2 = y1 + filterBoxes[n * 4 + 3];
|
||||
int id = classId[n];
|
||||
float obj_conf = objProbs[i];
|
||||
|
||||
group->results[last_count].box.left = (int)(clamp(x1, 0, model_in_w) / scale_w);
|
||||
group->results[last_count].box.top = (int)(clamp(y1, 0, model_in_h) / scale_h);
|
||||
group->results[last_count].box.right = (int)(clamp(x2, 0, model_in_w) / scale_w);
|
||||
group->results[last_count].box.bottom = (int)(clamp(y2, 0, model_in_h) / scale_h);
|
||||
group->results[last_count].prop = obj_conf;
|
||||
char *label = labels[id];
|
||||
strncpy(group->results[last_count].name, label, OBJ_NAME_MAX_SIZE);
|
||||
|
||||
// printf("result %2d: (%4d, %4d, %4d, %4d), %s\n", i, group->results[last_count].box.left,
|
||||
// group->results[last_count].box.top,
|
||||
// group->results[last_count].box.right, group->results[last_count].box.bottom, label);
|
||||
last_count++;
|
||||
}
|
||||
group->count = last_count;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void deinitPostProcess()
|
||||
{
|
||||
for (int i = 0; i < OBJ_CLASS_NUM; i++)
|
||||
{
|
||||
if (labels[i] != nullptr)
|
||||
{
|
||||
free(labels[i]);
|
||||
labels[i] = nullptr;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
// Copyright (c) 2023 by Rockchip Electronics Co., Ltd. All Rights Reserved.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "sort_test/preprocess.h"
|
||||
|
||||
void letterbox(const cv::Mat &image, cv::Mat &padded_image, BOX_RECT &pads, const float scale, const cv::Size &target_size, const cv::Scalar &pad_color)
|
||||
{
|
||||
// 调整图像大小
|
||||
cv::Mat resized_image;
|
||||
cv::resize(image, resized_image, cv::Size(), scale, scale);
|
||||
|
||||
// 计算填充大小
|
||||
int pad_width = target_size.width - resized_image.cols;
|
||||
int pad_height = target_size.height - resized_image.rows;
|
||||
|
||||
pads.left = pad_width / 2;
|
||||
pads.right = pad_width - pads.left;
|
||||
pads.top = pad_height / 2;
|
||||
pads.bottom = pad_height - pads.top;
|
||||
|
||||
// 在图像周围添加填充
|
||||
cv::copyMakeBorder(resized_image, padded_image, pads.top, pads.bottom, pads.left, pads.right, cv::BORDER_CONSTANT, pad_color);
|
||||
}
|
||||
|
||||
int resize_rga(rga_buffer_t &src, rga_buffer_t &dst, const cv::Mat &image, cv::Mat &resized_image, const cv::Size &target_size)
|
||||
{
|
||||
im_rect src_rect;
|
||||
im_rect dst_rect;
|
||||
memset(&src_rect, 0, sizeof(src_rect));
|
||||
memset(&dst_rect, 0, sizeof(dst_rect));
|
||||
size_t img_width = image.cols;
|
||||
size_t img_height = image.rows;
|
||||
if (image.type() != CV_8UC3)
|
||||
{
|
||||
printf("source image type is %d!\n", image.type());
|
||||
return -1;
|
||||
}
|
||||
size_t target_width = target_size.width;
|
||||
size_t target_height = target_size.height;
|
||||
src = wrapbuffer_virtualaddr((void *)image.data, img_width, img_height, RK_FORMAT_RGB_888);
|
||||
dst = wrapbuffer_virtualaddr((void *)resized_image.data, target_width, target_height, RK_FORMAT_RGB_888);
|
||||
int ret = imcheck(src, dst, src_rect, dst_rect);
|
||||
if (IM_STATUS_NOERROR != ret)
|
||||
{
|
||||
fprintf(stderr, "rga check error! %s", imStrError((IM_STATUS)ret));
|
||||
return -1;
|
||||
}
|
||||
IM_STATUS STATUS = imresize(src, dst);
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,490 @@
|
|||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
#include <iostream>
|
||||
#include <stdio.h>
|
||||
#include "sort_test/ranging.h"
|
||||
#include "sort_test/kalmanboxtracker.h"
|
||||
#include "sort_test/sort.h"
|
||||
#include "sort_test/postprocess.h"
|
||||
#include <math.h>
|
||||
#include <stdlib.h>
|
||||
#include <fstream>
|
||||
#include <string.h>
|
||||
#include <sys/time.h>
|
||||
#include <dlfcn.h>
|
||||
#include "opencv2/core.hpp"
|
||||
#include "opencv2/imgcodecs.hpp"
|
||||
#include "opencv2/imgproc.hpp"
|
||||
#include <filesystem>
|
||||
#include <sys/stat.h>
|
||||
#include <sys/types.h>
|
||||
#include <dirent.h>
|
||||
#include <vector>
|
||||
#include <tuple>
|
||||
#include <string>
|
||||
#include <string.h>
|
||||
#include <algorithm>
|
||||
#include <ctime>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
|
||||
// #include <ros/ros.h>
|
||||
|
||||
|
||||
using namespace cv;
|
||||
|
||||
|
||||
std::string getTimestamp() {
|
||||
std::ostringstream oss;
|
||||
std::time_t t = std::time(nullptr);
|
||||
std::tm tm = *std::localtime(&t);
|
||||
oss << std::put_time(&tm, "%Y%m%d_%H%M%S");
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
// 保存图像和坐标文件
|
||||
void saveData(const cv::Mat& imgL, const cv::Mat& imgR, const int coords[4]) {
|
||||
std::string baseName = getTimestamp();
|
||||
std::string pathL = "imgGrayL/" + baseName + ".png";
|
||||
std::string pathR = "imgGrayR/" + baseName + ".png";
|
||||
std::string pathCoords = "coordinates/" + baseName + ".txt";
|
||||
|
||||
cv::imwrite(pathL, imgL);
|
||||
cv::imwrite(pathR, imgR);
|
||||
|
||||
std::ofstream file(pathCoords);
|
||||
if (file.is_open()) {
|
||||
file << coords[0] << " " << coords[1] << " " << coords[2] << " " << coords[3];
|
||||
file.close();
|
||||
}
|
||||
}
|
||||
|
||||
void Ranging::rectifyImage(Mat &oriImgL, Mat &oriImgR) //重映射函数
|
||||
{
|
||||
remap(oriImgL, oriImgL, mapX1, mapX2, cv::INTER_LINEAR);
|
||||
remap(oriImgR, oriImgR, mapY1, mapY2, cv::INTER_LINEAR);
|
||||
}
|
||||
|
||||
std::vector<float> Ranging::pic2cam(int u, int v) //像素坐标转相机坐标
|
||||
{
|
||||
//(u,v)->(x,y)"(loc[0],loc[1])"
|
||||
std::vector<float> loc;
|
||||
loc.push_back((u - cam_matrix_right.at<double>(0, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(0, 0));
|
||||
loc.push_back((v - cam_matrix_right.at<double>(1, 2)) * q.at<double>(2, 3) / cam_matrix_right.at<double>(1, 1));
|
||||
return loc;
|
||||
}
|
||||
|
||||
std::vector<int> Ranging::muban(Mat &left_image, Mat &right_image, const int *coordinates) //模板匹配
|
||||
{
|
||||
int x1 = coordinates[0], y1 = coordinates[1], x2 = coordinates[2], y2 = coordinates[3];
|
||||
Mat tpl = right_image.rowRange(max(y1 - 2, 0), min(y2 + 2, 479)).colRange(x1, x2); //获取目标框
|
||||
Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639); //待匹配图像，极线约束，只需要同水平区域
|
||||
int th = tpl.rows, tw = tpl.cols;
|
||||
Mat result;
|
||||
|
||||
matchTemplate(target, tpl, result, TM_CCOEFF_NORMED); //匹配方法：归一化相关系数即零均值归一化互相关
|
||||
double minVal, maxVal;
|
||||
Point minLoc, maxLoc;
|
||||
minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc); //得到匹配点坐标
|
||||
Point tl = maxLoc, br;
|
||||
|
||||
br.x = min(maxLoc.x + tw, 639); //转为像素坐标系
|
||||
br.y = min(maxLoc.y + th, 479); //转为像素坐标系
|
||||
//展示匹配结果
|
||||
br.x = min(maxLoc.x + tw, 319);
|
||||
br.y = min(maxLoc.y + th, 239);
|
||||
rectangle(target, tl, br, (0, 255, 0), 3);
|
||||
imshow("match-", target);
|
||||
waitKey(2);
|
||||
|
||||
std::vector<int> maxloc;
|
||||
maxloc.push_back(maxLoc.x);
|
||||
maxloc.push_back(maxLoc.y);
|
||||
return maxloc;
|
||||
}
|
||||
|
||||
void Ranging::horizon_estimate(Mat& img, Mat& bboxs,int k)
|
||||
{
|
||||
//保证摄像头与地面平行
|
||||
int x1 = bboxs.at<float>(k, 0);
|
||||
int x2 = bboxs.at<float>(k, 2);
|
||||
int y1 = bboxs.at<float>(k, 1);
|
||||
int y2 = bboxs.at<float>(k, 3);
|
||||
float Conf = bboxs.at<float>(k, 4);
|
||||
int cls = bboxs.at<float>(k, 5);
|
||||
float Y_B, Y_H;
|
||||
cv::Mat edge, grayImage;
|
||||
std::vector<cv::Point> idx;
|
||||
cv::Mat tpl = img.rowRange(y1, min(y2+5,479)).colRange(x1, x2); // 取感兴趣范围
|
||||
//Mat target = left_image.rowRange(max(y1 - 20, 0), min(y2 + 20, 479)).colRange(0, 639);
|
||||
cv::Mat Z = Mat::zeros(2, tpl.cols, CV_32FC1);
|
||||
cvtColor(tpl, grayImage, COLOR_BGR2GRAY);
|
||||
GaussianBlur(grayImage,grayImage,Size(5,5),0);
|
||||
Canny(grayImage, edge, 120, 180, 3); //提取边缘，获取与地面接触点
|
||||
//cv::imshow("1",edge);
|
||||
//cv::waitKey(1);
|
||||
float cluster[650];
|
||||
for (int i = 0;i<650;i++)
|
||||
{
|
||||
cluster[i] = 0;
|
||||
}
|
||||
int y_b, y_h;
|
||||
int j = 0;
|
||||
for (int i = 0; i < x2-x1; i++)
|
||||
{
|
||||
//Mat temp = edge.rowRange(max(y1, 0), min(y2 + 4, 479)).colRange(x1, x2);
|
||||
Mat temp = edge.col(i); //取第i列
|
||||
// std::cout << "temp: " <<temp << std::endl;
|
||||
cv::findNonZero(temp, idx);
|
||||
std::vector<float> point_b = pic2cam(x1 + i, 240); //转为相机坐标系
|
||||
std::vector<float> point_H = pic2cam(320, 240);
|
||||
float alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
|
||||
if (idx.size() < 1)
|
||||
{
|
||||
Z.at<float>(0, i) = 0;
|
||||
Z.at<float>(1, i) = alfa;
|
||||
continue;
|
||||
}
|
||||
int y_b = idx[idx.size() - 1].y + y1; //
|
||||
y_b = int(y_b + y_b*0.03);
|
||||
int y_h = 240;
|
||||
point_b = pic2cam(x1 + i, y_b); //转为相机坐标系
|
||||
point_H = pic2cam(320, y_h);
|
||||
Y_B = point_b[1];
|
||||
Y_H = point_H[1];
|
||||
|
||||
float H_c = 60; //摄像头离地高度，单位mm
|
||||
float theta = 0; //摄像头与地面夹角，弧度
|
||||
float d = (1/cos(theta)* cos(theta)) * q.at<double>(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta);
|
||||
alfa = atan((point_b[0] - point_H[0]) / q.at<double>(2, 3));
|
||||
//cout << "d: " << d << endl;
|
||||
if (d > 700)
|
||||
{d = 0;}
|
||||
Z.at<float>(0, i) = d/cos(alfa);
|
||||
Z.at<float>(1, i) = alfa;
|
||||
}
|
||||
|
||||
this->Z = Z.clone();
|
||||
}
|
||||
|
||||
void Ranging::getInfo(Mat &imgL, Mat &imgR, Mat &detBoxes, Mat &info)
|
||||
{
|
||||
Mat imgGrayL, imgGrayR;
|
||||
cvtColor(imgL, imgGrayL, COLOR_BGR2GRAY);
|
||||
cvtColor(imgR, imgGrayR, COLOR_BGR2GRAY);
|
||||
Mat imgR_weight = imgR.clone();
|
||||
Mat infoRow;
|
||||
for (uchar i = 0; i < detBoxes.rows; i++)
|
||||
{
|
||||
int x1 = detBoxes.at<float>(i, 0);
|
||||
int y1 = detBoxes.at<float>(i, 1);
|
||||
int x2 = detBoxes.at<float>(i, 2);
|
||||
int y2 = detBoxes.at<float>(i, 3);
|
||||
|
||||
float Conf = detBoxes.at<float>(i, 4);
|
||||
|
||||
if (x1 > 600 || x2 < 50 || y1 < 5 || y2 > 475 || x1 < 2 || x2 > 590 || abs(x2 - x1) > 550) //当目标框偏左、偏右或者过大，略去该物体
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// rectangle(imgR, Point(int(x1), int(y1)),
|
||||
// Point(int(x2), int(y2)), Scalar(0, 0, 255)); //绘制目标框
|
||||
|
||||
int coordinates[4] = {x1, y1, x2, y2};
|
||||
saveData(imgGrayL, imgGrayR, coordinates);
|
||||
std::vector<int> disp_pixel = muban(imgGrayL, imgGrayR, coordinates); //模板匹配
|
||||
float disp_pixel_x = disp_pixel[0] - x1; //ˮ计算水平视差
|
||||
float disp_pixel_y = disp_pixel[1] - y1; //
|
||||
disp_pixel_x = (int)(disp_pixel_x + disp_pixel_x * 0.12); //0.12为模板匹配产生的误差，为经验值，通过拟合得到
|
||||
|
||||
//Mat disp_matrix = Mat(1, 1, CV_32F, Scalar(disp_pixel_x)), disp_pixel_xyz;
|
||||
Mat disp_matrix = Mat(imgGrayL.rows, imgGrayL.cols, CV_32F, Scalar(disp_pixel_x)); //定义视差矩阵，所有值均为水平视差，方便转换为三维坐标，并具有水平距离信息
|
||||
Mat threed_pixel_xyz, threedImage;
|
||||
reprojectImageTo3D(disp_matrix, threedImage, q, false); //2d->3d
|
||||
threed_pixel_xyz = threedImage.mul(threedImage); //每一像素点求平方，
|
||||
std::vector<Mat> channels;
|
||||
split(threed_pixel_xyz.clone(), channels);
|
||||
threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //计算欧式距离
|
||||
threed_pixel_xyz.forEach<float>([](float &value, const int *position) { value = sqrt(value); }); // 获得距离d
|
||||
|
||||
int mid_pixel = int((x1 + x2) / 2);
|
||||
std::vector<float> mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //计算角度，从像素坐标转为相机坐标
|
||||
std::vector<float> loc_tar = pic2cam(mid_pixel, imgGrayR.rows);
|
||||
float alfa = atan((loc_tar[0] - mid[0]) / q.at<double>(2, 3));
|
||||
|
||||
|
||||
if (disp_pixel_x > 240) // 距离太近，视差过大
|
||||
{
|
||||
char cm[15];
|
||||
//sprintf(cm, "cannot match !");
|
||||
// sprintf(cm, "%d , %.2f", cls,Conf);
|
||||
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||
infoRow.copyTo(info.row(i));
|
||||
continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
float median = threed_pixel_xyz.at<float>((int)(y1 + y2) / 2, (int)(x1 + x2) / 2);
|
||||
|
||||
std::vector<float> ltPoint = pic2cam(x1, y1);
|
||||
std::vector<float> rbPoint = pic2cam(x2, y2);
|
||||
float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //计算宽高
|
||||
float f = q.at<double>(2, 3);
|
||||
float f1 = sqrt(xx1 * xx1 + yy1 * yy1 + f * f); //推导得出
|
||||
//float w1 = median * sqrt((xx1 - xx2) * (xx1 - xx2) / 4) / f1;
|
||||
float h1 = median * sqrt((yy1 - yy2) * (yy1 - yy2) / 4) / f1;
|
||||
float f2 = sqrt(xx2 * xx2 + yy2 * yy2 + f * f);
|
||||
//float w2 = median * sqrt((xx2 - xx1) * (xx2 - xx1) / 4) / f2;
|
||||
float h2 = median * sqrt((yy2 - yy1) * (yy2 - yy1) / 4) / f2;
|
||||
float w1 = sqrt(pow((threedImage.at<cv::Vec3f>(y2, x1)[0] - threedImage.at<cv::Vec3f>(y2, x2)[0]), 2) +
|
||||
pow((threedImage.at<cv::Vec3f>(y2, x1)[1] - threedImage.at<cv::Vec3f>(y2, x2)[1]), 2) +
|
||||
pow((threedImage.at<cv::Vec3f>(y2, x1)[2] - threedImage.at<cv::Vec3f>(y2, x2)[2]), 2));
|
||||
|
||||
w1 = w1 / 10;
|
||||
h1 = (h1 + h2) / 10;
|
||||
median /= 10;
|
||||
if (median > 120) //过远测距误差较大
|
||||
{
|
||||
//char tf[9];
|
||||
//sprintf(tf, "Too far!");
|
||||
char cm[15];
|
||||
//sprintf(cm, "cannot match !");
|
||||
// sprintf(cm, "%d , %.2f", cls,Conf);
|
||||
putText(imgR, cm, Point((x1), (y1)), FONT_HERSHEY_PLAIN, 2.2, Scalar(0, 0, 255), 2);
|
||||
infoRow = (Mat_<float>(1, 4) << -1, -1, -1, -1);
|
||||
infoRow.copyTo(info.row(i));
|
||||
continue;
|
||||
}
|
||||
// <20><>ͼ<EFBFBD><CDBC><EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD><EFBFBD><EFBFBD>Ϣ
|
||||
// char dc[50], wh[50];
|
||||
// std::string cname = className[cls + 1];
|
||||
// sprintf(dc, "dis:%.2fcm %d", median, cls);
|
||||
// sprintf(wh, "W: %.2fcm H: %.2fcm alfa: %2f", w1, h1, alfa);
|
||||
// putText(imgR, dc, Point(x1, y2), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 2);
|
||||
// putText(imgR, wh, Point(x1, y1), FONT_HERSHEY_PLAIN, 1.5, Scalar(0, 0, 255), 1.5);
|
||||
|
||||
|
||||
//返回数据
|
||||
infoRow = (Mat_<float>(1, 4) << median, w1, h1, alfa);
|
||||
infoRow.copyTo(info.row(i));
|
||||
};
|
||||
}
|
||||
// cv::imshow("kk",imgR);
|
||||
// cv::waitKey(1);
|
||||
|
||||
}
|
||||
|
||||
Ranging::Ranging(int imgw, int imgh) : //初始化
|
||||
mapX1(imgh, imgw, CV_64F), //初始化矩阵 ，用于计算无畸变和修正转换映射。
|
||||
mapX2(imgh, imgw, CV_64F),
|
||||
mapY1(imgh, imgw, CV_64F),
|
||||
mapY2(imgh, imgw, CV_64F),
|
||||
q(4, 4, CV_64F),
|
||||
imgw(imgw),
|
||||
imgh(imgh)
|
||||
{
|
||||
// Z = Mat::zeros(2, 1, CV_32FC1);
|
||||
// int camera_index;
|
||||
// for (camera_index = 0; camera_index < 10; camera_index++) {
|
||||
// cv::VideoCapture cap(camera_index);
|
||||
// if (cap.isOpened()) {
|
||||
// std::cout << "Camera found at index " << camera_index << std::endl;
|
||||
// cap.release(); // 释放设备
|
||||
// } else {
|
||||
// std::cout << "No camera found at index " << camera_index << std::endl;
|
||||
// }
|
||||
// }
|
||||
// vcapture = cv::VideoCapture(camera_index);
|
||||
|
||||
// if (!vcapture.isOpened()) {
|
||||
// vcapture = cv::VideoCapture(41);
|
||||
// }
|
||||
|
||||
// if (!vcapture.isOpened()) {
|
||||
// std::cerr << "Error: Could not open camera." << std::endl;
|
||||
// // 处理打开摄像头失败的情况
|
||||
// return;
|
||||
// }
|
||||
|
||||
// std::cout << "Open camera sucess." << std::endl;
|
||||
|
||||
/* cv::CAP_V4L2是采用V4L2驱动打开摄像头，解决部分摄像头无法打开的情况 */
|
||||
/*if(cv::VideoCapture(0).isOpened()){
|
||||
vcapture.open(0, cv::CAP_V4L2);
|
||||
}
|
||||
else{
|
||||
vcapture.open(40, cv::CAP_V4L2);
|
||||
}
|
||||
//vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG);
|
||||
vcapture.set(cv::CAP_PROP_FPS, 30);
|
||||
vcapture.set(cv::CAP_PROP_FRAME_WIDTH, imgw * 2);
|
||||
vcapture.set(cv::CAP_PROP_FRAME_HEIGHT, imgh);
|
||||
vcapture.set(cv::CAP_PROP_BUFFERSIZE, 1);*/
|
||||
vcapture.open("/home/firefly/pibot_ros/ros_ws/src/sort_test/model/test.mp4");
|
||||
//vcapture.set(CAP_PROP_FOURCC, CAP_OPENCV_MJPEG);
|
||||
|
||||
|
||||
auto imgSize = Size(imgw, imgh);
|
||||
Mat r1(3, 3, CV_64F), r2(3, 3, CV_64F), p1(3, 4, CV_64F), p2(3, 4, CV_64F);
|
||||
|
||||
stereoRectify(cam_matrix_left.t(), distortion_l, cam_matrix_right.t(), distortion_r,
|
||||
imgSize, rotate.t(), trans, r1, r2, p1, p2, q);//立体校正
|
||||
|
||||
initUndistortRectifyMap(cam_matrix_left.t(), distortion_l, r1, p1, imgSize, CV_32F, mapX1, mapX2);//计算无畸变和修正转换映射
|
||||
initUndistortRectifyMap(cam_matrix_right.t(), distortion_r, r2, p2, imgSize, CV_32F, mapY1, mapY2);//计算无畸变和修正转换映射
|
||||
|
||||
// RKNN_Create(&hdx, modelPath); // 初始化检测模型
|
||||
std::cout<< " ******************* CAMERA init Finish ********************" << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
std::vector<Mat> Ranging::get_range(int frame_num, int detect_interval)
|
||||
{
|
||||
|
||||
double rang_old, rang_now;
|
||||
// rang_old = ros::Time::now().toSec(); //测试运行时间
|
||||
Mat frame, lframe, rframe;
|
||||
if (!vcapture.isOpened()) {
|
||||
std::cerr << "get_range:Error: Could not open camera." << std::endl;
|
||||
// 处理打开摄像头失败的情况
|
||||
return std::vector<Mat>{empty};
|
||||
}
|
||||
Sort mot_tracker = Sort(1, 3, 0.3);
|
||||
|
||||
int64 t = getTickCount();
|
||||
|
||||
vcapture >> frame;
|
||||
int64 t1 = getTickCount()-t;
|
||||
|
||||
std::cout << frame_num << " vcapture cost time:"<< t1/cv::getTickFrequency()<< "s" << std::endl;
|
||||
|
||||
if (!frame.empty())
|
||||
{
|
||||
|
||||
/*Mat lframe(frame.colRange(0, imgw).clone()); //拷贝左图
|
||||
Mat rframe(frame.colRange(imgw, imgw * 2).clone()); //拷贝右图
|
||||
|
||||
rectifyImage(lframe, rframe);
|
||||
|
||||
cv::Mat Rframe = rframe.clone();*/
|
||||
|
||||
cv::resize(frame, frame, cv::Size(640, 480)); // 读取本地视频
|
||||
cv::Mat Rframe = frame.clone();
|
||||
|
||||
if(frame_num <= 3 || frame_num % detect_interval == 0){
|
||||
detect_result_group = yolov5s.outputParse(Rframe);
|
||||
}
|
||||
else{
|
||||
detect_result_group = mot_tracker.detect_update(detect_result_group);
|
||||
}
|
||||
|
||||
|
||||
if (detect_result_group.count<=0)
|
||||
{
|
||||
// std::cout<<"detect nothing"<<std::endl;
|
||||
}
|
||||
|
||||
// detction box transfor to our format
|
||||
Mat detBoxes(detect_result_group.count, 5, CV_32F); //定义矩阵，存储目标检测内容，存储格式(x,y,x,y,conf,cls)
|
||||
/*
|
||||
char text[256];
|
||||
int count = 0;
|
||||
|
||||
for (int i = 0; i < detect_result_group.count; i++) //存储目标检测内容 (x,y,x,y,conf,cls)
|
||||
{
|
||||
detect_result_t *det_result = &(detect_result_group.results[i]);
|
||||
|
||||
//if(strcmp(det_result->name, "vase") == 0)
|
||||
if(strcmp(det_result->name, "vase") == 0|| strcmp(det_result->name, "suitcase") == 0||
|
||||
strcmp(det_result->name, "person") == 0|| strcmp(det_result->name, "keyboard") == 0||
|
||||
strcmp(det_result->name, "mouse") == 0|| strcmp(det_result->name, "bowl") == 0||
|
||||
strcmp(det_result->name, "cup") == 0|| strcmp(det_result->name, "bottle")==0)
|
||||
{
|
||||
count++;
|
||||
sprintf(text, "%s %.1f%%", det_result->name, det_result->prop * 100);
|
||||
// printf("%s @ (%d %d %d %d) %f\n", det_result->name, det_result->box.left, det_result->box.top,
|
||||
// det_result->box.right, det_result->box.bottom, det_result->prop);
|
||||
if(det_result->prop <=0.2)
|
||||
continue;
|
||||
|
||||
int xmin = det_result->box.left;
|
||||
int ymin = det_result->box.top;
|
||||
int xmax = det_result->box.right;
|
||||
int ymax = det_result->box.bottom;
|
||||
|
||||
|
||||
rectangle(Rframe, cv::Point(xmin, ymin), cv::Point(xmax, ymax), cv::Scalar(256, 0, 0, 256), 3);
|
||||
putText(Rframe, text, cv::Point(xmin, ymin + 12), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
|
||||
|
||||
// (x,y) (x,y) conf
|
||||
|
||||
detBoxes.at<float>(count-1, 0) = xmin;
|
||||
detBoxes.at<float>(count-1, 1) = ymin;
|
||||
detBoxes.at<float>(count-1, 2) = xmax;
|
||||
detBoxes.at<float>(count-1, 3) = ymax;
|
||||
detBoxes.at<float>(count-1, 4) = det_result->prop;
|
||||
// detBoxes.at<float>(i, 0) = xmin;
|
||||
// detBoxes.at<float>(i, 1) = ymin;
|
||||
// detBoxes.at<float>(i, 2) = xmax;
|
||||
// detBoxes.at<float>(i, 3) = ymax;
|
||||
// detBoxes.at<float>(i, 4) = det_result->prop;
|
||||
}
|
||||
// 实验测试，过滤过大的误检框
|
||||
// float ratio = (xmax - xmin) * (ymax - ymin) / 308480.;
|
||||
// if (ratio > 0.7)
|
||||
// {
|
||||
// detBoxes.at<float>(i, 5) = -1;
|
||||
// continue;
|
||||
// }
|
||||
}
|
||||
Mat finalBoxes(detBoxes, Range(0, count), Range::all());
|
||||
Mat info(count, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α
|
||||
// Mat info(detect_result_group.count, 4, CV_32F); // 存储测距信息，存储格式：距离d，宽w，高h，角度α
|
||||
if (count)
|
||||
// if (detect_result_group.count)
|
||||
{
|
||||
// getInfo(lframe, rframe, finalBoxes, info);
|
||||
getInfo(lframe, rframe, finalBoxes, info);
|
||||
|
||||
for(int i=0; i<info.rows;i++)
|
||||
{
|
||||
float median = info.at<float>(i, 0);
|
||||
// float w1 = info.at<float>(i, 1);
|
||||
// float h1 = info.at<float>(i, 2);
|
||||
// float alfa = info.at<float>(i, 3);
|
||||
|
||||
int x1 = finalBoxes.at<float>(i,0);
|
||||
int y1 = finalBoxes.at<float>(i,1);
|
||||
int x2 = finalBoxes.at<float>(i,2);
|
||||
int y2 = finalBoxes.at<float>(i,3);
|
||||
|
||||
// int x1 = detBoxes.at<float>(i,0);
|
||||
// int y1 = detBoxes.at<float>(i,1);
|
||||
// int x2 = detBoxes.at<float>(i,2);
|
||||
// int y2 = detBoxes.at<float>(i,3);
|
||||
char dc[50], wh[50];
|
||||
sprintf(dc, "dis:%.2fcm ", median);
|
||||
// sprintf(wh, "W: %.2fcm H: %.2fcm ", w1, h1);
|
||||
putText(Rframe, dc, Point(x1, y2), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
|
||||
// putText(Rframe, wh, Point(x1, y1), cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255, 255, 255));
|
||||
}
|
||||
}
|
||||
*/
|
||||
t = getTickCount() - t;
|
||||
char fps[50];
|
||||
sprintf(fps, "fps: %d", int(1 / (t / getTickFrequency())));
|
||||
putText(Rframe, fps, Point(20, 20), FONT_HERSHEY_PLAIN, 1, Scalar(0, 0, 255), 1.5);
|
||||
//int64 t = getTickCount();
|
||||
cv::imshow("k",Rframe);
|
||||
cv::waitKey(1);
|
||||
|
||||
return std::vector<Mat>{rframe, detBoxes, info};
|
||||
}
|
||||
return std::vector<Mat>{empty};
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,140 @@
|
|||
#include "sort_test/sort.h"
|
||||
|
||||
|
||||
|
||||
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 {};
|
||||
}
|
||||
|
||||
_detect_result_group_t Sort::detect_update(_detect_result_group_t detect_result_group)
|
||||
{
|
||||
std::vector<cv::Rect> dets;
|
||||
for(int i=0; i<detect_result_group.count;i++){
|
||||
detect_result_t item = detect_result_group.results[i];
|
||||
cv::Rect temp(item.box.left, item.box.top, item.box.right-item.box.left, item.box.bottom-item.box.top);
|
||||
dets.push_back(temp);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
for(int i=0; i<detect_result_group.count; i++){
|
||||
// cv::Rect rect(trackers[i][0], trackers[i][1], trackers[i][2]-trackers[i][0], trackers[i][3]-trackers[i][1]);
|
||||
detect_result_group.results[i].box.left = ret[i][0];
|
||||
detect_result_group.results[i].box.top = ret[i][1];
|
||||
detect_result_group.results[i].box.right = ret[i][2];
|
||||
detect_result_group.results[i].box.bottom = ret[i][3];
|
||||
}
|
||||
|
||||
if (ret.size() > 0)
|
||||
return detect_result_group;
|
||||
|
||||
return {};
|
||||
}
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
#include "sort_test/ultrasonic.h"
|
||||
|
||||
UltrasonicNode::UltrasonicNode(){
|
||||
r = libusb_init(&ctx); // 初始化libusb
|
||||
if (r < 0)
|
||||
{
|
||||
ROS_ERROR("Failed to initialize libusb");
|
||||
return ;
|
||||
}
|
||||
|
||||
cnt = libusb_get_device_list(ctx, &devs); // 获取设备列表
|
||||
if (cnt < 0)
|
||||
{
|
||||
ROS_ERROR("Failed to get device list");
|
||||
libusb_exit(ctx);
|
||||
return ;
|
||||
}
|
||||
|
||||
dev_handle = libusb_open_device_with_vid_pid(ctx, 0x048d, 0x5750); // 使用指定的vendor和product ID打开设备
|
||||
if (dev_handle == NULL)
|
||||
{
|
||||
ROS_ERROR("Cannot open device");
|
||||
libusb_free_device_list(devs, 1);
|
||||
libusb_exit(ctx);
|
||||
return ;
|
||||
}
|
||||
|
||||
// ROS_INFO("Device opened");
|
||||
|
||||
libusb_free_device_list(devs, 1); // 释放设备列表
|
||||
|
||||
if (libusb_kernel_driver_active(dev_handle, 0) == 1)
|
||||
{ // 检查是否有内核驱动程序激活
|
||||
ROS_INFO("Kernel driver active");
|
||||
if (libusb_detach_kernel_driver(dev_handle, 0) == 0)
|
||||
ROS_INFO("Kernel driver detached");
|
||||
else
|
||||
ROS_WARN("Failed to detach kernel driver");
|
||||
}
|
||||
|
||||
r = libusb_claim_interface(dev_handle, 0); // 请求接口
|
||||
if (r < 0)
|
||||
{
|
||||
ROS_ERROR("Cannot claim interface");
|
||||
libusb_close(dev_handle);
|
||||
libusb_exit(ctx);
|
||||
return ;
|
||||
}
|
||||
|
||||
ROS_INFO("Interface claimed");
|
||||
ultrasonic_pub = nh.advertise<pibot_msgs::avoid>("ultrasonic_data", 10);
|
||||
}
|
||||
UltrasonicNode::~UltrasonicNode(){
|
||||
r = libusb_release_interface(dev_handle, 0); // 释放接口
|
||||
if (r != 0)
|
||||
ROS_ERROR("Cannot release interface");
|
||||
libusb_close(dev_handle); // 关闭设备
|
||||
libusb_exit(ctx);
|
||||
|
||||
}
|
||||
void UltrasonicNode::readUltrasonic() {
|
||||
unsigned char data[64];
|
||||
int transferred;
|
||||
|
||||
while (ros::ok())
|
||||
{
|
||||
r = libusb_interrupt_transfer(dev_handle, (0x01 | LIBUSB_ENDPOINT_IN), data, 64, &transferred, 1000); // 执行批量传输
|
||||
// std::cout<<r<<std::endl;
|
||||
// if (r == 0 && transferred == 64)
|
||||
pibot_msgs::avoid msg;
|
||||
if (r == 0 )
|
||||
{
|
||||
range = *(float *)(data);
|
||||
// uint8_t bool_data = *(uint8_t *)(data + sizeof(float));
|
||||
// bool l1 = bool_data & (1 << 0);
|
||||
// bool l2 = bool_data & (1 << 1);
|
||||
// bool r1 = bool_data & (1 << 2);
|
||||
// bool r2 = bool_data & (1 << 3);
|
||||
|
||||
uint8_t r1 = *(uint8_t*)(data + 4);
|
||||
uint8_t r2 = *(uint8_t*)(data + 5);
|
||||
uint8_t l1 = *(uint8_t*)(data + 6);
|
||||
uint8_t l2 = *(uint8_t*)(data + 7);
|
||||
msg.ultrasonic_range = range;
|
||||
msg.l1 = l1;
|
||||
msg.l2 = l2;
|
||||
msg.r2 = r1;
|
||||
msg.r1 = r2;
|
||||
ultrasonic_pub.publish(msg);
|
||||
ROS_INFO("Received ultrasonic data: %f mm, l1: %d, l2: %d, r1: %d, r2: %d", msg.ultrasonic_range, msg.l1, msg.l2, msg.r1, msg.r2);
|
||||
// ROS_INFO("Magnetic field: %f",ultrasonic_range);
|
||||
}
|
||||
else
|
||||
{
|
||||
ROS_ERROR("Read error");
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "ultrasonic_node");
|
||||
UltrasonicNode ultrasonic_node;
|
||||
ros::Rate rate(10); // 10 Hz
|
||||
|
||||
while (ros::ok()) {
|
||||
ultrasonic_node.readUltrasonic();
|
||||
ros::spinOnce();
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,219 @@
|
|||
#include "sort_test/utils.h"
|
||||
#include "sort_test/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);
|
||||
|
||||
HungarianAlgorithm 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)
|
||||
{
|
||||
if (trackers.size() == 0)
|
||||
{
|
||||
std::vector<int> unmatched_detections(detections.size());
|
||||
std::iota(unmatched_detections.begin(), unmatched_detections.end(), 0);
|
||||
return { {}, unmatched_detections, {} };
|
||||
}
|
||||
|
||||
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 };
|
||||
}
|
||||
Loading…
Reference in New Issue