From bededf80b964fc411633e1d280ed12d018323b12 Mon Sep 17 00:00:00 2001 From: FengHua0208 <1137650198@qq.com> Date: Thu, 21 Nov 2024 14:16:10 +0800 Subject: [PATCH] upbot_sort --- .../include/sort_test/hungarian.h | 40 ++ .../include/sort_test/kalmanboxtracker.h | 74 +++ old_sort_detection/include/sort_test/sort.h | 61 +++ old_sort_detection/include/sort_test/utils.h | 69 +++ old_sort_detection/include/sort_test/yolo.h | 54 ++ old_sort_detection/src/hungarian.cc | 389 ++++++++++++++ old_sort_detection/src/kalmanboxtracker.cc | 97 ++++ old_sort_detection/src/main.cc | 266 ++++++++++ old_sort_detection/src/sort.cc | 140 +++++ old_sort_detection/src/utils.cc | 219 ++++++++ old_sort_detection/src/yolo.cc | 183 +++++++ upbot_SORT/README.md | 47 ++ upbot_SORT/include/sort_test/Timer.h | 33 ++ upbot_SORT/include/sort_test/detection.h | 48 ++ upbot_SORT/include/sort_test/hungarian.h | 40 ++ .../include/sort_test/kalmanboxtracker.h | 74 +++ upbot_SORT/include/sort_test/postprocess.h | 42 ++ upbot_SORT/include/sort_test/preprocess.h | 16 + upbot_SORT/include/sort_test/ranging.h | 102 ++++ upbot_SORT/include/sort_test/sort.h | 62 +++ upbot_SORT/include/sort_test/ultrasonic.h | 37 ++ upbot_SORT/include/sort_test/utils.h | 69 +++ upbot_SORT/src/.ranging.cc.swp | Bin 0 -> 24576 bytes upbot_SORT/src/detection.cc | 201 +++++++ upbot_SORT/src/hungarian.cc | 389 ++++++++++++++ upbot_SORT/src/kalmanboxtracker.cc | 97 ++++ upbot_SORT/src/main.cc | 152 ++++++ upbot_SORT/src/postprocess.cc | 373 +++++++++++++ upbot_SORT/src/preprocess.cc | 61 +++ upbot_SORT/src/ranging.cc | 490 ++++++++++++++++++ upbot_SORT/src/sort.cc | 140 +++++ upbot_SORT/src/ultrasonic.cc | 112 ++++ upbot_SORT/src/utils.cc | 219 ++++++++ 33 files changed, 4396 insertions(+) create mode 100644 old_sort_detection/include/sort_test/hungarian.h create mode 100644 old_sort_detection/include/sort_test/kalmanboxtracker.h create mode 100644 old_sort_detection/include/sort_test/sort.h create mode 100644 old_sort_detection/include/sort_test/utils.h create mode 100644 old_sort_detection/include/sort_test/yolo.h create mode 100644 old_sort_detection/src/hungarian.cc create mode 100644 old_sort_detection/src/kalmanboxtracker.cc create mode 100644 old_sort_detection/src/main.cc create mode 100644 old_sort_detection/src/sort.cc create mode 100644 old_sort_detection/src/utils.cc create mode 100644 old_sort_detection/src/yolo.cc create mode 100644 upbot_SORT/README.md create mode 100644 upbot_SORT/include/sort_test/Timer.h create mode 100644 upbot_SORT/include/sort_test/detection.h create mode 100644 upbot_SORT/include/sort_test/hungarian.h create mode 100644 upbot_SORT/include/sort_test/kalmanboxtracker.h create mode 100644 upbot_SORT/include/sort_test/postprocess.h create mode 100644 upbot_SORT/include/sort_test/preprocess.h create mode 100644 upbot_SORT/include/sort_test/ranging.h create mode 100644 upbot_SORT/include/sort_test/sort.h create mode 100644 upbot_SORT/include/sort_test/ultrasonic.h create mode 100644 upbot_SORT/include/sort_test/utils.h create mode 100644 upbot_SORT/src/.ranging.cc.swp create mode 100644 upbot_SORT/src/detection.cc create mode 100644 upbot_SORT/src/hungarian.cc create mode 100644 upbot_SORT/src/kalmanboxtracker.cc create mode 100644 upbot_SORT/src/main.cc create mode 100644 upbot_SORT/src/postprocess.cc create mode 100644 upbot_SORT/src/preprocess.cc create mode 100644 upbot_SORT/src/ranging.cc create mode 100644 upbot_SORT/src/sort.cc create mode 100644 upbot_SORT/src/ultrasonic.cc create mode 100644 upbot_SORT/src/utils.cc diff --git a/old_sort_detection/include/sort_test/hungarian.h b/old_sort_detection/include/sort_test/hungarian.h new file mode 100644 index 0000000..9c9c9b0 --- /dev/null +++ b/old_sort_detection/include/sort_test/hungarian.h @@ -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 +#include + + +class HungarianAlgorithm +{ +public: + HungarianAlgorithm(); + ~HungarianAlgorithm(); + float Solve(std::vector >& DistMatrix, std::vector& Assignment); + +private: + void assignmentoptimal(int* assignment, float* cost, float* distMatrix, int nOfRows, int nOfColumns); + void buildassignmentvector(int* assignment, bool* starMatrix, int nOfRows, int nOfColumns); + void computeassignmentcost(int* assignment, float* cost, float* distMatrix, int nOfRows); + void step2a(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); + void step2b(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); + void step3(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); + void step4(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col); + void step5(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); +}; + + +#endif \ No newline at end of file diff --git a/old_sort_detection/include/sort_test/kalmanboxtracker.h b/old_sort_detection/include/sort_test/kalmanboxtracker.h new file mode 100644 index 0000000..23f096d --- /dev/null +++ b/old_sort_detection/include/sort_test/kalmanboxtracker.h @@ -0,0 +1,74 @@ +#pragma once +#include +#include + + +/** + * @brief KalmanBoxTracker 卡尔曼跟踪器 + */ +class KalmanBoxTracker +{ +public: + /** + * @brief KalmanBoxTracker 卡尔曼跟踪器类构造函数 + * @param bbox 检测框 + */ + KalmanBoxTracker(std::vector bbox); + + /** + * @brief update 通过观测的检测框更新系统状态 + * @param bbox 检测框 + */ + void update(std::vector bbox); + + /** + * @brief predict 估计预测的边界框 + */ + std::vector predict(); + + /** + * @brief get_state 返回当前检测框状态 + */ + std::vector 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> m_history; + + /** + * @param m_hits + */ + int m_hits; + + /** + * @param m_hit_streak 忽略目标初始的若干帧 + */ + int m_hit_streak; + + /** + * @param m_age predict被调用的次数计数 + */ + int m_age; +}; \ No newline at end of file diff --git a/old_sort_detection/include/sort_test/sort.h b/old_sort_detection/include/sort_test/sort.h new file mode 100644 index 0000000..0acc6e9 --- /dev/null +++ b/old_sort_detection/include/sort_test/sort.h @@ -0,0 +1,61 @@ + +#pragma once +#include +#include + +#include "utils.h" +#include "kalmanboxtracker.h" + + +/** + * @brief Sort Sort锟姐法 + */ +class Sort +{ +public: + /** + * @brief Sort 锟斤拷锟届函锟斤拷 + * @param max_age 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + * @param min_hits 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + * @param iou_threshold iou锟斤拷值 + */ + Sort(int max_age, int min_hits, float iou_threshold); + + /** + * @brief update 锟斤拷锟铰硷拷锟斤拷 + * @param dets 锟斤拷锟斤拷 + */ + std::vector> update(std::vector dets); + + /** + * @brief detect_update 锟斤拷锟铰硷拷锟斤拷 + * @param detect_result_group 锟斤拷锟斤拷 + */ + _detect_result_group_t detect_update(_detect_result_group_t detect_result_group); + +private: + /** + * @param m_max_age 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + */ + int m_max_age; + + /** + * @param m_min_hits 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + */ + int m_min_hits; + + /** + * @param m_iou_threshold iou锟斤拷值 + */ + float m_iou_threshold; + + /** + * @param m_trackers 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟侥硷拷锟斤拷 + */ + std::vector m_trackers; + + /** + * @param m_frame_count 帧锟斤拷 + */ + int m_frame_count; +}; \ No newline at end of file diff --git a/old_sort_detection/include/sort_test/utils.h b/old_sort_detection/include/sort_test/utils.h new file mode 100644 index 0000000..08fb9f2 --- /dev/null +++ b/old_sort_detection/include/sort_test/utils.h @@ -0,0 +1,69 @@ +#pragma once +#include +#include +#include + + +/** + * @brief draw_label 画检测框 + * @param input_image 输入图像 + * @param label 标签名称 + * @param left 标签距图像左侧距离 + * @param top 标签距图像上侧距离 + */ +void draw_label(cv::Mat& input_image, std::string label, int left, int top); + + +/** + * @brief get_center 计算检测框中心 + * @param detections 输入检测框 + */ +std::vector get_center(std::vector detections); + + +/** + * @brief get_center 计算两点间距离 + * @param p1 点1 + * @param p2 点2 + */ +float get_distance(cv::Point p1, cv::Point p2); + + +/** + * @brief get_center 计算两检测框中心 + * @param p1 检测框1 + * @param p2 检测框2 + */ +float get_center_distance(std::vector bbox1, std::vector bbox2); + + +/** + * @brief convert_bbox_to_z 将检测框由[x1,y1,x2,y2]的形式转化为[x,y,s,r]的形式 + * @param bbox 检测框 + */ +std::vector convert_bbox_to_z(std::vector bbox); + + +/** + * @brief convert_x_to_bbox 将检测框由[x,y,s,r]的形式转化为[x1,y1,x2,y2]的形式 + * @param x 检测框 + */ +std::vector convert_x_to_bbox(std::vector x); + + +/** + * @brief iou 计算两检测框的iou + * @param box1 检测框1 + * @param box2 检测框2 + */ +float iou(std::vector box1, std::vector box2); + + +/** + * @brief associate_detections_to_tracks 将检测结果和跟踪结果关联 + * @param detections 检测结果 + * @param trackers 跟踪结果 + * @param iou_threshold iou矩阵 + */ +std::tuple>, std::vector, std::vector> +associate_detections_to_tracks(std::vector detections, std::vector> trackers, float iou_threshold = 0.3); \ No newline at end of file diff --git a/old_sort_detection/include/sort_test/yolo.h b/old_sort_detection/include/sort_test/yolo.h new file mode 100644 index 0000000..93d1ecd --- /dev/null +++ b/old_sort_detection/include/sort_test/yolo.h @@ -0,0 +1,54 @@ +#pragma once +#include +#include + +#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); + void drawPred(cv::Mat& img, std::vector result, std::vector 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 _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" }; +}; diff --git a/old_sort_detection/src/hungarian.cc b/old_sort_detection/src/hungarian.cc new file mode 100644 index 0000000..284470f --- /dev/null +++ b/old_sort_detection/src/hungarian.cc @@ -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 +#include // for DBL_MAX +#include // for fabs() +#include "Hungarian.h" + + +HungarianAlgorithm::HungarianAlgorithm() {} +HungarianAlgorithm::~HungarianAlgorithm() {} + + +//********************************************************// +// A single function wrapper for solving assignment problem. +//********************************************************// +float HungarianAlgorithm::Solve(std::vector >& DistMatrix, std::vector& Assignment) +{ + unsigned int nRows = DistMatrix.size(); + unsigned int nCols = DistMatrix[0].size(); + + float* distMatrixIn = new float[nRows * nCols]; + int* assignment = new int[nRows]; + float cost = 0.0; + + // Fill in the distMatrixIn. Mind the index is "i + nRows * j". + // Here the cost matrix of size MxN is defined as a float precision array of N*M elements. + // In the solving functions matrices are seen to be saved MATLAB-internally in row-order. + // (i.e. the matrix [1 2; 3 4] will be stored as a vector [1 3 2 4], NOT [1 2 3 4]). + for (unsigned int i = 0; i < nRows; i++) + for (unsigned int j = 0; j < nCols; j++) + distMatrixIn[i + nRows * j] = DistMatrix[i][j]; + + // call solving function + assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols); + + Assignment.clear(); + for (unsigned int r = 0; r < nRows; r++) + Assignment.push_back(assignment[r]); + + delete[] distMatrixIn; + delete[] assignment; + return cost; +} + + +//********************************************************// +// Solve optimal solution for assignment problem using Munkres algorithm, also known as Hungarian Algorithm. +//********************************************************// +void 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); +} \ No newline at end of file diff --git a/old_sort_detection/src/kalmanboxtracker.cc b/old_sort_detection/src/kalmanboxtracker.cc new file mode 100644 index 0000000..71a2eae --- /dev/null +++ b/old_sort_detection/src/kalmanboxtracker.cc @@ -0,0 +1,97 @@ +#include "utils.h" +#include "kalmanboxtracker.h" + + +int KalmanBoxTracker::count = 0; + + +KalmanBoxTracker::KalmanBoxTracker(std::vector bbox) +{ + + m_kf = new cv::KalmanFilter(7, 4); + + m_kf->transitionMatrix = cv::Mat::eye(7, 7, CV_32F); + m_kf->transitionMatrix.at(0, 4) = 1; + m_kf->transitionMatrix.at(1, 5) = 1; + m_kf->transitionMatrix.at(2, 6) = 1; + + m_kf->measurementMatrix = cv::Mat::eye(4, 7, CV_32F); + + m_kf->measurementNoiseCov = cv::Mat::eye(4, 4, CV_32F); + m_kf->measurementNoiseCov.at(2, 2) *= 10; + m_kf->measurementNoiseCov.at(3, 3) *= 10; + + m_kf->errorCovPost = cv::Mat::eye(7, 7, CV_32F); + m_kf->errorCovPost.at(4, 4) *= 1000; + m_kf->errorCovPost.at(5, 5) *= 1000; + m_kf->errorCovPost.at(6, 6) *= 1000; + m_kf->errorCovPost *= 10; + + m_kf->processNoiseCov = cv::Mat::eye(7, 7, CV_32F); + m_kf->processNoiseCov.at(4, 4) *= 0.01; + m_kf->processNoiseCov.at(5, 5) *= 0.01; + m_kf->processNoiseCov.at(6, 6) *= 0.001; + + m_kf->statePost = cv::Mat::eye(7, 1, CV_32F); + m_kf->statePost.at(0, 0) = convert_bbox_to_z(bbox)[0]; + m_kf->statePost.at(1, 0) = convert_bbox_to_z(bbox)[1]; + m_kf->statePost.at(2, 0) = convert_bbox_to_z(bbox)[2]; + m_kf->statePost.at(3, 0) = convert_bbox_to_z(bbox)[3]; + + m_time_since_update = 0; + + m_id = count; + count++; + + m_history = {}; + + m_hits = 0; + + m_hit_streak = 0; + + m_age = 0; + + //std::cout << m_kf->transitionMatrix << std::endl; + //std::cout << m_kf->measurementMatrix << std::endl; + //std::cout << m_kf->measurementNoiseCov << std::endl; + //std::cout << m_kf->errorCovPost << std::endl; + //std::cout << m_kf->processNoiseCov << std::endl; + //std::cout << m_kf->statePost << std::endl; +} + + +void KalmanBoxTracker::update(std::vector bbox) +{ + m_time_since_update = 0; + m_history = {}; + m_hits++; + m_hit_streak++; + cv::Mat measurement(4, 1, CV_32F); + for (size_t i = 0; i < 4; i++) + measurement.at(i) = convert_bbox_to_z(bbox)[i]; + //std::cout << measurement << std::endl; + m_kf->correct(measurement); +} + + +std::vector KalmanBoxTracker::predict() +{ + + //std::cout << m_kf->statePost << std::endl; + if (m_kf->statePost.at(2, 0) + m_kf->statePost.at(6, 0) <= 0) + m_kf->statePost.at(6, 0) = 0; + m_kf->predict(); + m_age++; + if (m_time_since_update > 0) + m_hit_streak = 0; + m_time_since_update++; + m_history.push_back(convert_x_to_bbox(m_kf->statePost)); + return m_history.back(); +} + + +std::vector KalmanBoxTracker::get_state() +{ + //std::cout << m_kf->statePost << std::endl; + return convert_x_to_bbox(m_kf->statePost); +} \ No newline at end of file diff --git a/old_sort_detection/src/main.cc b/old_sort_detection/src/main.cc new file mode 100644 index 0000000..021af11 --- /dev/null +++ b/old_sort_detection/src/main.cc @@ -0,0 +1,266 @@ +#include "yolo.h" +#include "kalmanboxtracker.h" +#include +#include +#include +#include +#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 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 detection_rects; + + while (cap.read(frame)) { + cap >> frame; + + if (frame.empty()) { + cout << "视频已结束" << endl; + break; + } + + vector 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> 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> 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 +#include +#include +#include +#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 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 result; + if (yolo.Detect(frame, net, result)) { + // 跟踪已检测到的目标 + vector 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> 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(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; +} +*/ \ No newline at end of file diff --git a/old_sort_detection/src/sort.cc b/old_sort_detection/src/sort.cc new file mode 100644 index 0000000..d210012 --- /dev/null +++ b/old_sort_detection/src/sort.cc @@ -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> Sort::update(std::vector dets) +{ + m_frame_count++; + std::vector> trks(m_trackers.size(), std::vector(5, 0)); + std::vector to_del; + std::vector> ret; + + for (size_t i = 0; i < trks.size(); i++) + { + std::vector pos = m_trackers[i].predict(); + std::vector trk{ (int)pos[0], (int)pos[1], (int)pos[2], (int)pos[3], 0 }; + trks[i] = trk; + } + + for (int i = to_del.size() - 1; i >= 0; i--) + m_trackers.erase(m_trackers.begin() + i); + + auto [matched, unmatched_dets, unmatched_trks] = associate_detections_to_tracks(dets, trks, m_iou_threshold); + + for (size_t i = 0; i < matched.size(); i++) + { + int id = matched[i].first; + std::vector vec{ dets[id].x, dets[id].y, dets[id].x + dets[id].width, dets[id].y + dets[id].height }; + m_trackers[matched[i].second].update(vec); + } + + for (auto i : unmatched_dets) + { + std::vector vec{ dets[i].x, dets[i].y, dets[i].x + dets[i].width, dets[i].y + dets[i].height }; + KalmanBoxTracker trk(vec); + m_trackers.push_back(trk); + } + int n = m_trackers.size(); + + for (int i = m_trackers.size() - 1; i >= 0; i--) + { + auto trk = m_trackers[i]; + //std::cout << KalmanBoxTracker::count << std::endl; + std::vector d = trk.get_state(); + if ((trk.m_time_since_update < 1) && (trk.m_hit_streak >= m_min_hits || m_frame_count <= m_min_hits)) + { + std::vector tmp = d; + tmp.push_back(trk.m_id + 1); + ret.push_back(tmp); + } + n--; + if (trk.m_time_since_update > m_max_age) + m_trackers.erase(m_trackers.begin() + n); + } + + if (ret.size() > 0) + return ret; + + return {}; +} + +_detect_result_group_t Sort::detect_update(_detect_result_group_t detect_result_group); +{ + std::vector dets; + for(int i=0; i> trks(m_trackers.size(), std::vector(5, 0)); + std::vector to_del; + std::vector> ret; + + for (size_t i = 0; i < trks.size(); i++) + { + std::vector pos = m_trackers[i].predict(); + std::vector trk{ (int)pos[0], (int)pos[1], (int)pos[2], (int)pos[3], 0 }; + trks[i] = trk; + } + + for (int i = to_del.size() - 1; i >= 0; i--) + m_trackers.erase(m_trackers.begin() + i); + + auto [matched, unmatched_dets, unmatched_trks] = associate_detections_to_tracks(dets, trks, m_iou_threshold); + + for (size_t i = 0; i < matched.size(); i++) + { + int id = matched[i].first; + std::vector vec{ dets[id].x, dets[id].y, dets[id].x + dets[id].width, dets[id].y + dets[id].height }; + m_trackers[matched[i].second].update(vec); + } + + for (auto i : unmatched_dets) + { + std::vector vec{ dets[i].x, dets[i].y, dets[i].x + dets[i].width, dets[i].y + dets[i].height }; + KalmanBoxTracker trk(vec); + m_trackers.push_back(trk); + } + int n = m_trackers.size(); + + for (int i = m_trackers.size() - 1; i >= 0; i--) + { + auto trk = m_trackers[i]; + //std::cout << KalmanBoxTracker::count << std::endl; + std::vector d = trk.get_state(); + if ((trk.m_time_since_update < 1) && (trk.m_hit_streak >= m_min_hits || m_frame_count <= m_min_hits)) + { + std::vector tmp = d; + tmp.push_back(trk.m_id + 1); + ret.push_back(tmp); + } + n--; + if (trk.m_time_since_update > m_max_age) + m_trackers.erase(m_trackers.begin() + n); + } + + for(int i=0; i 0) + return detect_result_group; + + return {}; +} \ No newline at end of file diff --git a/old_sort_detection/src/utils.cc b/old_sort_detection/src/utils.cc new file mode 100644 index 0000000..23782f9 --- /dev/null +++ b/old_sort_detection/src/utils.cc @@ -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 get_center(std::vector detections) +{ + std::vector detections_center(detections.size()); + for (size_t i = 0; i < detections.size(); i++) + { + detections_center[i] = cv::Point(detections[i].x + detections[i].width / 2, detections[i].y + detections[i].height / 2); + } + + return detections_center; +} + + +float get_distance(cv::Point p1, cv::Point p2) +{ + return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); +} + + +float get_center_distance(std::vector bbox1, std::vector bbox2) +{ + float x1 = bbox1[0], x2 = bbox2[0]; + float y1 = bbox1[1], y2 = bbox2[1]; + float w1 = bbox1[2] - bbox1[0], w2 = bbox2[2] - bbox2[0]; + float h1 = bbox1[3] - bbox1[1], h2 = bbox2[3] - bbox2[1]; + cv::Point p1(x1 + w1 / 2, y1 + h1 / 2), p2(x2 + w2 / 2, y2 + h2 / 2); + return get_distance(p1, p2); +} + + +std::vector convert_bbox_to_z(std::vector bbox) +{ + float w = bbox[2] - bbox[0]; + float h = bbox[3] - bbox[1]; + float x = bbox[0] + w / 2; + float y = bbox[1] + h / 2; + float s = w * h; + float r = w / h; + + return { x, y, s, r }; +} + + +std::vector convert_x_to_bbox(std::vector x) +{ + float w = sqrt(x[2] * x[3]); + float h = x[2] / w; + return { x[0] - w / 2, x[1] - h / 2, x[0] + w / 2, x[1] + h / 2 }; +} + + +float iou(std::vector box1, std::vector box2) +{ + int x1 = std::max(box1[0], box2[0]); + int y1 = std::max(box1[1], box2[1]); + int x2 = std::min(box1[2], box2[2]); + int y2 = std::min(box1[3], box2[3]); + int w = std::max(0, x2 - x1); + int h = std::max(0, y2 - y1); + int inter_area = w * h; + float iou = inter_area * 1.0 / ((box1[2] - box1[0]) * (box1[3] - box1[1]) + (box2[2] - box2[0]) * (box2[3] - box2[1]) - inter_area); + + return iou; +} + + +template +std::vector sort_indices(const std::vector& v) +{ + std::vector idx(v.size()); + std::iota(idx.begin(), idx.end(), 0); + std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) { return v[i1] < v[i2]; }); + return idx; +} + + +std::vector> linear_assignment(cv::Mat iou_matrix) +{ + std::vector> costMatrix(iou_matrix.cols, std::vector(iou_matrix.rows)); + for (size_t i = 0; i < iou_matrix.cols; i++) + for (size_t j = 0; j < iou_matrix.rows; j++) + costMatrix[i][j] = iou_matrix.at(j, i); + + HungarianAlgorithm HungAlgo; + std::vector assignment; + HungAlgo.Solve(costMatrix, assignment); + + std::vector> tmp(2); + for (size_t i = 0; i < assignment.size(); i++) + { + if (assignment[i] >= 0) + { + tmp[0].push_back(assignment[i]); + tmp[1].push_back(i); + } + } + + std::vector indices = sort_indices(tmp[0]); + std::sort(tmp[0].begin(), tmp[0].end()); + + std::vector> ret(2); + ret[0] = tmp[0]; + for (size_t i = 0; i < ret[0].size(); i++) + ret[1].push_back(tmp[1][indices[i]]); + + return ret; +} + + +std::tuple>, std::vector, std::vector> +associate_detections_to_tracks(std::vector detections, std::vector> trackers, float iou_threshold) +{ + if (trackers.size() == 0) + { + std::vector 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 detection{ detections[i].x, detections[i].y, detections[i].x + detections[i].width, detections[i].y + detections[i].height }; + std::vector tracker = trackers[j]; + iou_matrix.at(i, j) = iou(detection, tracker); + } + } + //std::cout << iou_matrix << std::endl; + + std::vector> matched_indices(2); + if (std::min(iou_matrix.rows, iou_matrix.cols) > 0) + { + cv::Mat a(iou_matrix.rows, iou_matrix.cols, CV_32F, cv::Scalar(0)); + for (size_t i = 0; i < a.rows; i++) + { + for (size_t j = 0; j < a.cols; j++) + { + if (iou_matrix.at(i, j) > iou_threshold) + a.at(i, j) = 1; + } + } + //std::cout << a << std::endl; + + cv::Mat a_sum0(iou_matrix.cols, 1, CV_32F, cv::Scalar(0)); + cv::reduce(a, a_sum0, 0, cv::REDUCE_SUM); + std::vector sum0(iou_matrix.cols); + for (size_t i = 0; i < sum0.size(); i++) + sum0[i] = a_sum0.at(0, i); + float a_sum0_max = *std::max_element(sum0.begin(), sum0.end()); + + cv::Mat a_sum1(1, iou_matrix.rows, CV_32F, cv::Scalar(0)); + cv::reduce(a, a_sum1, 1, cv::REDUCE_SUM); + std::vector sum1(iou_matrix.rows); + for (size_t i = 0; i < sum1.size(); i++) + sum1[i] = a_sum1.at(i, 0); + float a_sum1_max = *std::max_element(sum1.begin(), sum1.end()); + + if (int(a_sum0_max) == 1 && int(a_sum1_max) == 1) + { + std::vector nonZeroCoordinates; + cv::findNonZero(a, nonZeroCoordinates); + std::sort(nonZeroCoordinates.begin(), nonZeroCoordinates.end(), [](cv::Point p1, cv::Point p2) {return p1.y < p2.y; }); + for (int i = 0; i < nonZeroCoordinates.size(); i++) + { + matched_indices[0].push_back(nonZeroCoordinates[i].y); + matched_indices[1].push_back(nonZeroCoordinates[i].x); + } + } + else + { + matched_indices = linear_assignment(-iou_matrix); + } + } + + std::vector unmatched_detections; + for (size_t i = 0; i < detections.size(); i++) + { + if (std::find(matched_indices[0].begin(), matched_indices[0].end(), i) == matched_indices[0].end()) + unmatched_detections.push_back(i); + } + + std::vector unmatched_trackers; + for (size_t i = 0; i < trackers.size(); i++) + { + if (std::find(matched_indices[1].begin(), matched_indices[1].end(), i) == matched_indices[1].end()) + unmatched_trackers.push_back(i); + } + + std::vector> matches; + for (size_t i = 0; i < matched_indices[0].size(); i++) + { + //std::cout << matched_indices[0][i] << " " << matched_indices[1][i] << std::endl; + if (iou_matrix.at(matched_indices[0][i], matched_indices[1][i]) < iou_threshold) + { + unmatched_detections.push_back(matched_indices[0][i]); + unmatched_trackers.push_back(matched_indices[1][i]); + } + else + matches.push_back({ matched_indices[0][i], matched_indices[1][i] }); + } + + return { matches, unmatched_detections, unmatched_trackers }; +} \ No newline at end of file diff --git a/old_sort_detection/src/yolo.cc b/old_sort_detection/src/yolo.cc new file mode 100644 index 0000000..6b8e659 --- /dev/null +++ b/old_sort_detection/src/yolo.cc @@ -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) { + 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 netOutputImg; + vector outputLayerName{"345","403", "461","output" }; + net.forward(netOutputImg, outputLayerName[3]); //获取output的输出 + //net.forward(netOutputImg, net.getUnconnectedOutLayersNames()); + std::vector classIds;//结果id数组 + std::vector confidences;//结果每个id对应置信度数组 + std::vector 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 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 result, vector 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(); + +} diff --git a/upbot_SORT/README.md b/upbot_SORT/README.md new file mode 100644 index 0000000..e728eed --- /dev/null +++ b/upbot_SORT/README.md @@ -0,0 +1,47 @@ +## Yolov5s+SORT(鍗″皵鏇+鍖堢墮鍒) + +鍒╃敤鐩爣璺熻釜瀵规娴嬫杩涜棰勬祴鐨勬妧鏈紝瀵圭洰鏍囨娴嬩换鍔¤繘琛屽姞閫燂紝鍦▂olov5妫娴嬬殑甯т箣闂寸┛鎻掍娇鐢ㄥ崱灏旀浖婊ゆ尝杩涜鐩爣妫娴嬫鐨勯娴嬶紝鍑忓皯瀵规ā鍨嬬殑璋冪敤锛屽疄鐜扮洰鏍囨娴嬪抚鐜囨彁鍗囩殑鐩殑銆 + +#### 浠ｇ爜杩愯鐜锛 + +| 寮鍙戠幆澧 | rk3588 | +| -------- | -------------------- | +| OpenCV | opencv-3.4.3-windows | +| 璋冪敤妯″瀷 | YOLOv5s.rknn | + + + +#### 涓昏浠ｇ爜璁剧疆锛 + +鐜閰嶇疆锛 + +``` +杩涘叆鍒癛OS鐜涓荤洰褰曚腑 +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锛堝涓猵utText鐨勬搷浣滐紝娑堣楁椂闂村彲蹇界暐涓嶈锛 + +鍙岀洰鍖归厤鑺辫垂50~140ms锛堝嚭鐜版尝鍔級 + +甯х巼鏄剧ず澶х害鑺辫垂0.03ms锛堝崟涓猵utText鐨勬搷浣滐紝娑堣楁椂闂村彲蹇界暐涓嶈锛 + +imgshow澶х害鑺辫垂3.5ms锛堝崟甯у浘鍍廼mgshow鐨勬搷浣滐級 + + + + + + + diff --git a/upbot_SORT/include/sort_test/Timer.h b/upbot_SORT/include/sort_test/Timer.h new file mode 100644 index 0000000..245f662 --- /dev/null +++ b/upbot_SORT/include/sort_test/Timer.h @@ -0,0 +1,33 @@ +/** + * @file Timer.h + * @author hcy + * @brief 鐢ㄤ簬璁℃椂 + * @version 1 + * @date 2023-04-10 + * + */ + + +#include +#include + +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 start; + std::chrono::time_point end; + std::chrono::duration duration; +}; \ No newline at end of file diff --git a/upbot_SORT/include/sort_test/detection.h b/upbot_SORT/include/sort_test/detection.h new file mode 100644 index 0000000..26b502f --- /dev/null +++ b/upbot_SORT/include/sort_test/detection.h @@ -0,0 +1,48 @@ +#include +#include +#include +#include + +#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; // 榛樿鐨凬MS闃堝 + 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 out_scales; + std::vector 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); + + +}; diff --git a/upbot_SORT/include/sort_test/hungarian.h b/upbot_SORT/include/sort_test/hungarian.h new file mode 100644 index 0000000..9c9c9b0 --- /dev/null +++ b/upbot_SORT/include/sort_test/hungarian.h @@ -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 +#include + + +class HungarianAlgorithm +{ +public: + HungarianAlgorithm(); + ~HungarianAlgorithm(); + float Solve(std::vector >& DistMatrix, std::vector& Assignment); + +private: + void assignmentoptimal(int* assignment, float* cost, float* distMatrix, int nOfRows, int nOfColumns); + void buildassignmentvector(int* assignment, bool* starMatrix, int nOfRows, int nOfColumns); + void computeassignmentcost(int* assignment, float* cost, float* distMatrix, int nOfRows); + void step2a(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); + void step2b(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); + void step3(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); + void step4(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim, int row, int col); + void step5(int* assignment, float* distMatrix, bool* starMatrix, bool* newStarMatrix, bool* primeMatrix, bool* coveredColumns, bool* coveredRows, int nOfRows, int nOfColumns, int minDim); +}; + + +#endif \ No newline at end of file diff --git a/upbot_SORT/include/sort_test/kalmanboxtracker.h b/upbot_SORT/include/sort_test/kalmanboxtracker.h new file mode 100644 index 0000000..23f096d --- /dev/null +++ b/upbot_SORT/include/sort_test/kalmanboxtracker.h @@ -0,0 +1,74 @@ +#pragma once +#include +#include + + +/** + * @brief KalmanBoxTracker 卡尔曼跟踪器 + */ +class KalmanBoxTracker +{ +public: + /** + * @brief KalmanBoxTracker 卡尔曼跟踪器类构造函数 + * @param bbox 检测框 + */ + KalmanBoxTracker(std::vector bbox); + + /** + * @brief update 通过观测的检测框更新系统状态 + * @param bbox 检测框 + */ + void update(std::vector bbox); + + /** + * @brief predict 估计预测的边界框 + */ + std::vector predict(); + + /** + * @brief get_state 返回当前检测框状态 + */ + std::vector 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> m_history; + + /** + * @param m_hits + */ + int m_hits; + + /** + * @param m_hit_streak 忽略目标初始的若干帧 + */ + int m_hit_streak; + + /** + * @param m_age predict被调用的次数计数 + */ + int m_age; +}; \ No newline at end of file diff --git a/upbot_SORT/include/sort_test/postprocess.h b/upbot_SORT/include/sort_test/postprocess.h new file mode 100644 index 0000000..855ecea --- /dev/null +++ b/upbot_SORT/include/sort_test/postprocess.h @@ -0,0 +1,42 @@ +#ifndef _upbot_vision_POSTPROCESS_H_ +#define _upbot_vision_POSTPROCESS_H_ + +#include +#include + +#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 &qnt_zps, std::vector &qnt_scales, + detect_result_group_t *group); + +void deinitPostProcess(); +#endif //_upbot_vision_POSTPROCESS_H_ diff --git a/upbot_SORT/include/sort_test/preprocess.h b/upbot_SORT/include/sort_test/preprocess.h new file mode 100644 index 0000000..7d8d03f --- /dev/null +++ b/upbot_SORT/include/sort_test/preprocess.h @@ -0,0 +1,16 @@ +#ifndef _upbot_vision_PREPROCESS_H_ +#define _upbot_vision_PREPROCESS_H_ + +#include +#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_ diff --git a/upbot_SORT/include/sort_test/ranging.h b/upbot_SORT/include/sort_test/ranging.h new file mode 100644 index 0000000..9e6600f --- /dev/null +++ b/upbot_SORT/include/sort_test/ranging.h @@ -0,0 +1,102 @@ +#pragma once + +#include +#include + +#include +#include +#include "sort_test/detection.h" +#include + + +using namespace cv; + +class Ranging +{ +private: + // rknn_handle hdx; + cv::VideoCapture vcapture; + Detection yolov5s; +/* 2-stereo + Mat cam_matrix_left = (Mat_(3, 3) << + 4.791153893499601e+02, 0, 0, + 0,4.798785696902847e+02, 0, + 3.195422157433843e+02, 2.355081129251360e+02, 1); + Mat cam_matrix_right = (Mat_(3, 3) << + 4.851946576927952e+02, 0, 0, + 0,4.848412081674973e+02, 0, + 3.139237321746737e+02, 2.316610217516937e+02, 1); + Mat distortion_l = (Mat_(1, 5) <<0.091215339806518,-0.088601421082219, 0, + 0, 0); + + Mat distortion_r = (Mat_(1, 5) <<0.086266675232625,-0.070869167707634, 0, + 0, 0); + Mat rotate = (Mat_(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_(3, 1) << + -61.636981845981540, -1.107000282904877, -1.084419989523733); +*/ +/* + Mat cam_matrix_left = (Mat_(3, 3) << + 4.869084743604942e+02, 0, 0, + 0,4.859921620798602e+02, 0, + 2.813183643903652e+02, 2.267657091677993e+02, 1); + Mat cam_matrix_right = (Mat_(3, 3) << + 4.859133331805883e+02, 0, 0, + 0,4.850356748771951e+02, 0, + 2.970046483040089e+02, 2.324763397214774e+02, 1); + Mat distortion_l = (Mat_(1, 5) <<0.121235284781974,-0.161097849662596, 0, + 0, 0); + + Mat distortion_r = (Mat_(1, 5) <<0.105479235005840,-0.120347246815955, 0, + 0, 0); + Mat rotate = (Mat_(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_(3, 1) << + -60.319997608188590, -0.019664800882533, -0.638993708428792); +*/ +// 3_stereo: + Mat cam_matrix_left = (Mat_(3, 3) << + 725.8008655, 0, 606.83544339, + 0, 725.71074058, 355.60009442, + 0, 0, 1); + Mat cam_matrix_right = (Mat_(3, 3) << + 720.47514477, 0, 611.91679085, + 0, 720.93965216, 328.55158146, + 0, 0, 1); + Mat distortion_l = (Mat_(1, 5) << + 0.0794014, -0.07158048, -0.00233537, -0.00538731, -0.07631366); + + Mat distortion_r = (Mat_(1, 5) << + 0.10602397, -0.12121708, -0.0039814, -0.00316623, -0.06940564); + Mat rotate = (Mat_(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_(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 pic2cam(int u, int v); + std::vector muban(cv::Mat &left_image, cv::Mat &right_image, const int *coordinates); + std::vector get_range(int frame_num, int detect_interval); + void horizon_estimate(cv::Mat& img, cv::Mat& bboxs,int k); + +}; + diff --git a/upbot_SORT/include/sort_test/sort.h b/upbot_SORT/include/sort_test/sort.h new file mode 100644 index 0000000..076c079 --- /dev/null +++ b/upbot_SORT/include/sort_test/sort.h @@ -0,0 +1,62 @@ + +#pragma once +#include +#include + +#include "utils.h" +#include "kalmanboxtracker.h" +#include "sort_test/postprocess.h" + + +/** + * @brief Sort Sort锟姐法 + */ +class Sort +{ +public: + /** + * @brief Sort 锟斤拷锟届函锟斤拷 + * @param max_age 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + * @param min_hits 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + * @param iou_threshold iou锟斤拷值 + */ + Sort(int max_age, int min_hits, float iou_threshold); + + /** + * @brief update 锟斤拷锟铰硷拷锟斤拷 + * @param dets 锟斤拷锟斤拷 + */ + std::vector> update(std::vector dets); + + /** + * @brief detect_update 锟斤拷锟铰硷拷锟斤拷 + * @param detect_result_group 锟斤拷锟斤拷 + */ + _detect_result_group_t detect_update(_detect_result_group_t detect_result_group); + +private: + /** + * @param m_max_age 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + */ + int m_max_age; + + /** + * @param m_min_hits 未锟斤拷锟斤拷时锟斤拷锟斤拷锟斤拷 + */ + int m_min_hits; + + /** + * @param m_iou_threshold iou锟斤拷值 + */ + float m_iou_threshold; + + /** + * @param m_trackers 锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟斤拷锟侥硷拷锟斤拷 + */ + std::vector m_trackers; + + /** + * @param m_frame_count 帧锟斤拷 + */ + int m_frame_count; +}; diff --git a/upbot_SORT/include/sort_test/ultrasonic.h b/upbot_SORT/include/sort_test/ultrasonic.h new file mode 100644 index 0000000..6720cce --- /dev/null +++ b/upbot_SORT/include/sort_test/ultrasonic.h @@ -0,0 +1,37 @@ +#include +#include +#include +#include +#include +#include "pibot_msgs/avoid.h" +#include + + + +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("ultrasonic", 1)) { + // std::string port_name; + // nh.param("port", port_name, "/dev/ttyUSB0"); + // serial.open(port_name); + // serial.set_option(boost::asio::serial_port_base::baud_rate(9600)); + // } + UltrasonicNode(); + ~UltrasonicNode(); + + + void readUltrasonic(); +}; + \ No newline at end of file diff --git a/upbot_SORT/include/sort_test/utils.h b/upbot_SORT/include/sort_test/utils.h new file mode 100644 index 0000000..08fb9f2 --- /dev/null +++ b/upbot_SORT/include/sort_test/utils.h @@ -0,0 +1,69 @@ +#pragma once +#include +#include +#include + + +/** + * @brief draw_label 画检测框 + * @param input_image 输入图像 + * @param label 标签名称 + * @param left 标签距图像左侧距离 + * @param top 标签距图像上侧距离 + */ +void draw_label(cv::Mat& input_image, std::string label, int left, int top); + + +/** + * @brief get_center 计算检测框中心 + * @param detections 输入检测框 + */ +std::vector get_center(std::vector detections); + + +/** + * @brief get_center 计算两点间距离 + * @param p1 点1 + * @param p2 点2 + */ +float get_distance(cv::Point p1, cv::Point p2); + + +/** + * @brief get_center 计算两检测框中心 + * @param p1 检测框1 + * @param p2 检测框2 + */ +float get_center_distance(std::vector bbox1, std::vector bbox2); + + +/** + * @brief convert_bbox_to_z 将检测框由[x1,y1,x2,y2]的形式转化为[x,y,s,r]的形式 + * @param bbox 检测框 + */ +std::vector convert_bbox_to_z(std::vector bbox); + + +/** + * @brief convert_x_to_bbox 将检测框由[x,y,s,r]的形式转化为[x1,y1,x2,y2]的形式 + * @param x 检测框 + */ +std::vector convert_x_to_bbox(std::vector x); + + +/** + * @brief iou 计算两检测框的iou + * @param box1 检测框1 + * @param box2 检测框2 + */ +float iou(std::vector box1, std::vector box2); + + +/** + * @brief associate_detections_to_tracks 将检测结果和跟踪结果关联 + * @param detections 检测结果 + * @param trackers 跟踪结果 + * @param iou_threshold iou矩阵 + */ +std::tuple>, std::vector, std::vector> +associate_detections_to_tracks(std::vector detections, std::vector> trackers, float iou_threshold = 0.3); \ No newline at end of file diff --git a/upbot_SORT/src/.ranging.cc.swp b/upbot_SORT/src/.ranging.cc.swp new file mode 100644 index 0000000000000000000000000000000000000000..7d1c6c677ea42e66b9b2be2d01c0094494c53862 GIT binary patch literal 24576 zcmeHOdvH|OdB1jSw@Pf+aWZl1nKajOOz%o7?E{3d5VAoQpcD`l0vi~*UhUr1UVC@% zvU?Y4Q5^FyuNn}>=20F73TE|I(Xgt0t&Yph|%%1*#ONQlLtKDg~+( zs8XOxf&V`g(2{owFQ7ZO8(mV2-`k3Q?=-%AiyR17H{!0*(XQfqozY zECt#C5AYyxJ8&By06)G5>49TF82CNl5#XD^-`y<;9|Grq-N4s@(`f8|U@x!-s0UoY zdtXF;;Jq3_I0ftj_5u$8|MQ!I@K3-_APuB|>AVQ^d=>a993&qCXMjzB2CM*P0UyCRvKd$g)B&Hu+3_Z@1Ly++ zz>~mizzw*72Z1jE{{qKM9ykD`fmy&e02;5qHk~JY+(qL_YvJbRHF8K(Q!SC08q`|F zq^cyeK#w>}Oe!INC>WReePX?+xJCaouWP0#dOYLT&yHOk&L6y*J2b2x-jzGLC4Xr| zsHq99X>N`viSQydu|iI%k|xKKZcz#MyJiYzjUJE4Dh;U_O>AiqYbjB)Sc`XhqaYS9 zC&ICrD05*@6IsM8Q4aUYjUhEADQJerqYv)QpFOMSTlXQE$5WgzrS^5Tf8PNo#UyrLxLMR$8*sGQG(=(<+mVft3$Y zt2pI69-?huP|<#0&PqOmo;*~Er=x10RO_pC6NRV;ZNWq$EA^&=*^YRyS9UYfxkaDf zi+?llZz>}um|L`yiEW(=I=cdM=5_h!bShMmw15l{Msw$q_A}P){3+ zMqO+Wsm}#NPXR?^kC5q8A2XSrg_#%vxCu#(|`uhK`+E`j#wU;;}(|(=5uZ!ZVScIXkAZ_ z+MjL=YR2T1o^y*$MmKoIHZ>6`pKcnB6XRQd`2^q*BZ04c0vZigLVtx4*leOIAVJEu zm3W0FipMiHdU*W&`rOMe>Kh036PGO>)Q2vOAKNYr2y0X&EYeKHF|X$4sG3sNs)>M{ z)-djxEP)@U&{(;Ng{E-2xT)&rToA&nl9a4vQV9{_Af|?Rz5Kl~nMp?jJ;Bg(Br6uE zp~fsRCN->50!TETz&Wa@uGK1@W%qc@DNCfF0zDpcx}w>kXhSWyoUySLpMo?-H)FEI zBai@7s3+wXC9tW#sfn{QjSepmagABfml)G*KO~$MMQV$w!IVhWUzn_)lJ!9v_)OmS z;L$Jc$)6b3w+~Hh+L0f5CHLAPR#D|zXd;k6=F0*GFn{@|y(~~8D_6~?UB$etVm47g z1Zq1Q9-@XZeoA5?3zAZDIup~};uKI}Sr7!9g0#kqlE9eq5WKLa>Noc3TQ3-W+%Uvj z>!=N-uspm%W30n!rY9y7oms)W0+W$2nB7>vV#JKrz>)AXTh_fW(AL)3K6h?lQD@h} z_9uBe7?pE-hjN$pe0=SIetq}Y)ph#zy;xiIO_#8$=Js6BcU+n{_>O*fi@xrfm0)am zH%}w5R)gt{igGOpfjZPxjiv@d$ zTzAN1zP9BLo`bfMJGPlsWQnGbs;0t90%Mn6>0O0YoaB`=(1$=K<(z?jN=c&688{Uy z_JB|#7A(mX&-4TnB(B6MHpQ9L5~n!2>qeo8H1rP)WSygY7pR*%eJpo)loPl}xW!D# zh8Krelh$A%gen(+&`Bx%0ag($8=R$EC9G?HmrJY@S35(o1W!eUH_PdB8M3IT(#%aN zC2{uoDn(AOS*erMh6-y@!Q+uK?lrE4R;k~eb=69Upz)<2-zrya(Lfowt)VjYUmAl9 z^jQ3?A^JNd9AutT54MLR9!yGDq&nigU2f5OLFjVQA@Bp@k{ZylsJp!@u%Kg6`|K`* zj49d9yui$i7lb~|Ge2EEbMY!kONkQUIv{5`-Vl_<7|>`I^=V?uj@C)h|7G#O-_SLFMIT2dp$12>LOks{h zlgZlh(IGV~hfrww1R*sg7sa)NW06pzv=z`$kkr`?n;v%6E#Tvr(o>ia(=}QUQd@BH zJ;l|b#;BtURF0w+WNJ}V)F=lF#>6B<6e6}aqflR*O*EtVFOMbr|1H?0S74`-{crBS z55cbA1pEQ;bzlOv`bl69uoCz(@NcluzYn|)tOsI15AYE1Hf(i2@D1Q+u+RS$xCpET zvOp&=8@L5G9mzzpEG zf!l!_u*-V@veTynzYFN-=Z}C9UyRSHxo@PC2=1@^B~UxF6UIsmIh z%RbL70-jRDV_3^2sZ)^@lmfRHmbE!*zYN1dNkmj5J7pxL#qjcm(zz|B@N4Tkh8+p2#gmlBkE-ff9Ld{N&G>+qtMaFu%QP$^7=^fyE1EcPxT$ z%PlSm1!KV!naY%~2@WaI*J%1gsQh#i%7_+`Sedo>-z+{7h8d1P$p>Xu((n&{rEVqk z+}xDBIs=6vE4lD+5>XkUXfQ>MT4_1xD3n}LM#eTZgW_CJfHgapOQ?PjAbg`g5{irStv8HAA37R!G7fL6J#f^@+OU#tHgY3RTn@uI=@=k04;!d( zh{g#eM+?WxOoWD!ZqH0PldT;-HN|cETFeaeYh+?ZVcATk$EQptW?q)fRIcA}gTa$Vw0<9t}QBGc7rj?+LvQ?cK`w;N{BWzS53iPJbOcE}7 z2s^$fLcOD8MM20+1}Al^atZL>n2vtt8x!^K?$EmV&ZMnHQ7>i+rvAMT!Z2)^zz^Vli_wc&X{pcm%@I+XAK zH0nR13}$Xw$ZKKZbq(2=Gndi@*fz`VWA;z*gW{;BlZ9_z&3k!@yGDt3V#M{d>SD z@Dk7qc!8h6wm$=W3mAt@p9lUC_;X+bupC$f%m*F@?f||3+y)51d$9HA19t#Ad>?-S zTm+5*yMbMR0z`pcpbeM<)B>MiPyVOCPk;}BL4fw}JAn>hKJXRbe&8(j>9+z~fW?3W z`~-9!1&H2zh~~u)@!6VCFo}IFxiKwk5-ERevlj;zcXcif%vm~jF4W_W741-g4dGHb z_1v!63)`Vjcg&mLjRhp$8@Q3Q}5)pWC^# ztE~;|0eaK9xP1{efERviar-#&Lb~rHPHF^{tz&@Z7$m*q`BvA%AikH|d+-7VA8` zDa=H|{NcwLI?*0-n9Hf4n9hXcbh?qwHaHJ8unySLoLgG<%!cI9PrR7lwYjKu`iYC< zm*24JXQdD+c>QuJ#g0?7r&4OFS!`1?v9Op>HIWEpG^nH{l%1sdL)IWDY3hI+#&RSM z@c!8?HB&)=5vMIv!m`@N*-xUgA;K+{H(4o6^JB$;3Z(}EAfFs30S0(|U{#6ntNGbS zpM5Sj`(7o^l5XAK<0mEnKi1%EOw)eEF5iz!t;S!}_V0 zXe02*n-lxr7DccoICpG#=lHo}`E&cgk@-DW^}+r6?(Jiv=W^dYj*W=ij@P*l|D$yq zZq^XN+T}1_xcUjYk@g5xjRb($jAhJf3hiyMwo};{9(srk${G7Snqn=JA2vSIYxLhDUD4Ih-P*a_v6S7dm2E$<%r>A`5I}vd9apanlpgh<*B-`Xb8K zP{s=XEH#+YkIJS8J-DgaO|*dx?vGgZcLjP`MHL(2+=pV)9EVd%NX_LG-}(?kw6jEF zBo<7k7vX&+ooA^Reb_x^tv3Fl@i#T$k>=yk>-c-`8vn)r4@Rt4{_?@=1?Af8XVz9; ziKf_F=1524id;!5mW?7JQ2~YA7K)qPhEsT8H5_DfRp2;(BQ^!ga9lAzP$dqYpc-l) z35#y#jKTP0Al!6ns7%{D9+Q(;t`JU{VS$O$K^F0$+AIp|l;jo;V^ zLqcFVjDJQZ&n(gBE#yGvH<0kZUW`I@_mm`HP6GVnF$JYMYoPZe1&8beYUU1gKjXp+* z^y-u*-C{qzw^pZjk~*Vw{K6Q#I+0Z_W#Pzc5VM#m%;L1aJY?L!i{vk)n8#zZzf4*E zaEHNs#i&qOj;{)3@sc7`5^D_Qs6&1_a!T#0WB=>L2;Pck7|+Z2t)xdj)*}#}Y-qR- zsegoI3GYwJS48ejzGJq*f z@DRZ}BqSl~BTn{p?s2f1Tg~h!$UFqnkI4y=7q+uZPNQ+ooDlh7#v&gr@1ij{IH$#? zdmyRw%dtRzcCEuzLzcxDWUt{0EDGPv8^q0OTVWhEE^_JOSJT zT!zix4ETY&fGx1=9|!&$w)}qq{|Q_GP69iCXMhIaZ$M9?=}W*?qHV>;$+ZRVH&cdf zm4noeA};>5s~n$0-S|wZMp-q(o&}^8V81A7&Mvqw=_-RsK(vl~k@XX2#z$XfPJqp% zCNO^^#==awF45X2!}i4K$naDo!T2|_ilgz-YNu~fb}NM258@M0W717-2%G6QuU zPV#rLbDWcxgIta>V6gCvA)NRm8BtP}B^tTA99~tQMGdkNtyZE|d`tYQ zNQp$U@{~w9?0d8aLs8RsTe(W&M4qxH7|URn8B)qT1esLR3VhG-BM*oy7Im@}iG5(l z;Ca4lKnFc<>@)TSD%On5;F?4N5|Re5-u{9n-|?Rx6FYxD*o#&2=h-@l4FAN3xFR&JT{^>@YihjXN8;iyLsN z7AK1_s8(ZBGJM?6VgrY%pWW)zC;7yH3XfxuUa+lx?!eg4w((Om zQMr!<{mgA!mpinLj!7Th0=DG`i+LJ~-8(dPW0dLQ|J6Qw#R0W;wChg94MOoRfIM@$A8$p5&B98gVV6%}u zUSf8$cH~F)>s#L`JaSHjp-lUd2RhVc+W4$wGV|$$S^D`9}WWGledODKikt7wc4-S*c`iluzo^nOLcnlbVijJ|gTUVTNfa z3Q9Nb5?Yn8O@8}MA<0TZm*WzG1%O2YeKOsJKncV!=JLYRSeNZMFBww=TvdG8j*;A% z8VA9R)OahK+Nht2YAbOUU2!P9kY%1Np!8Evrg_AiH;ILseKroCQ#W1ad3i0Wq#Ihv z;lIM}8@oqYv%4aeGN4;Kq z9~*8aa~mhjucV4$+Eg*`q86xrNq$e3tA6}r{$%Q3jd7KaS|a&aBekfk1zp&sLWB7` z8Th5jBLe3KLeStO+ylgn#6>_?0|nKQ63TOOR-mcH=MxKMorb)zcCzeybti_P^U=C@ z?Gz456ir5|Z5HGQiEJw`&RNjC(6{y@J!RanXYBfpcwDH#K%hm)U{Zm+xD+0O#xz}HrDJ=(Rk%}!rvLx| literal 0 HcmV?d00001 diff --git a/upbot_SORT/src/detection.cc b/upbot_SORT/src/detection.cc new file mode 100644 index 0000000..b24dd8a --- /dev/null +++ b/upbot_SORT/src/detection.cc @@ -0,0 +1,201 @@ +#include "sort_test/detection.h" +#include + + +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; +} diff --git a/upbot_SORT/src/hungarian.cc b/upbot_SORT/src/hungarian.cc new file mode 100644 index 0000000..7a0373d --- /dev/null +++ b/upbot_SORT/src/hungarian.cc @@ -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 +#include // for DBL_MAX +#include // for fabs() +#include "sort_test/hungarian.h" + + +HungarianAlgorithm::HungarianAlgorithm() {} +HungarianAlgorithm::~HungarianAlgorithm() {} + + +//********************************************************// +// A single function wrapper for solving assignment problem. +//********************************************************// +float HungarianAlgorithm::Solve(std::vector >& DistMatrix, std::vector& Assignment) +{ + unsigned int nRows = DistMatrix.size(); + unsigned int nCols = DistMatrix[0].size(); + + float* distMatrixIn = new float[nRows * nCols]; + int* assignment = new int[nRows]; + float cost = 0.0; + + // Fill in the distMatrixIn. Mind the index is "i + nRows * j". + // Here the cost matrix of size MxN is defined as a float precision array of N*M elements. + // In the solving functions matrices are seen to be saved MATLAB-internally in row-order. + // (i.e. the matrix [1 2; 3 4] will be stored as a vector [1 3 2 4], NOT [1 2 3 4]). + for (unsigned int i = 0; i < nRows; i++) + for (unsigned int j = 0; j < nCols; j++) + distMatrixIn[i + nRows * j] = DistMatrix[i][j]; + + // call solving function + assignmentoptimal(assignment, &cost, distMatrixIn, nRows, nCols); + + Assignment.clear(); + for (unsigned int r = 0; r < nRows; r++) + Assignment.push_back(assignment[r]); + + delete[] distMatrixIn; + delete[] assignment; + return cost; +} + + +//********************************************************// +// Solve optimal solution for assignment problem using Munkres algorithm, also known as Hungarian Algorithm. +//********************************************************// +void 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); +} diff --git a/upbot_SORT/src/kalmanboxtracker.cc b/upbot_SORT/src/kalmanboxtracker.cc new file mode 100644 index 0000000..75ca508 --- /dev/null +++ b/upbot_SORT/src/kalmanboxtracker.cc @@ -0,0 +1,97 @@ +#include "sort_test/utils.h" +#include "sort_test/kalmanboxtracker.h" + + +int KalmanBoxTracker::count = 0; + + +KalmanBoxTracker::KalmanBoxTracker(std::vector bbox) +{ + + m_kf = new cv::KalmanFilter(7, 4); + + m_kf->transitionMatrix = cv::Mat::eye(7, 7, CV_32F); + m_kf->transitionMatrix.at(0, 4) = 1; + m_kf->transitionMatrix.at(1, 5) = 1; + m_kf->transitionMatrix.at(2, 6) = 1; + + m_kf->measurementMatrix = cv::Mat::eye(4, 7, CV_32F); + + m_kf->measurementNoiseCov = cv::Mat::eye(4, 4, CV_32F); + m_kf->measurementNoiseCov.at(2, 2) *= 10; + m_kf->measurementNoiseCov.at(3, 3) *= 10; + + m_kf->errorCovPost = cv::Mat::eye(7, 7, CV_32F); + m_kf->errorCovPost.at(4, 4) *= 1000; + m_kf->errorCovPost.at(5, 5) *= 1000; + m_kf->errorCovPost.at(6, 6) *= 1000; + m_kf->errorCovPost *= 10; + + m_kf->processNoiseCov = cv::Mat::eye(7, 7, CV_32F); + m_kf->processNoiseCov.at(4, 4) *= 0.01; + m_kf->processNoiseCov.at(5, 5) *= 0.01; + m_kf->processNoiseCov.at(6, 6) *= 0.001; + + m_kf->statePost = cv::Mat::eye(7, 1, CV_32F); + m_kf->statePost.at(0, 0) = convert_bbox_to_z(bbox)[0]; + m_kf->statePost.at(1, 0) = convert_bbox_to_z(bbox)[1]; + m_kf->statePost.at(2, 0) = convert_bbox_to_z(bbox)[2]; + m_kf->statePost.at(3, 0) = convert_bbox_to_z(bbox)[3]; + + m_time_since_update = 0; + + m_id = count; + count++; + + m_history = {}; + + m_hits = 0; + + m_hit_streak = 0; + + m_age = 0; + + //std::cout << m_kf->transitionMatrix << std::endl; + //std::cout << m_kf->measurementMatrix << std::endl; + //std::cout << m_kf->measurementNoiseCov << std::endl; + //std::cout << m_kf->errorCovPost << std::endl; + //std::cout << m_kf->processNoiseCov << std::endl; + //std::cout << m_kf->statePost << std::endl; +} + + +void KalmanBoxTracker::update(std::vector bbox) +{ + m_time_since_update = 0; + m_history = {}; + m_hits++; + m_hit_streak++; + cv::Mat measurement(4, 1, CV_32F); + for (size_t i = 0; i < 4; i++) + measurement.at(i) = convert_bbox_to_z(bbox)[i]; + //std::cout << measurement << std::endl; + m_kf->correct(measurement); +} + + +std::vector KalmanBoxTracker::predict() +{ + + //std::cout << m_kf->statePost << std::endl; + if (m_kf->statePost.at(2, 0) + m_kf->statePost.at(6, 0) <= 0) + m_kf->statePost.at(6, 0) = 0; + m_kf->predict(); + m_age++; + if (m_time_since_update > 0) + m_hit_streak = 0; + m_time_since_update++; + m_history.push_back(convert_x_to_bbox(m_kf->statePost)); + return m_history.back(); +} + + +std::vector KalmanBoxTracker::get_state() +{ + //std::cout << m_kf->statePost << std::endl; + return convert_x_to_bbox(m_kf->statePost); +} diff --git a/upbot_SORT/src/main.cc b/upbot_SORT/src/main.cc new file mode 100644 index 0000000..5aadb20 --- /dev/null +++ b/upbot_SORT/src/main.cc @@ -0,0 +1,152 @@ +#include +#include +#include +#include +#include "sort_test/ranging.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "opencv2/imgcodecs/imgcodecs.hpp" +#include +#include +#include +#include +#include +#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 running(true); +std::queue> frameInfo; +std::mutex g_mutex; +int detect_interval = 1; + +void *ranging(void *args) // ranging绾跨▼ +{ + int64 t = getTickCount(); + ros::Publisher dis_pub_; + ros::NodeHandle nh; + // <>鍐呬负鑷畾涔塺os娑堟伅绫诲瀷 瀛樺偍闅滅鐗╀俊鎭寘鎷笌闅滅鐗╃殑璺濈銆侀殰纰嶇墿瀹藉害銆侀珮搴﹀拰涓庨殰纰嶇墿涔嬮棿鐨勫す瑙 + dis_pub_ = nh.advertise("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 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(i,0)>0&&info.at(i,0)<200) + { + pibot_msgs::dis_info data; + data.distance = info.at(i,0); + data.width = info.at(i,1); + data.height = info.at(i,2); + data.angle = info.at(i,3); + dis_array.dis.push_back(data); + // std::cout< +#include +#include +#include +#include +#include +#include +#include +#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 &outputLocations, std::vector classIds, std::vector &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 &input, int left, int right, std::vector &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 &boxes, std::vector &objProbs, std::vector &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 &qnt_zps, + std::vector &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 filterBoxes; + std::vector objProbs; + std::vector 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 indexArray; + for (int i = 0; i < validCount; ++i) + { + indexArray.push_back(i); + } + + quick_sort_indice_inverse(objProbs, 0, validCount - 1, indexArray); + + std::set 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; + } + } +} diff --git a/upbot_SORT/src/preprocess.cc b/upbot_SORT/src/preprocess.cc new file mode 100644 index 0000000..29e4fe6 --- /dev/null +++ b/upbot_SORT/src/preprocess.cc @@ -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; +} diff --git a/upbot_SORT/src/ranging.cc b/upbot_SORT/src/ranging.cc new file mode 100644 index 0000000..f80f166 --- /dev/null +++ b/upbot_SORT/src/ranging.cc @@ -0,0 +1,490 @@ +#include +#include +#include +#include +#include "sort_test/ranging.h" +#include "sort_test/kalmanboxtracker.h" +#include "sort_test/sort.h" +#include "sort_test/postprocess.h" +#include +#include +#include +#include +#include +#include +#include "opencv2/core.hpp" +#include "opencv2/imgcodecs.hpp" +#include "opencv2/imgproc.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// #include + + +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 Ranging::pic2cam(int u, int v) //鍍忕礌鍧愭爣杞浉鏈哄潗鏍 +{ + //(u,v)->(x,y)"(loc[0],loc[1])" + std::vector loc; + loc.push_back((u - cam_matrix_right.at(0, 2)) * q.at(2, 3) / cam_matrix_right.at(0, 0)); + loc.push_back((v - cam_matrix_right.at(1, 2)) * q.at(2, 3) / cam_matrix_right.at(1, 1)); + return loc; +} + +std::vector 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 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(k, 0); + int x2 = bboxs.at(k, 2); + int y1 = bboxs.at(k, 1); + int y2 = bboxs.at(k, 3); + float Conf = bboxs.at(k, 4); + int cls = bboxs.at(k, 5); + float Y_B, Y_H; + cv::Mat edge, grayImage; + std::vector 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: " < point_b = pic2cam(x1 + i, 240); //杞负鐩告満鍧愭爣绯 + std::vector point_H = pic2cam(320, 240); + float alfa = atan((point_b[0] - point_H[0]) / q.at(2, 3)); + if (idx.size() < 1) + { + Z.at(0, i) = 0; + Z.at(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(2, 3) * H_c / (Y_B - Y_H)- H_c*tan(theta); + alfa = atan((point_b[0] - point_H[0]) / q.at(2, 3)); + //cout << "d: " << d << endl; + if (d > 700) + {d = 0;} + Z.at(0, i) = d/cos(alfa); + Z.at(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(i, 0); + int y1 = detBoxes.at(i, 1); + int x2 = detBoxes.at(i, 2); + int y2 = detBoxes.at(i, 3); + + float Conf = detBoxes.at(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 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 channels; + split(threed_pixel_xyz.clone(), channels); + threed_pixel_xyz = channels[0] + channels[1] + channels[2]; //璁＄畻娆у紡璺濈 + threed_pixel_xyz.forEach([](float &value, const int *position) { value = sqrt(value); }); // 鑾峰緱璺濈d + + int mid_pixel = int((x1 + x2) / 2); + std::vector mid = pic2cam(imgGrayR.cols / 2, imgGrayR.rows); //璁＄畻瑙掑害锛屼粠鍍忕礌鍧愭爣杞负鐩告満鍧愭爣 + std::vector loc_tar = pic2cam(mid_pixel, imgGrayR.rows); + float alfa = atan((loc_tar[0] - mid[0]) / q.at(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_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + continue; + } + else + { + float median = threed_pixel_xyz.at((int)(y1 + y2) / 2, (int)(x1 + x2) / 2); + + std::vector ltPoint = pic2cam(x1, y1); + std::vector rbPoint = pic2cam(x2, y2); + float xx1 = ltPoint[0], yy1 = ltPoint[1], xx2 = rbPoint[0], yy2 = rbPoint[1]; //璁＄畻瀹介珮 + float f = q.at(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(y2, x1)[0] - threedImage.at(y2, x2)[0]), 2) + + pow((threedImage.at(y2, x1)[1] - threedImage.at(y2, x2)[1]), 2) + + pow((threedImage.at(y2, x1)[2] - threedImage.at(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_(1, 4) << -1, -1, -1, -1); + infoRow.copyTo(info.row(i)); + continue; + } + // 锟斤拷图锟斤拷锟较伙拷锟斤拷锟斤拷息 + // 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_(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鏄噰鐢╒4L2椹卞姩鎵撳紑鎽勫儚澶达紝瑙ｅ喅閮ㄥ垎鎽勫儚澶存棤娉曟墦寮鐨勬儏鍐 */ + /*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 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{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"<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(count-1, 0) = xmin; + detBoxes.at(count-1, 1) = ymin; + detBoxes.at(count-1, 2) = xmax; + detBoxes.at(count-1, 3) = ymax; + detBoxes.at(count-1, 4) = det_result->prop; + // detBoxes.at(i, 0) = xmin; + // detBoxes.at(i, 1) = ymin; + // detBoxes.at(i, 2) = xmax; + // detBoxes.at(i, 3) = ymax; + // detBoxes.at(i, 4) = det_result->prop; + } + // 瀹為獙娴嬭瘯锛岃繃婊よ繃澶х殑璇妗 + // float ratio = (xmax - xmin) * (ymax - ymin) / 308480.; + // if (ratio > 0.7) + // { + // detBoxes.at(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(i, 0); + // float w1 = info.at(i, 1); + // float h1 = info.at(i, 2); + // float alfa = info.at(i, 3); + + int x1 = finalBoxes.at(i,0); + int y1 = finalBoxes.at(i,1); + int x2 = finalBoxes.at(i,2); + int y2 = finalBoxes.at(i,3); + + // int x1 = detBoxes.at(i,0); + // int y1 = detBoxes.at(i,1); + // int x2 = detBoxes.at(i,2); + // int y2 = detBoxes.at(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{rframe, detBoxes, info}; + } + return std::vector{empty}; +} + + diff --git a/upbot_SORT/src/sort.cc b/upbot_SORT/src/sort.cc new file mode 100644 index 0000000..43e0612 --- /dev/null +++ b/upbot_SORT/src/sort.cc @@ -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> Sort::update(std::vector dets) +{ + m_frame_count++; + std::vector> trks(m_trackers.size(), std::vector(5, 0)); + std::vector to_del; + std::vector> ret; + + for (size_t i = 0; i < trks.size(); i++) + { + std::vector pos = m_trackers[i].predict(); + std::vector trk{ (int)pos[0], (int)pos[1], (int)pos[2], (int)pos[3], 0 }; + trks[i] = trk; + } + + for (int i = to_del.size() - 1; i >= 0; i--) + m_trackers.erase(m_trackers.begin() + i); + + auto [matched, unmatched_dets, unmatched_trks] = associate_detections_to_tracks(dets, trks, m_iou_threshold); + + for (size_t i = 0; i < matched.size(); i++) + { + int id = matched[i].first; + std::vector vec{ dets[id].x, dets[id].y, dets[id].x + dets[id].width, dets[id].y + dets[id].height }; + m_trackers[matched[i].second].update(vec); + } + + for (auto i : unmatched_dets) + { + std::vector vec{ dets[i].x, dets[i].y, dets[i].x + dets[i].width, dets[i].y + dets[i].height }; + KalmanBoxTracker trk(vec); + m_trackers.push_back(trk); + } + int n = m_trackers.size(); + + for (int i = m_trackers.size() - 1; i >= 0; i--) + { + auto trk = m_trackers[i]; + //std::cout << KalmanBoxTracker::count << std::endl; + std::vector d = trk.get_state(); + if ((trk.m_time_since_update < 1) && (trk.m_hit_streak >= m_min_hits || m_frame_count <= m_min_hits)) + { + std::vector tmp = d; + tmp.push_back(trk.m_id + 1); + ret.push_back(tmp); + } + n--; + if (trk.m_time_since_update > m_max_age) + m_trackers.erase(m_trackers.begin() + n); + } + + if (ret.size() > 0) + return ret; + + return {}; +} + +_detect_result_group_t Sort::detect_update(_detect_result_group_t detect_result_group) +{ + std::vector dets; + for(int i=0; i> trks(m_trackers.size(), std::vector(5, 0)); + std::vector to_del; + std::vector> ret; + + for (size_t i = 0; i < trks.size(); i++) + { + std::vector pos = m_trackers[i].predict(); + std::vector trk{ (int)pos[0], (int)pos[1], (int)pos[2], (int)pos[3], 0 }; + trks[i] = trk; + } + + for (int i = to_del.size() - 1; i >= 0; i--) + m_trackers.erase(m_trackers.begin() + i); + + auto [matched, unmatched_dets, unmatched_trks] = associate_detections_to_tracks(dets, trks, m_iou_threshold); + + for (size_t i = 0; i < matched.size(); i++) + { + int id = matched[i].first; + std::vector vec{ dets[id].x, dets[id].y, dets[id].x + dets[id].width, dets[id].y + dets[id].height }; + m_trackers[matched[i].second].update(vec); + } + + for (auto i : unmatched_dets) + { + std::vector vec{ dets[i].x, dets[i].y, dets[i].x + dets[i].width, dets[i].y + dets[i].height }; + KalmanBoxTracker trk(vec); + m_trackers.push_back(trk); + } + int n = m_trackers.size(); + + for (int i = m_trackers.size() - 1; i >= 0; i--) + { + auto trk = m_trackers[i]; + //std::cout << KalmanBoxTracker::count << std::endl; + std::vector d = trk.get_state(); + if ((trk.m_time_since_update < 1) && (trk.m_hit_streak >= m_min_hits || m_frame_count <= m_min_hits)) + { + std::vector tmp = d; + tmp.push_back(trk.m_id + 1); + ret.push_back(tmp); + } + n--; + if (trk.m_time_since_update > m_max_age) + m_trackers.erase(m_trackers.begin() + n); + } + + for(int i=0; i 0) + return detect_result_group; + + return {}; +} diff --git a/upbot_SORT/src/ultrasonic.cc b/upbot_SORT/src/ultrasonic.cc new file mode 100644 index 0000000..8de9f9d --- /dev/null +++ b/upbot_SORT/src/ultrasonic.cc @@ -0,0 +1,112 @@ +#include "sort_test/ultrasonic.h" + +UltrasonicNode::UltrasonicNode(){ + r = libusb_init(&ctx); // 鍒濆鍖杔ibusb + 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); // 浣跨敤鎸囧畾鐨剉endor鍜宲roduct 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("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< get_center(std::vector detections) +{ + std::vector detections_center(detections.size()); + for (size_t i = 0; i < detections.size(); i++) + { + detections_center[i] = cv::Point(detections[i].x + detections[i].width / 2, detections[i].y + detections[i].height / 2); + } + + return detections_center; +} + + +float get_distance(cv::Point p1, cv::Point p2) +{ + return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); +} + + +float get_center_distance(std::vector bbox1, std::vector bbox2) +{ + float x1 = bbox1[0], x2 = bbox2[0]; + float y1 = bbox1[1], y2 = bbox2[1]; + float w1 = bbox1[2] - bbox1[0], w2 = bbox2[2] - bbox2[0]; + float h1 = bbox1[3] - bbox1[1], h2 = bbox2[3] - bbox2[1]; + cv::Point p1(x1 + w1 / 2, y1 + h1 / 2), p2(x2 + w2 / 2, y2 + h2 / 2); + return get_distance(p1, p2); +} + + +std::vector convert_bbox_to_z(std::vector bbox) +{ + float w = bbox[2] - bbox[0]; + float h = bbox[3] - bbox[1]; + float x = bbox[0] + w / 2; + float y = bbox[1] + h / 2; + float s = w * h; + float r = w / h; + + return { x, y, s, r }; +} + + +std::vector convert_x_to_bbox(std::vector x) +{ + float w = sqrt(x[2] * x[3]); + float h = x[2] / w; + return { x[0] - w / 2, x[1] - h / 2, x[0] + w / 2, x[1] + h / 2 }; +} + + +float iou(std::vector box1, std::vector box2) +{ + int x1 = std::max(box1[0], box2[0]); + int y1 = std::max(box1[1], box2[1]); + int x2 = std::min(box1[2], box2[2]); + int y2 = std::min(box1[3], box2[3]); + int w = std::max(0, x2 - x1); + int h = std::max(0, y2 - y1); + int inter_area = w * h; + float iou = inter_area * 1.0 / ((box1[2] - box1[0]) * (box1[3] - box1[1]) + (box2[2] - box2[0]) * (box2[3] - box2[1]) - inter_area); + + return iou; +} + + +template +std::vector sort_indices(const std::vector& v) +{ + std::vector idx(v.size()); + std::iota(idx.begin(), idx.end(), 0); + std::sort(idx.begin(), idx.end(), [&v](size_t i1, size_t i2) { return v[i1] < v[i2]; }); + return idx; +} + + +std::vector> linear_assignment(cv::Mat iou_matrix) +{ + std::vector> costMatrix(iou_matrix.cols, std::vector(iou_matrix.rows)); + for (size_t i = 0; i < iou_matrix.cols; i++) + for (size_t j = 0; j < iou_matrix.rows; j++) + costMatrix[i][j] = iou_matrix.at(j, i); + + HungarianAlgorithm HungAlgo; + std::vector assignment; + HungAlgo.Solve(costMatrix, assignment); + + std::vector> tmp(2); + for (size_t i = 0; i < assignment.size(); i++) + { + if (assignment[i] >= 0) + { + tmp[0].push_back(assignment[i]); + tmp[1].push_back(i); + } + } + + std::vector indices = sort_indices(tmp[0]); + std::sort(tmp[0].begin(), tmp[0].end()); + + std::vector> ret(2); + ret[0] = tmp[0]; + for (size_t i = 0; i < ret[0].size(); i++) + ret[1].push_back(tmp[1][indices[i]]); + + return ret; +} + + +std::tuple>, std::vector, std::vector> +associate_detections_to_tracks(std::vector detections, std::vector> trackers, float iou_threshold) +{ + if (trackers.size() == 0) + { + std::vector 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 detection{ detections[i].x, detections[i].y, detections[i].x + detections[i].width, detections[i].y + detections[i].height }; + std::vector tracker = trackers[j]; + iou_matrix.at(i, j) = iou(detection, tracker); + } + } + //std::cout << iou_matrix << std::endl; + + std::vector> matched_indices(2); + if (std::min(iou_matrix.rows, iou_matrix.cols) > 0) + { + cv::Mat a(iou_matrix.rows, iou_matrix.cols, CV_32F, cv::Scalar(0)); + for (size_t i = 0; i < a.rows; i++) + { + for (size_t j = 0; j < a.cols; j++) + { + if (iou_matrix.at(i, j) > iou_threshold) + a.at(i, j) = 1; + } + } + //std::cout << a << std::endl; + + cv::Mat a_sum0(iou_matrix.cols, 1, CV_32F, cv::Scalar(0)); + cv::reduce(a, a_sum0, 0, cv::REDUCE_SUM); + std::vector sum0(iou_matrix.cols); + for (size_t i = 0; i < sum0.size(); i++) + sum0[i] = a_sum0.at(0, i); + float a_sum0_max = *std::max_element(sum0.begin(), sum0.end()); + + cv::Mat a_sum1(1, iou_matrix.rows, CV_32F, cv::Scalar(0)); + cv::reduce(a, a_sum1, 1, cv::REDUCE_SUM); + std::vector sum1(iou_matrix.rows); + for (size_t i = 0; i < sum1.size(); i++) + sum1[i] = a_sum1.at(i, 0); + float a_sum1_max = *std::max_element(sum1.begin(), sum1.end()); + + if (int(a_sum0_max) == 1 && int(a_sum1_max) == 1) + { + std::vector nonZeroCoordinates; + cv::findNonZero(a, nonZeroCoordinates); + std::sort(nonZeroCoordinates.begin(), nonZeroCoordinates.end(), [](cv::Point p1, cv::Point p2) {return p1.y < p2.y; }); + for (int i = 0; i < nonZeroCoordinates.size(); i++) + { + matched_indices[0].push_back(nonZeroCoordinates[i].y); + matched_indices[1].push_back(nonZeroCoordinates[i].x); + } + } + else + { + matched_indices = linear_assignment(-iou_matrix); + } + } + + std::vector unmatched_detections; + for (size_t i = 0; i < detections.size(); i++) + { + if (std::find(matched_indices[0].begin(), matched_indices[0].end(), i) == matched_indices[0].end()) + unmatched_detections.push_back(i); + } + + std::vector unmatched_trackers; + for (size_t i = 0; i < trackers.size(); i++) + { + if (std::find(matched_indices[1].begin(), matched_indices[1].end(), i) == matched_indices[1].end()) + unmatched_trackers.push_back(i); + } + + std::vector> matches; + for (size_t i = 0; i < matched_indices[0].size(); i++) + { + //std::cout << matched_indices[0][i] << " " << matched_indices[1][i] << std::endl; + if (iou_matrix.at(matched_indices[0][i], matched_indices[1][i]) < iou_threshold) + { + unmatched_detections.push_back(matched_indices[0][i]); + unmatched_trackers.push_back(matched_indices[1][i]); + } + else + matches.push_back({ matched_indices[0][i], matched_indices[1][i] }); + } + + return { matches, unmatched_detections, unmatched_trackers }; +}