From ab584ffebc7f4979b04492a3948be33543460bd6 Mon Sep 17 00:00:00 2001
From: Scheaven <xuepengqiang>
Date: 星期四, 12 八月 2021 19:05:02 +0800
Subject: [PATCH] add T
---
config.json | 7
src/config.h | 15
src/tracker_tools/linear_assignment.cpp | 161 ++
src/tracker_tools/nn_matching.h | 35
src/core/detecter_manager.cpp | 126 +
src/utils/draw_util.h | 4
src/Munkres_assign/munkres/munkres.h | 463 ++++++
CMakeLists.txt | 4
src/Munkres_assign/hungarianoper.h | 12
src/tracker_tools/tracker.cpp | 343 ++++
src/Munkres_assign/munkres/munkres.cpp | 25
src/utils/geometry_util.h | 129 +
src/utils/queue_util.hpp | 168 ++
src/tracker_tools/tracker.h | 77 +
src/tracker_tools/model.cpp | 23
src/tracker_tools/track.h | 121 +
src/tracker_tools/dataType.h | 45
src/utils/geometry_util.cpp | 445 ++++++
src/utils/timer_utils.hpp | 38
src/tracker_tools/kalmfilter.h | 32
logs/trt_warn.log | 0
src/tracker_tools/track.cpp | 312 ++++
src/utils/draw_util.cpp | 12
src/tracker_tools/linear_assignment.h | 47
src/tracker_tools/nn_matching.cpp | 159 ++
logs/trt_debug.log | 0
src/utils/stack_util.hpp | 127 +
src/tracker_tools/feature_util.h | 14
src/Munkres_assign/munkres/matrix.h | 248 +++
src/core/ari_manager.h | 18
src/additional/fall_run_wander.h | 16
logs/trt_error.log | 0
src/core/detecter_manager.h | 46
src/additional/fall_run_wander.cpp | 193 ++
src/Munkres_assign/hungarianoper.cpp | 33
src/core/ari_manager.cpp | 365 ++++
src/h_interface.cpp | 61
src/tracker_tools/model.h | 46
src/tracker_tools/kalmfilter.cpp | 155 ++
src/h_interface.h | 3
src/detecter_tools/model.h | 2
src/tracker_tools/feature_util.cpp | 28
42 files changed, 4,121 insertions(+), 37 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e4f58ef..2dbe013 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -51,11 +51,11 @@
cuda_add_library(${PROJECT_NAME} SHARED ${sources})
-target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS} src/ src/core src/utils/ src/detecter_tools/)
+target_include_directories(${PROJECT_NAME} PRIVATE ${OpenCV_INCLUDE_DIRS} src/ src/core src/utils/ src/detecter_tools/ src/tracker_tools/ src/Munkers_assign/ src/detecter_tools/ src/additional/)
target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${LIBS})
add_executable(demo demo.cpp)
-target_include_directories(demo PRIVATE src/ src/core src/utils/ src/detecter_tools/)
+target_include_directories(demo PRIVATE src/ src/core src/utils/ src/detecter_tools/ src/tracker_tools/ src/Munkers_assign/ src/additional/)
target_link_libraries(demo ${PROJECT_NAME})
# add_executable(demo demo.cpp ${sources})
diff --git a/config.json b/config.json
index ad37796..3b3be65 100644
--- a/config.json
+++ b/config.json
@@ -4,6 +4,11 @@
"param": {
// "model_path": "/opt/vasystem/bin/models/baseDetector/baseDetetor.bin" // para閲岃竟鑷繁绠楁硶鍙兘鐢ㄥ埌鐨勫弬鏁�
"model_path": "/opt/vasystem/bin/models/baseDetector/baseDetetor_small.bin", // para閲岃竟鑷繁绠楁硶鍙兘鐢ㄥ埌鐨勫弬鏁�
- "type":2
+ "type":2,
+ "max_cam_num": 8,
+ "wander_time": 5,
+ "mv_velocity": 10.0,
+ "fall_rate":1.3,
+ "isTrack":false
}
}
diff --git a/logs/trt_debug.log b/logs/trt_debug.log
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/logs/trt_debug.log
diff --git a/logs/trt_error.log b/logs/trt_error.log
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/logs/trt_error.log
diff --git a/logs/trt_warn.log b/logs/trt_warn.log
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/logs/trt_warn.log
diff --git a/src/Munkres_assign/hungarianoper.cpp b/src/Munkres_assign/hungarianoper.cpp
new file mode 100644
index 0000000..8ee3000
--- /dev/null
+++ b/src/Munkres_assign/hungarianoper.cpp
@@ -0,0 +1,33 @@
+#include "hungarianoper.h"
+
+Eigen::Matrix<float, -1, 2, Eigen::RowMajor> HungarianOper::Solve(const DYNAMICM &cost_matrix)
+{
+ int rows = cost_matrix.rows();
+ int cols = cost_matrix.cols();
+ Matrix<double> matrix(rows, cols);
+ for (int row = 0; row < rows; row++) {
+ for (int col = 0; col < cols; col++) {
+ matrix(row, col) = cost_matrix(row, col);
+ }
+ }
+ //Munkres get matrix;
+ Munkres<double> m;
+ m.solve(matrix);
+
+ //
+ std::vector<std::pair<int, int>> pairs;
+ for (int row = 0; row < rows; row++) {
+ for (int col = 0; col < cols; col++) {
+ int tmp = (int)matrix(row, col);
+ if (tmp == 0) pairs.push_back(std::make_pair(row, col));
+ }
+ }
+ //
+ int count = pairs.size();
+ Eigen::Matrix<float, -1, 2, Eigen::RowMajor> re(count, 2);
+ for (int i = 0; i < count; i++) {
+ re(i, 0) = pairs[i].first;
+ re(i, 1) = pairs[i].second;
+ }
+ return re;
+}//end Solve;
diff --git a/src/Munkres_assign/hungarianoper.h b/src/Munkres_assign/hungarianoper.h
new file mode 100644
index 0000000..581a15f
--- /dev/null
+++ b/src/Munkres_assign/hungarianoper.h
@@ -0,0 +1,12 @@
+#ifndef HUNGARIANOPER_H
+#define HUNGARIANOPER_H
+#include "munkres/munkres.h"
+//#include "munkres/adapters/boostmatrixadapter.h"
+#include "../tracker_tools/dataType.h"
+
+class HungarianOper {
+public:
+ static Eigen::Matrix<float, -1, 2, Eigen::RowMajor> Solve(const DYNAMICM &cost_matrix);
+};
+
+#endif // HUNGARIANOPER_H
diff --git a/src/Munkres_assign/munkres/matrix.h b/src/Munkres_assign/munkres/matrix.h
new file mode 100644
index 0000000..84074cd
--- /dev/null
+++ b/src/Munkres_assign/munkres/matrix.h
@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2007 John Weaver
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef _MATRIX_H_
+#define _MATRIX_H_
+
+#include <initializer_list>
+#include <cstdlib>
+#include <ostream>
+#include <cassert>
+#include <cstdlib>
+#include <algorithm>
+
+#define XYZMIN(x, y) (x)<(y)?(x):(y)
+#define XYZMAX(x, y) (x)>(y)?(x):(y)
+
+template <class T>
+class Matrix {
+public:
+ Matrix(){
+ m_rows = 0;
+ m_columns = 0;
+ m_matrix = nullptr;
+ }
+ Matrix(const size_t rows, const size_t columns) {
+ m_matrix = nullptr;
+ resize(rows, columns);
+ }
+ Matrix(const std::initializer_list<std::initializer_list<T>> init) {
+ m_matrix = nullptr;
+ m_rows = init.size();
+ if ( m_rows == 0 ) {
+ m_columns = 0;
+ } else {
+ m_columns = init.begin()->size();
+ if ( m_columns > 0 ) {
+ resize(m_rows, m_columns);
+ }
+ }
+
+ size_t i = 0, j;
+ for ( auto row = init.begin() ; row != init.end() ; ++row, ++i ) {
+ assert ( row->size() == m_columns && "All rows must have the same number of columns." );
+ j = 0;
+ for ( auto value = row->begin() ; value != row->end() ; ++value, ++j ) {
+ m_matrix[i][j] = *value;
+ }
+ }
+ }
+ Matrix(const Matrix<T> &other) {
+ if ( other.m_matrix != nullptr ) {
+ // copy arrays
+ m_matrix = nullptr;
+ resize(other.m_rows, other.m_columns);
+ for ( size_t i = 0 ; i < m_rows ; i++ ) {
+ for ( size_t j = 0 ; j < m_columns ; j++ ) {
+ m_matrix[i][j] = other.m_matrix[i][j];
+ }
+ }
+ } else {
+ m_matrix = nullptr;
+ m_rows = 0;
+ m_columns = 0;
+ }
+ }
+ Matrix<T> & operator= (const Matrix<T> &other){
+ if ( other.m_matrix != nullptr ) {
+ // copy arrays
+ resize(other.m_rows, other.m_columns);
+ for ( size_t i = 0 ; i < m_rows ; i++ ) {
+ for ( size_t j = 0 ; j < m_columns ; j++ ) {
+ m_matrix[i][j] = other.m_matrix[i][j];
+ }
+ }
+ } else {
+ // free arrays
+ for ( size_t i = 0 ; i < m_columns ; i++ ) {
+ delete [] m_matrix[i];
+ }
+
+ delete [] m_matrix;
+
+ m_matrix = nullptr;
+ m_rows = 0;
+ m_columns = 0;
+ }
+
+ return *this;
+ }
+ ~Matrix(){
+ if ( m_matrix != nullptr ) {
+ // free arrays
+ for ( size_t i = 0 ; i < m_rows ; i++ ) {
+ delete [] m_matrix[i];
+ }
+
+ delete [] m_matrix;
+ }
+ m_matrix = nullptr;
+ }
+ // all operations modify the matrix in-place.
+ void resize(const size_t rows, const size_t columns, const T default_value = 0) {
+ assert ( rows > 0 && columns > 0 && "Columns and rows must exist." );
+
+ if ( m_matrix == nullptr ) {
+ // alloc arrays
+ m_matrix = new T*[rows]; // rows
+ for ( size_t i = 0 ; i < rows ; i++ ) {
+ m_matrix[i] = new T[columns]; // columns
+ }
+
+ m_rows = rows;
+ m_columns = columns;
+ clear();
+ } else {
+ // save array pointer
+ T **new_matrix;
+ // alloc new arrays
+ new_matrix = new T*[rows]; // rows
+ for ( size_t i = 0 ; i < rows ; i++ ) {
+ new_matrix[i] = new T[columns]; // columns
+ for ( size_t j = 0 ; j < columns ; j++ ) {
+ new_matrix[i][j] = default_value;
+ }
+ }
+
+ // copy data from saved pointer to new arrays
+ size_t minrows = XYZMIN(rows, m_rows);
+ size_t mincols = XYZMIN(columns, m_columns);
+ for ( size_t x = 0 ; x < minrows ; x++ ) {
+ for ( size_t y = 0 ; y < mincols ; y++ ) {
+ new_matrix[x][y] = m_matrix[x][y];
+ }
+ }
+
+ // delete old arrays
+ if ( m_matrix != nullptr ) {
+ for ( size_t i = 0 ; i < m_rows ; i++ ) {
+ delete [] m_matrix[i];
+ }
+
+ delete [] m_matrix;
+ }
+
+ m_matrix = new_matrix;
+ }
+
+ m_rows = rows;
+ m_columns = columns;
+ }
+ void clear() {
+ assert( m_matrix != nullptr );
+
+ for ( size_t i = 0 ; i < m_rows ; i++ ) {
+ for ( size_t j = 0 ; j < m_columns ; j++ ) {
+ m_matrix[i][j] = 0;
+ }
+ }
+ }
+ T& operator () (const size_t x, const size_t y) {
+ assert ( x < m_rows );
+ assert ( y < m_columns );
+ assert ( m_matrix != nullptr );
+ return m_matrix[x][y];
+ }
+
+ const T& operator () (const size_t x, const size_t y) const {
+ assert ( x < m_rows );
+ assert ( y < m_columns );
+ assert ( m_matrix != nullptr );
+ return m_matrix[x][y];
+ }
+ const T mmin() const {
+ assert( m_matrix != nullptr );
+ assert ( m_rows > 0 );
+ assert ( m_columns > 0 );
+ T min = m_matrix[0][0];
+
+ for ( size_t i = 0 ; i < m_rows ; i++ ) {
+ for ( size_t j = 0 ; j < m_columns ; j++ ) {
+ min = std::min<T>(min, m_matrix[i][j]);
+ }
+ }
+
+ return min;
+ }
+
+ const T mmax() const {
+ assert( m_matrix != nullptr );
+ assert ( m_rows > 0 );
+ assert ( m_columns > 0 );
+ T max = m_matrix[0][0];
+
+ for ( size_t i = 0 ; i < m_rows ; i++ ) {
+ for ( size_t j = 0 ; j < m_columns ; j++ ) {
+ max = std::max<T>(max, m_matrix[i][j]);
+ }
+ }
+
+ return max;
+ }
+ inline size_t minsize() { return ((m_rows < m_columns) ? m_rows : m_columns); }
+ inline size_t columns() const { return m_columns;}
+ inline size_t rows() const { return m_rows;}
+
+ friend std::ostream& operator<<(std::ostream& os, const Matrix &matrix)
+ {
+ os << "Matrix:" << std::endl;
+ for (size_t row = 0 ; row < matrix.rows() ; row++ )
+ {
+ for (size_t col = 0 ; col < matrix.columns() ; col++ )
+ {
+ os.width(8);
+ os << matrix(row, col) << ",";
+ }
+ os << std::endl;
+ }
+ return os;
+ }
+
+private:
+ T **m_matrix;
+ size_t m_rows;
+ size_t m_columns;
+};
+
+//#ifndef USE_EXPORT_KEYWORD
+//#include "matrix.cpp"
+////#define export /*export*/
+//#endif
+
+#endif /* !defined(_MATRIX_H_) */
+
diff --git a/src/Munkres_assign/munkres/munkres.cpp b/src/Munkres_assign/munkres/munkres.cpp
new file mode 100644
index 0000000..165f7ec
--- /dev/null
+++ b/src/Munkres_assign/munkres/munkres.cpp
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2007 John Weaver
+ * Copyright (c) 2015 Miroslav Krajicek
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "munkres.h"
+
+template class Munkres<double>;
+template class Munkres<float>;
+template class Munkres<int>;
+
diff --git a/src/Munkres_assign/munkres/munkres.h b/src/Munkres_assign/munkres/munkres.h
new file mode 100644
index 0000000..7acab24
--- /dev/null
+++ b/src/Munkres_assign/munkres/munkres.h
@@ -0,0 +1,463 @@
+/*
+ * Copyright (c) 2007 John Weaver
+ * *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#if !defined(_MUNKRES_H_)
+#define _MUNKRES_H_
+
+#include "matrix.h"
+
+#include <list>
+#include <utility>
+#include <iostream>
+#include <cmath>
+#include <limits>
+
+
+template<typename Data> class Munkres
+{
+ static constexpr int NORMAL = 0;
+ static constexpr int STAR = 1;
+ static constexpr int PRIME = 2;
+public:
+
+ /*
+ *
+ * Linear assignment problem solution
+ * [modifies matrix in-place.]
+ * matrix(row,col): row major format assumed.
+ *
+ * Assignments are remaining 0 values
+ * (extra 0 values are replaced with -1)
+ *
+ */
+ void solve(Matrix<Data> &m) {
+ const size_t rows = m.rows(),
+ columns = m.columns(),
+ size = XYZMAX(rows, columns);
+
+#ifdef DEBUG
+ std::cout << "Munkres input: " << m << std::endl;
+#endif
+
+ // Copy input matrix
+ this->matrix = m;
+
+ if ( rows != columns ) {
+ // If the input matrix isn't square, make it square
+ // and fill the empty values with the largest value present
+ // in the matrix.
+ matrix.resize(size, size, matrix.mmax());
+ }
+
+
+ // STAR == 1 == starred, PRIME == 2 == primed
+ mask_matrix.resize(size, size);
+
+ row_mask = new bool[size];
+ col_mask = new bool[size];
+ for ( size_t i = 0 ; i < size ; i++ ) {
+ row_mask[i] = false;
+ }
+
+ for ( size_t i = 0 ; i < size ; i++ ) {
+ col_mask[i] = false;
+ }
+
+ // Prepare the matrix values...
+
+ // If there were any infinities, replace them with a value greater
+ // than the maximum value in the matrix.
+ replace_infinites(matrix);
+
+ minimize_along_direction(matrix, rows >= columns);
+ minimize_along_direction(matrix, rows < columns);
+
+ // Follow the steps
+ int step = 1;
+ while ( step ) {
+ switch ( step ) {
+ case 1:
+ step = step1();
+ // step is always 2
+ break;
+ case 2:
+ step = step2();
+ // step is always either 0 or 3
+ break;
+ case 3:
+ step = step3();
+ // step in [3, 4, 5]
+ break;
+ case 4:
+ step = step4();
+ // step is always 2
+ break;
+ case 5:
+ step = step5();
+ // step is always 3
+ break;
+ }
+ }
+
+ // Store results
+ for ( size_t row = 0 ; row < size ; row++ ) {
+ for ( size_t col = 0 ; col < size ; col++ ) {
+ if ( mask_matrix(row, col) == STAR ) {
+ matrix(row, col) = 0;
+ } else {
+ matrix(row, col) = -1;
+ }
+ }
+ }
+
+#ifdef DEBUG
+ std::cout << "Munkres output: " << matrix << std::endl;
+#endif
+ // Remove the excess rows or columns that we added to fit the
+ // input to a square matrix.
+ matrix.resize(rows, columns);
+
+ m = matrix;
+
+ delete [] row_mask;
+ delete [] col_mask;
+ }
+
+ static void replace_infinites(Matrix<Data> &matrix) {
+ const size_t rows = matrix.rows(),
+ columns = matrix.columns();
+ //assert( rows > 0 && columns > 0 );
+ double max = matrix(0, 0);
+ constexpr auto infinity = std::numeric_limits<double>::infinity();
+
+ // Find the greatest value in the matrix that isn't infinity.
+ for ( size_t row = 0 ; row < rows ; row++ ) {
+ for ( size_t col = 0 ; col < columns ; col++ ) {
+ if ( matrix(row, col) != infinity ) {
+ if ( max == infinity ) {
+ max = matrix(row, col);
+ } else {
+ max = XYZMAX(max, matrix(row, col));
+ }
+ }
+ }
+ }
+
+ // a value higher than the maximum value present in the matrix.
+ if ( max == infinity ) {
+ // This case only occurs when all values are infinite.
+ max = 0;
+ } else {
+ max++;
+ }
+
+ for ( size_t row = 0 ; row < rows ; row++ ) {
+ for ( size_t col = 0 ; col < columns ; col++ ) {
+ if ( matrix(row, col) == infinity ) {
+ matrix(row, col) = max;
+ }
+ }
+ }
+
+ }
+ static void minimize_along_direction(Matrix<Data> &matrix, const bool over_columns) {
+ const size_t outer_size = over_columns ? matrix.columns() : matrix.rows(),
+ inner_size = over_columns ? matrix.rows() : matrix.columns();
+
+ // Look for a minimum value to subtract from all values along
+ // the "outer" direction.
+ for ( size_t i = 0 ; i < outer_size ; i++ ) {
+ double min = over_columns ? matrix(0, i) : matrix(i, 0);
+
+ // As long as the current minimum is greater than zero,
+ // keep looking for the minimum.
+ // Start at one because we already have the 0th value in min.
+ for ( size_t j = 1 ; j < inner_size && min > 0 ; j++ ) {
+ min = XYZMIN(
+ min,
+ over_columns ? matrix(j, i) : matrix(i, j));
+ }
+
+ if ( min > 0 ) {
+ for ( size_t j = 0 ; j < inner_size ; j++ ) {
+ if ( over_columns ) {
+ matrix(j, i) -= min;
+ } else {
+ matrix(i, j) -= min;
+ }
+ }
+ }
+ }
+ }
+
+private:
+
+ inline bool find_uncovered_in_matrix(const double item, size_t &row, size_t &col) const {
+ const size_t rows = matrix.rows(),
+ columns = matrix.columns();
+
+ for ( row = 0 ; row < rows ; row++ ) {
+ if ( !row_mask[row] ) {
+ for ( col = 0 ; col < columns ; col++ ) {
+ if ( !col_mask[col] ) {
+ if ( matrix(row,col) == item ) {
+ return true;
+ }
+ }
+ }
+ }
+ }
+
+ return false;
+ }
+
+ bool pair_in_list(const std::pair<size_t,size_t> &needle, const std::list<std::pair<size_t,size_t> > &haystack) {
+ for ( std::list<std::pair<size_t,size_t> >::const_iterator i = haystack.begin() ; i != haystack.end() ; i++ ) {
+ if ( needle == *i ) {
+ return true;
+ }
+ }
+
+ return false;
+ }
+
+ int step1() {
+ const size_t rows = matrix.rows(),
+ columns = matrix.columns();
+
+ for ( size_t row = 0 ; row < rows ; row++ ) {
+ for ( size_t col = 0 ; col < columns ; col++ ) {
+ if ( 0 == matrix(row, col) ) {
+ for ( size_t nrow = 0 ; nrow < row ; nrow++ )
+ if ( STAR == mask_matrix(nrow,col) )
+ goto next_column;
+
+ mask_matrix(row,col) = STAR;
+ goto next_row;
+ }
+ next_column:;
+ }
+ next_row:;
+ }
+
+ return 2;
+ }
+
+ int step2() {
+ const size_t rows = matrix.rows(),
+ columns = matrix.columns();
+ size_t covercount = 0;
+
+ for ( size_t row = 0 ; row < rows ; row++ )
+ for ( size_t col = 0 ; col < columns ; col++ )
+ if ( STAR == mask_matrix(row, col) ) {
+ col_mask[col] = true;
+ covercount++;
+ }
+
+ if ( covercount >= matrix.minsize() ) {
+ #ifdef DEBUG
+ std::cout << "Final cover count: " << covercount << std::endl;
+ #endif
+ return 0;
+ }
+
+ #ifdef DEBUG
+ std::cout << "Munkres matrix has " << covercount << " of " << matrix.minsize() << " Columns covered:" << std::endl;
+ std::cout << matrix << std::endl;
+ #endif
+
+
+ return 3;
+ }
+
+ int step3() {
+ /*
+ Main Zero Search
+
+ 1. Find an uncovered Z in the distance matrix and prime it. If no such zero exists, go to Step 5
+ 2. If No Z* exists in the row of the Z', go to Step 4.
+ 3. If a Z* exists, cover this row and uncover the column of the Z*. Return to Step 3.1 to find a new Z
+ */
+ if ( find_uncovered_in_matrix(0, saverow, savecol) ) {
+ mask_matrix(saverow,savecol) = PRIME; // prime it.
+ } else {
+ return 5;
+ }
+
+ for ( size_t ncol = 0 ; ncol < matrix.columns() ; ncol++ ) {
+ if ( mask_matrix(saverow,ncol) == STAR ) {
+ row_mask[saverow] = true; //cover this row and
+ col_mask[ncol] = false; // uncover the column containing the starred zero
+ return 3; // repeat
+ }
+ }
+
+ return 4; // no starred zero in the row containing this primed zero
+ }
+
+ int step4() {
+ const size_t rows = matrix.rows(),
+ columns = matrix.columns();
+
+ // seq contains pairs of row/column values where we have found
+ // either a star or a prime that is part of the ``alternating sequence``.
+ std::list<std::pair<size_t,size_t> > seq;
+ // use saverow, savecol from step 3.
+ std::pair<size_t,size_t> z0(saverow, savecol);
+ seq.insert(seq.end(), z0);
+
+ // We have to find these two pairs:
+ std::pair<size_t,size_t> z1(-1, -1);
+ std::pair<size_t,size_t> z2n(-1, -1);
+
+ size_t row, col = savecol;
+ /*
+ Increment Set of Starred Zeros
+
+ 1. Construct the ``alternating sequence'' of primed and starred zeros:
+
+ Z0 : Unpaired Z' from Step 4.2
+ Z1 : The Z* in the column of Z0
+ Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists
+ Z[2N+1] : The Z* in the column of Z[2N]
+
+ The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
+ */
+ bool madepair;
+ do {
+ madepair = false;
+ for ( row = 0 ; row < rows ; row++ ) {
+ if ( mask_matrix(row,col) == STAR ) {
+ z1.first = row;
+ z1.second = col;
+ if ( pair_in_list(z1, seq) ) {
+ continue;
+ }
+
+ madepair = true;
+ seq.insert(seq.end(), z1);
+ break;
+ }
+ }
+
+ if ( !madepair )
+ break;
+
+ madepair = false;
+
+ for ( col = 0 ; col < columns ; col++ ) {
+ if ( mask_matrix(row, col) == PRIME ) {
+ z2n.first = row;
+ z2n.second = col;
+ if ( pair_in_list(z2n, seq) ) {
+ continue;
+ }
+ madepair = true;
+ seq.insert(seq.end(), z2n);
+ break;
+ }
+ }
+ } while ( madepair );
+
+ for ( std::list<std::pair<size_t,size_t> >::iterator i = seq.begin() ;
+ i != seq.end() ;
+ i++ ) {
+ // 2. Unstar each starred zero of the sequence.
+ if ( mask_matrix(i->first,i->second) == STAR )
+ mask_matrix(i->first,i->second) = NORMAL;
+
+ // 3. Star each primed zero of the sequence,
+ // thus increasing the number of starred zeros by one.
+ if ( mask_matrix(i->first,i->second) == PRIME )
+ mask_matrix(i->first,i->second) = STAR;
+ }
+
+ // 4. Erase all primes, uncover all columns and rows,
+ for ( size_t row = 0 ; row < mask_matrix.rows() ; row++ ) {
+ for ( size_t col = 0 ; col < mask_matrix.columns() ; col++ ) {
+ if ( mask_matrix(row,col) == PRIME ) {
+ mask_matrix(row,col) = NORMAL;
+ }
+ }
+ }
+
+ for ( size_t i = 0 ; i < rows ; i++ ) {
+ row_mask[i] = false;
+ }
+
+ for ( size_t i = 0 ; i < columns ; i++ ) {
+ col_mask[i] = false;
+ }
+
+ // and return to Step 2.
+ return 2;
+ }
+
+ int step5() {
+ const size_t rows = matrix.rows(),
+ columns = matrix.columns();
+ /*
+ New Zero Manufactures
+
+ 1. Let h be the smallest uncovered entry in the (modified) distance matrix.
+ 2. Add h to all covered rows.
+ 3. Subtract h from all uncovered columns
+ 4. Return to Step 3, without altering stars, primes, or covers.
+ */
+ double h = 100000;//xyzoylz std::numeric_limits<double>::max();
+ for ( size_t row = 0 ; row < rows ; row++ ) {
+ if ( !row_mask[row] ) {
+ for ( size_t col = 0 ; col < columns ; col++ ) {
+ if ( !col_mask[col] ) {
+ if ( h > matrix(row, col) && matrix(row, col) != 0 ) {
+ h = matrix(row, col);
+ }
+ }
+ }
+ }
+ }
+
+ for ( size_t row = 0 ; row < rows ; row++ ) {
+ if ( row_mask[row] ) {
+ for ( size_t col = 0 ; col < columns ; col++ ) {
+ matrix(row, col) += h;
+ }
+ }
+ }
+
+ for ( size_t col = 0 ; col < columns ; col++ ) {
+ if ( !col_mask[col] ) {
+ for ( size_t row = 0 ; row < rows ; row++ ) {
+ matrix(row, col) -= h;
+ }
+ }
+ }
+
+ return 3;
+ }
+
+ Matrix<int> mask_matrix;
+ Matrix<Data> matrix;
+ bool *row_mask;
+ bool *col_mask;
+ size_t saverow = 0, savecol = 0;
+};
+
+
+#endif /* !defined(_MUNKRES_H_) */
diff --git a/src/additional/fall_run_wander.cpp b/src/additional/fall_run_wander.cpp
new file mode 100644
index 0000000..87bb133
--- /dev/null
+++ b/src/additional/fall_run_wander.cpp
@@ -0,0 +1,193 @@
+#include <unistd.h>
+#include <cstdlib>
+#include <algorithm>
+#include "fall_run_wander.h"
+
+/** 鍒ゆ柇璺屽��:
+ 涓や釜鐗瑰緛锛�1銆佸楂樼殑姣旂巼锛�2銆佹瘮鐜囩殑鍙樺寲閫熷害锛堝彇鍙樺寲鍚庣殑鍜屾渶鍒濊璁＄殑灏忓�硷級3銆佽穼鍊掑悗鐨勯噸蹇冧笅闄� 4銆佽穼鍊掕繃绋嬪皬浜�1.5s
+ return 璺屽�掔殑姒傜巼锛屼互鍙婃寔缁殑鏃堕棿
+ */
+
+//void detect_fall(DETECTBOX tmpbox, std::shared_ptr<Track>& track)
+//{
+// float rate = tmpbox(2)/tmpbox(3);
+// if(rate < track->rate)
+// {
+// track->rate = rate;
+// track->isFall = false;
+// }
+//
+//
+// // 鏇存柊track->rate_queue浠ュ強璁板綍minRate鐨勭浉鍏充俊鎭�
+// if(track->rate_queue.size()>=5)
+// {
+// track->rate_queue.pop();
+//
+// }else if(track->rate_queue.isEmpty())
+// {
+// track->minRate = rate;
+// gettimeofday(&track->min_rate_time,0);
+//
+// track->center_point.x = float(tmpbox(0)+tmpbox(2)/2);
+// track->center_point.y = float(tmpbox(1)+tmpbox(3)/2);
+// track->human_h = tmpbox(3);
+// }
+//
+// if(track->rate_queue.size()>0)
+// {
+// if(track->rate_queue.min()>=rate)
+// {
+// track->minRate = rate;
+// gettimeofday(&track->min_rate_time,0);
+// track->center_point.x = float(tmpbox(0)+tmpbox(2)/2);
+// track->center_point.y = float(tmpbox(1)+tmpbox(3)/2);
+// if(rate>0.25)
+// {
+// track->human_h = tmpbox(3);
+// track->human_w = tmpbox(2);
+// }
+// }
+// }
+//
+// if(track->hisMinRate>=rate)
+// {
+// track->hisMinRate=rate;
+// track->min_center_point.x = float(tmpbox(0)+tmpbox(2)/2);
+// track->min_center_point.y = float(tmpbox(1)+tmpbox(3)/2);
+// }
+//
+// track->rate_queue.push(rate);
+//
+// // 鍒ゆ柇鏄惁璺屽��
+//// float fall_threshold = 1.3; //榛樿璋冨弬1.3(鑳藉拰韫蹭笅鍖哄垎鐨勫弬鏁�)
+// gettimeofday(&track->fall_e_time,0); // 褰撳墠姣斾緥鏃堕棿
+// track->time_substract(&track->fall_diff_time,&track->min_rate_time,&track->fall_e_time); //璺濈鏈�灏忔瘮渚嬬殑鏃堕棿
+// int fall_rate_time = (int)(track->fall_diff_time.tv_sec*1000 + track->fall_diff_time.tv_usec/1000);
+// float fall_threshold = m_staticStruct::fall_rate;
+//
+// if((rate > std::min(fall_threshold, 5*track->rate)||track->human_h>=tmpbox(3)*2*track->human_w/tmpbox(2)) && fall_rate_time<2000 && std::min(track->min_center_point.y, track->center_point.y)<float(tmpbox(1)+tmpbox(3)/2))
+// {
+// track->isFall = true;
+//// std::cout << fall_threshold <<"-- fall_threshold--:" << rate << ":" << 5*track->rate <<std::endl;
+// }
+// else if(rate < 0.9)
+// {
+// track->isFall = false;
+// track->fall_total_time = 0 ;
+// track->first_Fall = true;
+// }
+//
+// // 璁＄畻璺屽�掔疆淇″害
+// if(track->isFall)
+// {
+// /**-- start璁＄畻绱Н璺屽�掓椂闂�*/
+// if(track->first_Fall) //鏄惁寮�濮嬭绠楄穼鍊掓椂闂�
+// {
+// track->first_Fall = false;
+// gettimeofday(&track->fall_s_time,0);
+// }
+// gettimeofday(&track->fall_e_time,0);
+// track->time_substract(&track->fall_diff_time,&track->fall_s_time,&track->fall_e_time); //璺濈鏈�灏忔瘮渚嬬殑鏃堕棿
+// track->fall_total_time = (int)(track->fall_diff_time.tv_sec + track->fall_diff_time.tv_usec/1000000);
+//
+// /** -- end璁＄畻绱Н璺屽�掓椂闂�*/
+//
+// track->rateScale = int(std::min(float((rate - 0.6)*72), 100.0f));
+//
+// if(track->rateScale<0)
+// {
+// track->rateScale = 0-track->rateScale;
+// }
+// }else
+// {
+// track->rateScale = int(std::max(float(10 - (fall_threshold - rate)*10), 0.0f));
+// }
+//
+// // std::cout << track->fall_total_time << "::" << fall_rate_time << ":" <<track->min_center_point.y <<"-"<< track->center_point.y << ":"<<float(tmpbox(1)+tmpbox(3)/2) <<"-- fall--:" << rate << ":" << track->isFall <<std::endl;
+// // std::cout << track->human_h << ":::f:::" << tmpbox(3)*4*track->human_w/tmpbox(2) <<std::endl;
+//}
+
+/**
+ 绠楁硶鎬濇兂: 鍒ゆ柇鐐圭殑浜轰綋鑴氱偣鐨勭Щ鍔ㄧЩ鍔ㄩ�熷害锛岀Щ鍔ㄧ殑閫熷害涓哄崟甯х殑绉诲姩璺濈姣斾笂鍗曞抚鐢婚潰鎵�鑺辫垂鐨勬椂闂�
+ 鍋氫簡涓�涓�10甯х殑闃熷垪锛屽闃熷垪涓殑閫熷害杩涜骞虫粦澶勭悊
+
+ 鏀硅繘鐐癸細鏍规嵁涓嶅悓鐨勭敾闈綅缃� 杩涜绉诲姩璺濈鐨勬斁缂�
+ 妯旱鐢婚潰璺濈鐨勬潈閲嶅钩琛★紙妯旱鐨勫彉鍖栭�熷害涓嶄竴鏍凤級
+*/
+float detect_runing(DETECTBOX tmpbox, std::shared_ptr<Track>& track, cv::Mat img)
+{
+ float human_pointX = tmpbox(0) + tmpbox(2)/2;
+ float human_pointY = tmpbox(1);
+ double rate = tmpbox(2)/tmpbox(3);
+ double curr_area = tmpbox(2)*tmpbox(3);
+
+ float move_velocity = 0;
+ float ave_velocity = 0;
+ if(track->is_runing_p)
+ {
+ track->last_point.x = human_pointX;
+ track->last_point.y = human_pointY;
+ track->is_runing_p = false;
+ }else
+ {
+ SPoint curr_point(human_pointX, human_pointY);
+ float move_distance = calDistance(curr_point, track->last_point);
+ if(tmpbox(1)<0 ||tmpbox(1)+tmpbox(3)>=img.rows)
+ {
+ ave_velocity=0;
+ queue<float> empty;
+ swap(empty,track->velocity_queue);
+ track->sum_velocity=0;
+ track->run_rate = rate;
+ }
+ move_velocity = move_distance/track->single_time;
+
+// std::cout << curr_point.x << ":" << curr_point.y <<"-------------------" <<track->last_point.x<< "::" << track->last_point.y <<std::endl;
+// std::cout << move_distance <<"-------- move_velocity:" << track->single_time << "::" << move_velocity <<std::endl;
+
+ track->last_point.x = human_pointX;
+ track->last_point.y = human_pointY;
+
+ if(track->velocity_queue.size()==10)
+ {
+ float last_velocity = track->velocity_queue.front();
+ track->sum_velocity -= last_velocity;
+ track->velocity_queue.pop();
+// std::cout << "-------- last_velocity:" << last_velocity <<std::endl;
+ }
+ track->velocity_queue.push(move_velocity);
+ track->sum_velocity += move_velocity;
+ }
+
+ if (track->velocity_queue.size()>0)
+ {
+ ave_velocity = track->sum_velocity/track->velocity_queue.size();
+ }else
+ {
+ ave_velocity = 0;
+ }
+
+ double diff_rate = fabs(rate - track->run_rate);
+ double min_area = curr_area>track->rate_area?track->rate_area:curr_area;
+ double max_area = curr_area>track->rate_area?curr_area:track->rate_area;
+ double rate_area = fabs(min_area/max_area); //闈㈢Н姣斾緥鍙樺寲
+ double last_diff_rate = fabs(rate - track->last_rate);
+// std::cout <<rate_area<<"::::"<< rate << " ---------------------------------rate:"<<track->run_rate <<"===== diff_rate:" << diff_rate <<":::" << last_diff_rate <<std::endl;
+ if(diff_rate>0.2 || last_diff_rate>0.2 || tmpbox(1)<0 || rate_area<0.8 ||tmpbox(1)+tmpbox(3)>=img.rows)
+ {
+ ave_velocity=0;
+ queue<float> empty;
+ swap(empty,track->velocity_queue);
+ track->sum_velocity=0;
+ track->run_rate = rate;
+ }
+ track->last_rate = rate;
+ track->rate_area = curr_area;
+
+// std::cout << move_velocity << " :"<<track->sum_velocity <<"===== sum_velocity:" << track->velocity_queue.size() << "::" << ave_velocity <<std::endl;
+
+// std::cout << ave_velocity << "===== m_staticStruct::mv_velocity:" << std::endl;
+
+
+ return ave_velocity;
+}
\ No newline at end of file
diff --git a/src/additional/fall_run_wander.h b/src/additional/fall_run_wander.h
new file mode 100644
index 0000000..c2703ba
--- /dev/null
+++ b/src/additional/fall_run_wander.h
@@ -0,0 +1,16 @@
+//
+// Created by Scheaven on 2020/6/1.
+//
+
+#ifndef INC_01_DARKNET_SORT_FALL_RUN_WANDER_H
+#define INC_01_DARKNET_SORT_FALL_RUN_WANDER_H
+
+#include "../tracker_tools/track.h"
+#include "../config.h"
+
+
+void detect_fall(DETECTBOX tmpbox, std::shared_ptr<Track>& track);
+
+float detect_runing(DETECTBOX tmpbox, std::shared_ptr<Track>& track, cv::Mat img);
+
+#endif //INC_01_DARKNET_SORT_FALL_RUN_WANDER_H
diff --git a/src/config.h b/src/config.h
index 78526b6..e7f6f80 100644
--- a/src/config.h
+++ b/src/config.h
@@ -29,6 +29,16 @@
#include "cuda_runtime_api.h"
#include "NvInfer.h"
+const int nn_budget=100;
+const float max_cosine_distance=0.7;
+
+#define NN_BUDGET 100
+#define MAX_COSINE_DISTANCE 0.7
+#define MAX_IOU_DISTANCE 0.9
+#define MAX_AGE 450
+#define MAX_OUT_TIME 30.0
+#define N_INIT 5
+
struct bbox_t {
unsigned int x, y, w, h; // (x,y) - top-left corner, (w, h) - width & height of bounded box
float prob; // confidence - probability that the object was found correctly
@@ -42,6 +52,11 @@
{
static std::string model_path;
static int type;
+ static bool isTrack;
+ static int max_cam_num;
+ static int wander_time;
+ static float mv_velocity;
+ static float fall_rate;
}M_STATICSTRUCT, *P_STATICSTRUCT;
#endif //INC_01_CPP_SORT_CONFIG_H
diff --git a/src/core/ari_manager.cpp b/src/core/ari_manager.cpp
index 857f820..0ec2289 100644
--- a/src/core/ari_manager.cpp
+++ b/src/core/ari_manager.cpp
@@ -3,7 +3,7 @@
//
#include "ari_manager.h"
-
+#include "../additional/fall_run_wander.h"
AriManager::AriManager()
{
@@ -25,7 +25,90 @@
}
-void AriManager::single_SDK(const int cam_id, const void *img, TResult& t_result)
+void AriManager::release()
+{
+ DetecterManager::getInstance()->release();
+}
+
+void AriManager::init_load_model()
+{
+ DEBUG("::loading detecter model!");
+ DetecterManager::getInstance();
+}
+
+bool AriManager::add_cam(const int cam_id)
+{
+ if (cam_id==(this->CAMERAS_VCT.size()+1))
+ {
+ auto cam_tracker = std::make_shared<tracker>(max_cosine_distance, nn_budget);
+ this->CAMERAS_VCT.push_back(cam_tracker);
+ return true;
+ }else if(cam_id<this->CAMERAS_VCT.size())
+ {
+ WARN("The camera ID " + to_string(cam_id) + "is occupied!");
+ return false;
+ }else
+ {
+ WARN("Camera ID" + to_string(cam_id) + "discontinuous!");
+ return false;
+ }
+}
+
+void AriManager::single_SDK(const int cam_id, const void *img, TResult *t_result, char* img_time, const char* mode)
+{
+ TImage *frame_img = (TImage*)img;
+ // DEBUG((boost::format("%f, %f")%frame_img->width %frame_img->height).str());
+ cv::Mat frame(Size(frame_img->width, frame_img->height), CV_8UC3); // size 鏄痺,h Mat鏄痟,w
+ frame.data = (uchar*)frame_img->data; //娉ㄦ剰涓嶈兘鍐欎负锛�(uchar*)pFrameBGR->data
+ cv::Mat draw_frame = frame.clone();
+ Timer s_timer;
+ s_timer.reset();
+ DETECTIONS detections;
+ DetecterManager::getInstance()->detecter_main(draw_frame, detections);
+ s_timer.out("eve detecter_main");
+
+ std::string mode_type = mode;
+ DEBUG("detections human size::" + to_string(detections.size()));
+ if(detections.size()>0)
+ {
+ if (mode_type == "video")
+ {
+ // if(HUMAN_STRUCT::reid_Extractor->featsEncoder(frame, detections) == false)
+ // {
+ // WARN("Encoder human feature failed!");
+ // }
+ // auto t_strat2 = std::chrono::steady_clock::now();
+ // s_timer.out(to_string(detections.size())+" :eve reid_Extractor");
+ single_tracker(cam_id, detections, img_time);
+ s_timer.out("eve single_tracker");
+ // auto t_strat3 = std::chrono::steady_clock::now();
+ // std::cout<< "detections.size()" << detections.size()<< std::endl;
+ switch_SDK_TResult(cam_id, frame, detections, t_result);
+ // auto t_strat4 = std::chrono::steady_clock::now();
+ s_timer.out("eve switch_SDK_TResult");
+ // double t_9 = std::chrono::duration<double, std::milli>(t_strat8 - t_strat9).count();
+ // double t_8 = std::chrono::duration<double, std::milli>(t_strat7 - t_strat8).count();
+ // double t_7 = std::chrono::duration<double, std::milli>(t_strat6 - t_strat7).count();
+ // double t_6 = std::chrono::duration<double, std::milli>(t_strat1 - t_strat6).count();
+ // double t_1 = std::chrono::duration<double, std::milli>(t_strat2 - t_strat1).count();
+ // double t_2 = std::chrono::duration<double, std::milli>(t_strat3 - t_strat2).count();
+ // double t_3 = std::chrono::duration<double, std::milli>(t_strat4 - t_strat3).count();
+ // double t_0 = std::chrono::duration<double, std::milli>(t_strat4 - t_strat9).count();
+ // std::cout << "fps time:" << to_string(t_9) << " : " << to_string(t_8) << " : " << to_string(t_7) << " : " << to_string(t_6) <<std::endl;
+ // std::cout << "fps runing_time:" << to_string(t_1) << " : " << to_string(t_2) << " : " << to_string(t_3) << std::endl;
+ // std::cout << "fps all time " << t_0<< std::endl;
+ // }
+ DEBUG(":::::::::single_SDK end:::::");
+ }
+ else
+ {
+ switch_SDK_TResult(detections, t_result);
+ }
+ }
+
+}
+
+void AriManager::single_SDK(const int cam_id, const void *img, TResult* t_result)
{
TImage *frame_img = (TImage*)img;
cv::Mat frame(Size(frame_img->width, frame_img->height), CV_8UC3);
@@ -59,8 +142,7 @@
// cv::waitKey(0);
this->detector->detect(batch_img, batch_res);
- t_result.targets = (Target*)malloc(sizeof(Target)*batch_res[0].size());
- // 灏嗙畻娉曠粨鏋滆浆鍖栦负鏍囧噯鐨勬牸寮忥紙浠ョ洰鏍囨娴嬩负渚嬶級""
+ t_result->targets = (Target*)malloc(sizeof(Target)*batch_res[0].size());
int w_count = 0;
for (const auto &result_box:batch_res[0])
{
@@ -74,31 +156,277 @@
target.rect.right = result_box.rect.x + result_box.rect.width;
target.rect.bottom = result_box.rect.y + result_box.rect.height;
- target.confidence = result_box.prob*100; // 缃俊搴﹁浆鍖栨垚鐧惧垎鍒�
+ target.confidence = result_box.prob*100;
// target.id = 1; //
// target.attribute 鍙牴鎹疄闄呮儏鍐垫潵娣诲姞锛岃緭鍑烘牸寮忚姹傛槸json鏍煎紡
- //string attribute_json = "{";
- //attribute_json += "\"smokingScore\":" + to_string(track->smokeScore)+",";
- //if(attribute_json.length()>2)
- //{
- // attribute_json = attribute_json.substr(0, attribute_json.length()-1) +"}";
- // target.attribute = new char[strlen(attribute_json.c_str())+1];
- // target.attribute_size = strlen(attribute_json.c_str());
- // strcpy(target.attribute, attribute_json.c_str());
- //}
+ float mv_velocity = 0;
+ int runScore = 0;
+ string attribute_json = "{";
- t_result.targets[w_count] = target;
+ if (m_staticStruct::fall_rate!=0)
+ {
+ // detect_fall(tmpbox, track);
+ attribute_json += "\"fallScore\":" + to_string(0)+",";
+ }
+
+ if (m_staticStruct::mv_velocity!=0)
+ {
+ attribute_json += "\"runScore\":" + to_string(0)+",";
+ }
+
+ attribute_json += "\"hatScore\":" + to_string(0)+",";
+
+ attribute_json += "\"helmetScore\":" + to_string(0)+",";
+
+ attribute_json += "\"headScore\":" + to_string(0)+",";
+
+ attribute_json += "\"maskScore\":" + to_string(0)+",";
+
+ attribute_json += "\"smokingScore\":" + to_string(0)+",";
+
+ DEBUG("image attribute_json:: "+ attribute_json);
+
+ if(attribute_json.length()>2)
+ {
+ //杞崲杈撳嚭鐨刯son鏍煎紡 {"fallScore":100,"runScore":15.8,"wanderScore":10}
+ attribute_json = attribute_json.substr(0, attribute_json.length()-1) +"}";
+ target.attribute = new char[strlen(attribute_json.c_str())+1];
+ target.attribute_size = strlen(attribute_json.c_str());
+ strcpy(target.attribute, attribute_json.c_str());
+ }
+
+ t_result->targets[w_count] = target;
w_count ++;
}
}
std::cout << "eve batch_res size:: "<< batch_res[0].size() << " w_count: " << w_count <<std::endl;
- t_result.count = w_count;
- draw_SDK_result(cam_id, frame, t_result); //澶氱嚎绋嬫棤娉曡皟鐢ㄧ粯鍥�
+ t_result->count = w_count;
+ // draw_SDK_result(cam_id, frame, t_result);
}
-void AriManager::init_target(Target *t){
+
+void AriManager::switch_SDK_TResult(DETECTIONS detections, TResult *t_result)
+{
+ t_result->targets = (Target*)malloc(sizeof(Target) * detections.size());
+ int w_count = 0;
+ for(auto& detection :detections)
+ {
+ Target target;
+ init_target(&target);
+ // RESULT_STRUCT result;
+
+ DETECTBOX tmpbox = detection.tlwh;
+
+ istringstream iss(random_int(6));
+ iss >> target.id;
+
+ target.rect.left = tmpbox(0);
+ target.rect.top = tmpbox(1);
+ target.rect.right = tmpbox(0) + tmpbox(2);
+ target.rect.bottom = tmpbox(1) + tmpbox(3);
+
+ target.confidence = detection.confidence*100;
+
+ float mv_velocity = 0;
+ int runScore = 0;
+ string attribute_json = "{";
+
+ if (m_staticStruct::fall_rate!=0)
+ {
+ // detect_fall(tmpbox, track);
+ attribute_json += "\"fallScore\":" + to_string(detection.fallScore)+",";
+ }
+
+ if (m_staticStruct::mv_velocity!=0)
+ {
+ attribute_json += "\"runScore\":" + to_string(detection.runScore)+",";
+ }
+
+ attribute_json += "\"hatScore\":" + to_string(detection.hatScore)+",";
+
+ attribute_json += "\"helmetScore\":" + to_string(detection.helmetScore)+",";
+
+ attribute_json += "\"headScore\":" + to_string(detection.headScore)+",";
+
+ attribute_json += "\"maskScore\":" + to_string(detection.maskScore)+",";
+
+ attribute_json += "\"smokingScore\":" + to_string(detection.smokeScore)+",";
+
+ DEBUG("image attribute_json:: "+ attribute_json);
+
+ if(attribute_json.length()>2)
+ {
+ //杞崲杈撳嚭鐨刯son鏍煎紡 {"fallScore":100,"runScore":15.8,"wanderScore":10}
+ attribute_json = attribute_json.substr(0, attribute_json.length()-1) +"}";
+ target.attribute = new char[strlen(attribute_json.c_str())+1];
+ target.attribute_size = strlen(attribute_json.c_str());
+ strcpy(target.attribute, attribute_json.c_str());
+ }
+
+ t_result->targets[w_count] = target;
+ w_count ++;
+ }
+
+ t_result->count = w_count;
+}
+
+// 杞崲鎴怱DK 鎯宠鐨勭粨鏋�
+void AriManager::switch_SDK_TResult(int cam_id, cv::Mat img, DETECTIONS detections, TResult *t_result)
+{
+ t_result->targets = (Target*)malloc(sizeof(Target) * this->CAMERAS_VCT[cam_id]->tracks.size());
+ int w_count = 0;
+ for(auto& track :this->CAMERAS_VCT[cam_id]->tracks)
+ {
+ // if(not track->isCurrent) //褰撳墠鐢婚潰娌℃湁鍒欒繑鍥�
+ // continue;
+ Target target;
+ init_target(&target);
+ // RESULT_STRUCT result;
+ if(!track->is_confirmed() || track->time_since_update >= 1) continue;
+ // if(track->is_confirmed() && track->time_since_update >= 1) continue;
+
+ /*
+ * 绠楁硶鎬濇兂锛氳窡韪櫒璺熻釜鐨勬椂鍊欙紝鏄惁鑳藉寮哄埗灏嗛娴嬮敊浣嶇疆鐨刡ox妗嗗己鍒舵洿鏂版垚鏂扮殑box妗嗭紝鑰宨d涓嶅彂鐢熷彉鍖�
+ * */
+
+// DETECTBOX tmpbox = track->to_tlwh();
+ DETECTBOX tmpbox = track->to_xywh();
+
+ target.rect.left = tmpbox(0);
+ target.rect.top = tmpbox(1);
+ target.rect.right = tmpbox(0) + tmpbox(2);
+ target.rect.bottom = tmpbox(1) + tmpbox(3);
+ // DEBUG((boost::format("%s:%d, %s:%d, %s:%d, %s:%d") %"id:" %track->track_id %"conf:" %track->confidence %", to tmpbox y:" %tmpbox(1) %", tmpbox(3) h:" %tmpbox(3)).str());
+
+ target.id = track->track_id;
+ target.confidence = track->confidence;
+
+ float mv_velocity = 0;
+ int runScore = 0;
+ string attribute_json = "{";
+
+ if (m_staticStruct::fall_rate!=0)
+ {
+ // detect_fall(tmpbox, track);
+ attribute_json += "\"fallScore\":" + to_string(track->rateScale)+"," + "\"fallTime\":" + to_string(track->fall_total_time) + ",";
+ }
+
+ if (m_staticStruct::mv_velocity!=0)
+ {
+ if(track->isRuning)
+ {
+ mv_velocity = detect_runing(tmpbox, track, img);
+ float run_velocity = mv_velocity;
+ if(tmpbox(2)/tmpbox(3)<0)
+ {
+ run_velocity*=(img.cols*img.rows)/(tmpbox(2)*tmpbox(3));
+ }else
+ {
+ run_velocity*=10;
+ }
+// std::cout << mv_velocity << "mv_velocity::::::::::" << run_velocity << "::::::::::::"<<img.cols*img.rows<<":::"<<tmpbox(2)*tmpbox(3)<<std::endl;
+ runScore = (track->confidence+2)*int(std::min(4*run_velocity, 100.0f));
+ attribute_json += "\"runScore\":" + to_string(runScore)+",";
+ }else if(track->is_hat)
+ {
+ mv_velocity = detect_runing(tmpbox, track, img);
+ float run_velocity = mv_velocity;
+ if(tmpbox(2)/tmpbox(3)<0)
+ {
+ run_velocity*=(img.cols*img.rows)/(tmpbox(2)*tmpbox(3));
+ }else
+ {
+ run_velocity*=15;
+ }
+// std::cout << mv_velocity << "mv_velocity::::::::::" << run_velocity << "::::::::::::"<<img.cols*img.rows<<":::"<<tmpbox(2)*tmpbox(3)<<std::endl;
+ runScore = int(std::min(4*run_velocity, 100.0f));
+ attribute_json += "\"runScore\":" + to_string(runScore)+",";
+ }else
+ attribute_json += "\"runScore\":" + to_string(runScore)+",";
+
+
+ }
+
+ //std::cout << track->track_id <<"-- human wander last time:" << track->last_time << ":" << m_staticStruct::wander_time <<std::endl;
+
+ if(m_staticStruct::wander_time!=0)
+ {
+ if (track->last_time>m_staticStruct::wander_time)
+ track->isWander = true;
+ else
+ track->isWander = false;
+ attribute_json += "\"wanderTime\":" + to_string(track->last_time)+",";
+ }
+
+ if(track->is_hat)
+ {
+ attribute_json += "\"hatScore\":" + to_string(track->hatScore)+",";
+ }else
+ {
+ attribute_json += "\"hatScore\":" + to_string(10)+",";
+ }
+
+ if(track->is_mask)
+ {
+ attribute_json += "\"maskScore\":" + to_string(track->maskScore)+",";
+ }else
+ {
+ attribute_json += "\"maskScore\":" + to_string(10)+",";
+ }
+
+
+ if(track->is_smoke)
+ {
+ attribute_json += "\"smokingScore\":" + to_string(track->smokeScore)+",";
+ }else
+ {
+ attribute_json += "\"smokingScore\":" + to_string(10)+",";
+ }
+
+// cout << ":::::::::::::"<<attribute_json << endl;
+
+ if(attribute_json.length()>2)
+ {
+ //杞崲杈撳嚭鐨刯son鏍煎紡 {"fallScore":100,"runScore":15.8,"wanderScore":10}
+ attribute_json = attribute_json.substr(0, attribute_json.length()-1) +"}";
+ target.attribute = new char[strlen(attribute_json.c_str())+1];
+ target.attribute_size = strlen(attribute_json.c_str());
+ strcpy(target.attribute, attribute_json.c_str());
+ }
+
+ t_result->targets[w_count] = target;
+ w_count ++;
+ }
+
+ t_result->count = w_count;
+// printf("================================%d\n\n\n\n", t_result->count);
+#ifdef S_DEBUG
+ draw_SDK_result(cam_id, img, t_result);
+#endif
+}
+
+void AriManager::single_tracker(int cam_id, DETECTIONS& detections, char* img_time)
+{
+// DETECTIONS detections = deal_features(boxes, feats_vec); // 浣跨敤浼犻�掔壒寰佽拷韪椂闇�瑕佹墦寮�璇ユ柟娉�
+// printf("cam_id::::%d",cam_id);
+ std::string str2 = img_time;
+ img_time[str2.find_last_of(":")] = '.';
+
+// DEBUG( img_time);
+ auto cam_tracker = this->CAMERAS_VCT[cam_id];
+
+ DEBUG("single_tracker start:: ");
+ cam_tracker->predict();
+ cam_tracker->update(CAMERAS_VCT, detections, cam_id, img_time);
+ DEBUG("single_tracker::");
+ this->CAMERAS_VCT[cam_id] = cam_tracker;
+}
+
+
+void AriManager::init_target(Target *t)
+{
t->attribute = NULL;
t->feature = NULL;
t->id = 0;
@@ -107,3 +435,4 @@
t->attribute_size = 0;
t->feature_size = 0;
}
+
diff --git a/src/core/ari_manager.h b/src/core/ari_manager.h
index 9bec3f9..dd96894 100644
--- a/src/core/ari_manager.h
+++ b/src/core/ari_manager.h
@@ -8,6 +8,10 @@
#include "config.h"
#include "detector.h"
#include "../utils/draw_util.h"
+#include "../tracker_tools/tracker.h"
+#include "detecter_manager.h"
+// #include "../utils/result_util.h"
+
using namespace std;
using namespace cv;
@@ -18,14 +22,26 @@
int cam_id; //鎸囧畾寰呰拷韪憚鍍忔満缂栧彿
Mat frame;
+ std::vector<std::shared_ptr<tracker>> CAMERAS_VCT;
+ std::vector<DETECTIONS> DETECTION_VCT;
+
private:
std::shared_ptr<Detector> detector;
static void init_target(Target *t);
+ void switch_SDK_TResult(int cam_id, cv::Mat img, DETECTIONS detections, TResult *t_result);
+ void switch_SDK_TResult(DETECTIONS detections, TResult *t_result);
+ void single_tracker(int cam_id, DETECTIONS& detections, char* img_time);
public:
AriManager();
~AriManager();
- void single_SDK(const int cam_id, const void *img, TResult& t_result); // 妫�娴�+杩借釜 鎺ュ彛(SDK灏佽)
+ void release();
+ void init_load_model();
+ bool add_cam(int cam_id);
+ // void single_detect_tracking(int cam_id, cv::Mat frame, FRAME_RESULT result_vec); // 妫�娴�+杩借釜 鎺ュ彛
+ void single_SDK(const int cam_id, const void *img, TResult *t_result, char* img_time, const char* mode);
+ void single_SDK(const int cam_id, const void *img, TResult* t_result);
+ // void mul_detect_tracking(std::vector<int> cam_ids, std::vector<cv::Mat> frame_vec, std::vector<FRAME_RESULT>& results_vec); // 澶氭憚鍍忓ご杩借釜
};
#endif //INC_01_CPP_SORT_TRACKER_MANAGER_H
diff --git a/src/core/detecter_manager.cpp b/src/core/detecter_manager.cpp
new file mode 100644
index 0000000..526c8d9
--- /dev/null
+++ b/src/core/detecter_manager.cpp
@@ -0,0 +1,126 @@
+//
+// Created by Scheaven on 2019/11/19.
+//
+
+#include "detecter_manager.h"
+#include <thread>
+#include <unistd.h>
+#include <cstdlib>
+
+DetecterManager* DetecterManager::instance = NULL;
+
+DetecterManager* DetecterManager::getInstance()
+{
+ if(instance==NULL)
+ {
+ instance = new DetecterManager();
+ }
+ return instance;
+}
+
+DetecterManager::DetecterManager()
+{
+ Config config;
+ // config.net_type = COMMON;
+ if(m_staticStruct::type==2)
+ config.net_type = SMALL;
+ else
+ config.net_type = COMMON;
+
+
+ this->detector = std::shared_ptr<Detector>(new Detector());
+ this->detector->init(config);
+ std::cout << "loading detector model......" << std::endl;
+}
+
+
+DetecterManager::~DetecterManager()
+{
+}
+
+void DetecterManager::release()
+{
+ // Detector::getInstance()->release();
+
+ delete DetecterManager::instance;
+ DetecterManager::instance = NULL;
+}
+
+void DetecterManager::detecter_main(cv::Mat &mat_image, DETECTIONS& detection)
+{
+ std::vector<BatchResult> batch_res;
+ std::vector<cv::Mat> batch_img;
+
+ batch_img.push_back(mat_image.clone());
+ this->detector->detect(batch_img, batch_res);
+ encoder_features(batch_res, detection);
+
+ // show_result(result_vec);
+// draw_boxes(mat_image, result_vec);
+}
+
+void DetecterManager::encoder_features(std::vector<BatchResult> boxes, DETECTIONS &detection)
+{
+ std::vector<float> confidences;
+ std::vector<int> human_index;
+
+ int result_index=0;
+
+ for (const auto &result_box:boxes[0])
+ {
+// #ifdef S_DEBUG
+// printf("--%d-%d-%d-%d-%d---result_box-----\n", result_box.obj_id, result_box.x, result_box.y, result_box.w, result_box.h);
+// #endif
+ if(result_box.id == 1)
+ {
+ // cv::Rect box = cv::Rect(result_box.x,result_box.y,result_box.w,result_box.h);
+ // get_detections(DETECTBOX(box.x, box.y,box.width, box.height), confidences[idx],d);
+ DETECTION_ROW tmpRow;
+ tmpRow.tlwh = DETECTBOX(result_box.rect.x,result_box.rect.y,result_box.rect.width,result_box.rect.height); //DETECTBOX(x, y, w, h);
+ tmpRow.confidence = result_box.prob*100;
+ tmpRow.obj_id = 0;
+ tmpRow.is_hat = true;
+ tmpRow.is_mask = true;
+ tmpRow.is_smoke = true;
+ tmpRow.hatScore = 0;
+ tmpRow.maskScore = 0;
+ tmpRow.smokeScore = 0;
+ // tmpRow.img_time = this->img_time;
+
+ tmpRow.isFall = false;
+ tmpRow.fallScore = 0;
+ tmpRow.runScore = 0;
+ tmpRow.isRun = false;
+
+// tmpRow.human_face = nullptr;
+
+ int sub_box_index=0;
+ human_index.push_back(result_index);
+
+ detection.push_back(tmpRow);
+ }
+ result_index++;
+ }
+}
+
+// bool DetecterManager::sort_score(bbox_t box1, bbox_t box2)
+// {
+// return (box1.prob>box2.prob);
+// }
+
+// float DetecterManager::box_iou(bbox_t box1, bbox_t box2)
+// {
+// int x1 = std::max(box1.x, box2.x);
+// int y1 = std::max(box1.y, box2.y);
+// int x2 = std::min((box1.x+box1.w),(box2.x+box2.w));
+// int y2 = std::min((box1.y+box1.h),(box2.y+box2.h));
+// float over_area = (x2-x1)*(y2-y1);
+// // printf("over_ares---%f ----%d----%d\n", over_area, box1.w*box1.h, box2.w*box2.h);
+// float iou = over_area/((box1.w*box1.h<box2.w*box2.h)?box1.w*box1.h:box2.w*box2.h);
+// return iou;
+// }
+
+void DetecterManager::set_imgTime(long img_time)
+{
+ this->img_time = img_time;
+}
diff --git a/src/core/detecter_manager.h b/src/core/detecter_manager.h
new file mode 100644
index 0000000..b4aab46
--- /dev/null
+++ b/src/core/detecter_manager.h
@@ -0,0 +1,46 @@
+//
+// Created by Scheaven on 2019/11/19.
+//
+#ifndef DETECTER_MANAGER_H
+#define DETECTER_MANAGER_H
+#include "../config.h"
+#include "../tracker_tools/model.h"
+#include "../utils/draw_util.h"
+// #include "../utils/config_util.h"
+#include "../utils/time_util.h"
+#include "../std_target.h"
+#include "detector.h"
+
+using namespace std;
+
+class DetecterManager {
+private:
+ std::vector<cv::Rect> boxes;
+ std::shared_ptr<Detector> detector;
+
+ struct timeval sys_t1, sys_t2, diff1;
+// Detector detector("../cfg/yolov3.cfg", "../model_dump/yolov3.weights");
+ // bool sort_score(bbox_t box1, bbox_t box2);
+ // float box_iou(bbox_t box1, bbox_t box2);
+
+
+public:
+ static DetecterManager* instance;
+ static DetecterManager* getInstance();
+ long img_time;
+
+public:
+ DetecterManager();
+ ~DetecterManager();
+ void release();
+ void detecter_main(cv::Mat &mat_image, DETECTIONS& detection);
+ void ex_features(string in_path);
+
+public:
+ void encoder_features(std::vector<BatchResult> boxes, DETECTIONS& detection);
+ void set_imgTime(long img_time);
+
+};
+
+
+#endif //INC_04_S_MUTICAM_TRACKING_DETECTER_MANAGER_H
diff --git a/src/detecter_tools/model.h b/src/detecter_tools/model.h
index b23cca9..eb3adff 100644
--- a/src/detecter_tools/model.h
+++ b/src/detecter_tools/model.h
@@ -93,7 +93,7 @@
22, 23, 24, 25, 27, 28, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44,
46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65,
67, 70, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 84, 85, 86, 87, 88, 89, 90};
- uint32_t m_BatchSize = 10;
+ uint32_t m_BatchSize = 1;
nvinfer1::INetworkDefinition* m_Network;
nvinfer1::IBuilder* m_Builder ;
nvinfer1::IHostMemory* m_ModelStream;
diff --git a/src/h_interface.cpp b/src/h_interface.cpp
index 2bd00be..98e2595 100644
--- a/src/h_interface.cpp
+++ b/src/h_interface.cpp
@@ -9,18 +9,28 @@
string m_staticStruct::model_path = "path";
int m_staticStruct::type = 1;
+bool m_staticStruct::isTrack= true;
+int m_staticStruct::max_cam_num = 0;
+int m_staticStruct::wander_time = 0;
+float m_staticStruct::mv_velocity = 0;
+float m_staticStruct::fall_rate = 0;
-// 鍒涘缓
+
API void* create(const char *conf, int *max_chan)
{
- CLog::Initialize("/opt/vasystem/bin/models/baseDetector/log.properties");
+ // CLog::Initialize("/opt/vasystem/bin/models/baseDetector/log.properties");
+ CLog::Initialize("../config/log4cplus.properties");
ReadJsonFromFile(conf);
AriManager *handle = new AriManager();
+ *max_chan = m_staticStruct::max_cam_num;
+ for (int i = 1; i <= m_staticStruct::max_cam_num; ++i)
+ {
+ handle->add_cam(i);
+ }
Timer::getInstance()->reset();
return handle;
}
-// 鍘熸煡鎵剧粨鏋滄帴鍙�
API void* get_result(void *handle, const void *img, const int chan)
{
AriManager *h = (AriManager*)handle;
@@ -28,8 +38,39 @@
init_TResult(t_result);
Timer::getInstance()->out("eveTime before yolo");
- h->single_SDK(chan, img, *t_result);
+ // h->single_SDK(chan, img, *t_result);
Timer::getInstance()->out("eveTime runing yolo");
+ return t_result;
+}
+
+
+API void* get_result2(void *handle, const void *img, const int chan, const char* timestamp, const char* mode)
+{
+ DEBUG("-----------------------begin------------ ");
+ AriManager *h = (AriManager*)handle;
+ TResult *t_result = (TResult*)malloc(sizeof(TResult));
+ init_TResult(t_result);
+
+ if(!m_staticStruct::isTrack)
+ {
+ Timer::getInstance()->out("get_result2 before yolo");
+ h->single_SDK(chan, img, t_result);
+ Timer::getInstance()->out("get_result2 runing yolo");
+ return t_result;
+ }
+
+ h->single_SDK(chan, img, t_result, const_cast<char*>(timestamp), mode);
+ DEBUG("--cam id:" + to_string(chan) + " image human_count:" + to_string(t_result->count));
+ for (int i = 0; i < t_result->count; ++i)
+ {
+ if(mode == "video"){
+ DEBUG("--human_id:" + to_string(t_result->targets[i].id)+" human_confidence:" + to_string(t_result->targets[i].confidence) + " human_attribute:" + std::string(t_result->targets[i].attribute) + " human_top:" + to_string(t_result->targets[i].rect.top)+" human_left:" + to_string(t_result->targets[i].rect.left) + " human_right:" + to_string(t_result->targets[i].rect.right)+ " human_bottom:" + to_string(t_result->targets[i].rect.bottom));
+ }
+ else{
+ DEBUG("human_confidence:" + to_string(t_result->targets[i].confidence) + " human_top:" + to_string(t_result->targets[i].rect.top)+" human_left:" + to_string(t_result->targets[i].rect.left) + " human_right:" + to_string(t_result->targets[i].rect.right)+ " human_bottom:" + to_string(t_result->targets[i].rect.bottom));
+ }
+ }
+ DEBUG("---------------------end over------------------------\n\n");
return t_result;
}
@@ -55,7 +96,6 @@
h = NULL;
}
-// 璇诲彇Json鏂囦欢
void ReadJsonFromFile(const char* filename)
{
Json::Reader reader;
@@ -76,8 +116,19 @@
{
std::string model_path = root["param"]["model_path"].asString();
int type = root["param"]["type"].asInt();
+ bool isTrack = root["param"]["isTrack"].asBool();
+ int max_cam_num = root["param"]["max_cam_num"].asInt();
+ int wander_time = root["param"]["wander_time"].asInt();
+ int mv_velocity = root["param"]["mv_velocity"].asFloat();
+ int fall_rate = root["param"]["fall_rate"].asFloat();
m_staticStruct::model_path = model_path;
m_staticStruct::type = type;
+ m_staticStruct::isTrack = isTrack;
+ m_staticStruct::max_cam_num = max_cam_num;
+ m_staticStruct::wander_time = wander_time;
+ m_staticStruct::mv_velocity = mv_velocity;
+ m_staticStruct::fall_rate = fall_rate;
+
}
in.close();
}
diff --git a/src/h_interface.h b/src/h_interface.h
index 714390d..e0de43d 100644
--- a/src/h_interface.h
+++ b/src/h_interface.h
@@ -15,9 +15,10 @@
API void* create(const char *conf, int *max_chan);
API void release(void* handle);
API void* get_result(void *handle, const void *img, const int chan);
+API void* get_result2(void *handle, const void *img, const int chan,const char* timestamp, const char* mode);
API void release_result(void *res);
-void ReadJsonFromFile(const char* filename); // 璇诲彇json鏂囦欢
+void ReadJsonFromFile(const char* filename);
int setCamConfig(const char* data);
int ReadJsonStr(void* handle, const char* data);
diff --git a/src/tracker_tools/dataType.h b/src/tracker_tools/dataType.h
new file mode 100644
index 0000000..3795e79
--- /dev/null
+++ b/src/tracker_tools/dataType.h
@@ -0,0 +1,45 @@
+#pragma once
+#ifndef DATATYPE_H
+#define DATATYPEH
+
+#include <cstddef>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+
+#define FEATURE_DIM 128 // 鐗瑰緛鎶藉彇鐨勭董搴�
+typedef unsigned char uint8;
+
+typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> DETECTBOX;
+typedef Eigen::Matrix<float, -1, 4, Eigen::RowMajor> DETECTBOXSS;
+typedef Eigen::Matrix<float, 1, 128, Eigen::RowMajor> FEATURE;
+typedef Eigen::Matrix<float, Eigen::Dynamic, 128, Eigen::RowMajor> FEATURESS;
+//typedef std::vector<FEATURE> FEATURESS;
+
+//Kalmanfilter
+//typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_FILTER;
+typedef Eigen::Matrix<float, 1, 8, Eigen::RowMajor> KAL_MEAN;
+typedef Eigen::Matrix<float, 8, 8, Eigen::RowMajor> KAL_COVA;
+typedef Eigen::Matrix<float, 1, 4, Eigen::RowMajor> KAL_HMEAN;
+typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> KAL_HCOVA;
+using KAL_DATA = std::pair<KAL_MEAN, KAL_COVA>;
+using KAL_HDATA = std::pair<KAL_HMEAN, KAL_HCOVA>;
+
+//main
+using RESULT_DATA = std::pair<int, DETECTBOX>;
+
+//tracker:
+using TRACKER_DATA = std::pair<uint64_t, FEATURESS>;
+using MATCH_DATA = std::pair<int, int>;
+typedef struct t{
+ std::vector<MATCH_DATA> matches;
+ std::vector<int> unmatched_tracks;
+ std::vector<int> unmatched_detections;
+}TRACHER_MATCHD;
+
+//linear_assignment:
+typedef Eigen::Matrix<float, -1, -1, Eigen::RowMajor> DYNAMICM;
+
+
+#endif // DATATYPE_H
diff --git a/src/tracker_tools/feature_util.cpp b/src/tracker_tools/feature_util.cpp
new file mode 100644
index 0000000..5e991f0
--- /dev/null
+++ b/src/tracker_tools/feature_util.cpp
@@ -0,0 +1,28 @@
+//
+// Created by Scheaven on 2020/9/3.
+//
+#include "feature_util.h"
+
+double feature_distance(FEATURE feat1, FEATURE feat2)
+{
+ double FeatA=0.0, FeatB=0.0, score=0.0;
+ for (int i = 0; i < 128; i++)
+ {
+ FeatA += double(feat1[i])*double(feat1[i]);
+ FeatB += double(feat2[i])*double(feat2[i]);
+ score += double(feat1[i])*double(feat2[i]);
+ }
+
+
+
+ FeatA = sqrt(FeatA);
+ FeatB = sqrt(FeatB);
+
+ score = score / (FeatA * FeatB);
+
+ if (score < 0) {
+ score = 0;
+ }
+ printf("--璇勫垎---%f--",score);
+ return score;
+}
diff --git a/src/tracker_tools/feature_util.h b/src/tracker_tools/feature_util.h
new file mode 100644
index 0000000..f303216
--- /dev/null
+++ b/src/tracker_tools/feature_util.h
@@ -0,0 +1,14 @@
+//
+// Created by Scheaven on 2020/9/3.
+//
+
+#ifndef FEATURE_UTIL_H
+#define FEATURE_UTIL_H
+
+#include "dataType.h"
+#include "../config.h"
+
+double feature_distance(FEATURE feat1, FEATURE feat2);
+
+
+#endif //FEATURE_UTIL_H
diff --git a/src/tracker_tools/kalmfilter.cpp b/src/tracker_tools/kalmfilter.cpp
new file mode 100644
index 0000000..1282bd5
--- /dev/null
+++ b/src/tracker_tools/kalmfilter.cpp
@@ -0,0 +1,155 @@
+#include "kalmfilter.h"
+#include <Eigen/Cholesky>
+
+const double KalmFilter::chi2inv95[10] = {
+ 0,
+ 3.8415,
+ 5.9915,
+ 7.8147,
+ 9.4877,
+ 11.070,
+ 12.592,
+ 14.067,
+ 15.507,
+ 16.919
+};
+KalmFilter::KalmFilter()
+{
+ int ndim = 4;
+ double dt = 1.;
+
+ _motion_mat = Eigen::MatrixXf::Identity(8, 8);
+ for(int i = 0; i < ndim; i++) {
+ _motion_mat(i, ndim+i) = dt;
+ }
+ _update_mat = Eigen::MatrixXf::Identity(4, 8);
+
+ this->_std_weight_position = 1. / 20;
+ this->_std_weight_velocity = 1. / 160;
+}
+
+KalmFilter::~KalmFilter()
+{
+
+}
+
+KAL_DATA KalmFilter::initiate(const DETECTBOX &measurement)
+{
+ DETECTBOX mean_pos = measurement;
+ DETECTBOX mean_vel;
+ for(int i = 0; i < 4; i++) mean_vel(i) = 0;
+
+ KAL_MEAN mean;
+ for(int i = 0; i < 8; i++){
+ if(i < 4) mean(i) = mean_pos(i);
+ else mean(i) = mean_vel(i - 4);
+ }
+
+ KAL_MEAN std;
+ std(0) = 2 * _std_weight_position * measurement[3];
+ std(1) = 2 * _std_weight_position * measurement[3];
+ std(2) = 1e-2;
+ std(3) = 2 * _std_weight_position * measurement[3];
+ std(4) = 10 * _std_weight_velocity * measurement[3];
+ std(5) = 10 * _std_weight_velocity * measurement[3];
+ std(6) = 1e-5;
+ std(7) = 10 * _std_weight_velocity * measurement[3];
+
+ KAL_MEAN tmp = std.array().square();
+ KAL_COVA var = tmp.asDiagonal();
+ return std::make_pair(mean, var);
+}
+
+void KalmFilter::predict(KAL_MEAN &mean, KAL_COVA &covariance)
+{
+ //revise the data;
+ DETECTBOX std_pos;
+ std_pos << _std_weight_position * mean(3),
+ _std_weight_position * mean(3),
+ 1e-2,
+ _std_weight_position * mean(3);
+ DETECTBOX std_vel;
+ std_vel << _std_weight_velocity * mean(3),
+ _std_weight_velocity * mean(3),
+ 1e-5,
+ _std_weight_velocity * mean(3);
+ KAL_MEAN tmp;
+ tmp.block<1,4>(0,0) = std_pos;
+ tmp.block<1,4>(0,4) = std_vel;
+ tmp = tmp.array().square();
+ KAL_COVA motion_cov = tmp.asDiagonal();
+ KAL_MEAN mean1 = this->_motion_mat * mean.transpose();
+ KAL_COVA covariance1 = this->_motion_mat * covariance *(_motion_mat.transpose());
+ covariance1 += motion_cov;
+
+ mean = mean1;
+ covariance = covariance1;
+}
+
+KAL_HDATA KalmFilter::project(const KAL_MEAN &mean, const KAL_COVA &covariance)
+{
+ DETECTBOX std;
+ std << _std_weight_position * mean(3), _std_weight_position * mean(3),
+ 1e-1, _std_weight_position * mean(3);
+ KAL_HMEAN mean1 = _update_mat * mean.transpose();
+ KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose());
+ Eigen::Matrix<float, 4, 4> diag = std.asDiagonal();
+ diag = diag.array().square().matrix();
+ covariance1 += diag;
+// covariance1.diagonal() << diag;
+ return std::make_pair(mean1, covariance1);
+}
+
+KAL_DATA
+KalmFilter::update(
+ const KAL_MEAN &mean,
+ const KAL_COVA &covariance,
+ const DETECTBOX &measurement)
+{
+ KAL_HDATA pa = project(mean, covariance);
+ KAL_HMEAN projected_mean = pa.first;
+ KAL_HCOVA projected_cov = pa.second;
+
+ //chol_factor, lower =
+ //scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False)
+ //kalmain_gain =
+ //scipy.linalg.cho_solve((cho_factor, lower),
+ //np.dot(covariance, self._upadte_mat.T).T,
+ //check_finite=False).T
+ Eigen::Matrix<float, 4, 8> B = (covariance * (_update_mat.transpose())).transpose();
+ Eigen::Matrix<float, 8, 4> kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4
+ Eigen::Matrix<float, 1, 4> innovation = measurement - projected_mean; //eg.1x4
+ auto tmp = innovation*(kalman_gain.transpose());
+ KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix();
+ KAL_COVA new_covariance = covariance - kalman_gain*projected_cov*(kalman_gain.transpose());
+ return std::make_pair(new_mean, new_covariance);
+}
+
+Eigen::Matrix<float, 1, -1>
+KalmFilter::gating_distance(
+ const KAL_MEAN &mean,
+ const KAL_COVA &covariance,
+ const std::vector<DETECTBOX> &measurements,
+ bool only_position)
+{
+ KAL_HDATA pa = this->project(mean, covariance);
+ // if(only_position) {
+ // printf("not implement!");
+ // exit(0);
+ // }
+ KAL_HMEAN mean1 = pa.first;
+ KAL_HCOVA covariance1 = pa.second;
+
+// Eigen::Matrix<float, -1, 4, Eigen::RowMajor> d(size, 4);
+ DETECTBOXSS d(measurements.size(), 4);
+ int pos = 0;
+ for(DETECTBOX box:measurements) {
+ d.row(pos++) = box - mean1;
+ }
+ Eigen::Matrix<float, -1, -1, Eigen::RowMajor> factor = covariance1.llt().matrixL();
+ Eigen::Matrix<float, -1, -1> z = factor.triangularView<Eigen::Lower>().solve<Eigen::OnTheRight>(d).transpose();
+ auto zz = ((z.array())*(z.array())).matrix();
+ auto square_maha = zz.colwise().sum();
+ return square_maha;
+}
+
diff --git a/src/tracker_tools/kalmfilter.h b/src/tracker_tools/kalmfilter.h
new file mode 100644
index 0000000..e9420fd
--- /dev/null
+++ b/src/tracker_tools/kalmfilter.h
@@ -0,0 +1,32 @@
+#ifndef KALMFILTER_H
+#define KALMFILTER_H
+
+#include "dataType.h"
+
+class KalmFilter
+{
+public:
+ static const double chi2inv95[10];
+ KalmFilter();
+ ~KalmFilter();
+ KAL_DATA initiate(const DETECTBOX& measurement);
+ void predict(KAL_MEAN& mean, KAL_COVA& covariance);
+ KAL_HDATA project(const KAL_MEAN& mean, const KAL_COVA& covariance);
+ KAL_DATA update(const KAL_MEAN& mean,
+ const KAL_COVA& covariance,
+ const DETECTBOX& measurement);
+
+ Eigen::Matrix<float, 1, -1> gating_distance(
+ const KAL_MEAN& mean,
+ const KAL_COVA& covariance,
+ const std::vector<DETECTBOX>& measurements,
+ bool only_position = false);
+
+private:
+ Eigen::Matrix<float, 8, 8, Eigen::RowMajor> _motion_mat;
+ Eigen::Matrix<float, 4, 8, Eigen::RowMajor> _update_mat;
+ float _std_weight_position;
+ float _std_weight_velocity;
+};
+
+#endif // KALMFILTER_H
diff --git a/src/tracker_tools/linear_assignment.cpp b/src/tracker_tools/linear_assignment.cpp
new file mode 100644
index 0000000..8d03bf6
--- /dev/null
+++ b/src/tracker_tools/linear_assignment.cpp
@@ -0,0 +1,161 @@
+#include "linear_assignment.h"
+#include "../Munkres_assign/hungarianoper.h"
+#include <map>
+
+linear_assignment *linear_assignment::instance = NULL;
+linear_assignment::linear_assignment()
+{
+}
+
+linear_assignment *linear_assignment::getInstance()
+{
+ if(instance == NULL) instance = new linear_assignment();
+ return instance;
+}
+
+linear_assignment::~linear_assignment()
+{
+ delete instance;
+}
+
+TRACHER_MATCHD
+linear_assignment::matching_cascade(
+ tracker *distance_metric,
+ tracker::GATED_METRIC_FUNC distance_metric_func,
+ float max_distance,
+ int cascade_depth,
+ std::vector<std::shared_ptr<Track>> &tracks,
+ const DETECTIONS &detections,
+ std::vector<int>& track_indices,
+ std::vector<int> detection_indices)
+{
+ TRACHER_MATCHD res;
+ for(size_t i = 0; i < detections.size(); i++) {
+ detection_indices.push_back(int(i));
+ }
+
+ std::vector<int> unmatched_detections;
+ unmatched_detections.assign(detection_indices.begin(), detection_indices.end());
+ res.matches.clear();
+ std::vector<int> track_indices_l;
+
+ std::map<int, int> matches_trackid;
+ for(int level = 0; level < cascade_depth; level++) {
+ if(unmatched_detections.size() == 0) break; //No detections left;
+
+ track_indices_l.clear();
+ for(int k:track_indices) {
+ if(tracks[k]->time_since_update == 1+level)
+ track_indices_l.push_back(k);
+ }
+ if(track_indices_l.size() == 0) continue; //Nothing to match at this level.
+
+ TRACHER_MATCHD tmp = min_cost_matching(
+ distance_metric, distance_metric_func,
+ max_distance, tracks, detections, track_indices_l,
+ unmatched_detections);
+ unmatched_detections.assign(tmp.unmatched_detections.begin(), tmp.unmatched_detections.end());
+ for(size_t i = 0; i < tmp.matches.size(); i++) {
+ MATCH_DATA pa = tmp.matches[i];
+ res.matches.push_back(pa);
+ matches_trackid.insert(pa);
+ }
+ }
+ res.unmatched_detections.assign(unmatched_detections.begin(), unmatched_detections.end());
+
+ for(size_t i = 0; i < track_indices.size(); i++) {
+ int tid = track_indices[i];
+ if(matches_trackid.find(tid) == matches_trackid.end())
+ res.unmatched_tracks.push_back(tid); // 鏈尮閰嶇殑杩藉姞
+ }
+
+ return res;
+}
+
+TRACHER_MATCHD
+linear_assignment::min_cost_matching(tracker *distance_metric,
+ tracker::GATED_METRIC_FUNC distance_metric_func,
+ float max_distance,
+ std::vector<std::shared_ptr<Track>> &tracks,
+ const DETECTIONS &detections,
+ std::vector<int> &track_indices,
+ std::vector<int> &detection_indices)
+{
+ TRACHER_MATCHD res;
+ if((detection_indices.size() == 0) || (track_indices.size() == 0)) {
+ res.matches.clear();
+ res.unmatched_tracks.assign(track_indices.begin(), track_indices.end());
+ res.unmatched_detections.assign(detection_indices.begin(), detection_indices.end());
+ return res;
+ }
+ DYNAMICM cost_matrix = (distance_metric->*(distance_metric_func))( // distance_metric_func鍏跺疄鏄皟鐢╰racker.cpp涓殑iou_cost 鍑芥暟
+ tracks, detections, track_indices, detection_indices);
+ for(int i = 0; i < cost_matrix.rows(); i++) {
+ for(int j = 0; j < cost_matrix.cols(); j++) {
+ float tmp = cost_matrix(i,j);
+ if(tmp > max_distance) cost_matrix(i,j) = max_distance + 1e-5;
+ }
+ }
+ Eigen::Matrix<float, -1, 2, Eigen::RowMajor> indices = HungarianOper::Solve(cost_matrix);
+ res.matches.clear();
+ res.unmatched_tracks.clear();
+ res.unmatched_detections.clear();
+ for(size_t col = 0; col < detection_indices.size(); col++) {
+ bool flag = false;
+ for(int i = 0; i < indices.rows(); i++)
+ if(indices(i, 1) == col) { flag = true; break;}
+ if(flag == false)res.unmatched_detections.push_back(detection_indices[col]);
+ }
+ for(size_t row = 0; row < track_indices.size(); row++) {
+ bool flag = false;
+ for(int i = 0; i < indices.rows(); i++)
+ if(indices(i, 0) == row) { flag = true; break; }
+ if(flag == false) res.unmatched_tracks.push_back(track_indices[row]);
+ }
+ for(int i = 0; i < indices.rows(); i++) {
+ int row = indices(i, 0);
+ int col = indices(i, 1);
+
+ int track_idx = track_indices[row];
+ int detection_idx = detection_indices[col];
+ if(cost_matrix(row, col) > max_distance) {
+ res.unmatched_tracks.push_back(track_idx);
+ res.unmatched_detections.push_back(detection_idx);
+ } else res.matches.push_back(std::make_pair(track_idx, detection_idx));
+ }
+ return res;
+}
+
+DYNAMICM
+linear_assignment::gate_cost_matrix(
+ KalmFilter *kf,
+ DYNAMICM &cost_matrix,
+ std::vector<std::shared_ptr<Track>> &tracks,
+ const DETECTIONS &detections,
+ const std::vector<int> &track_indices,
+ const std::vector<int> &detection_indices,
+ float gated_cost, bool only_position)
+{
+ int gating_dim = (only_position == true?2:4);
+ double gating_threshold = KalmFilter::chi2inv95[gating_dim];
+ std::vector<DETECTBOX> measurements;
+ for(int i:detection_indices)
+ {
+ DETECTION_ROW t = detections[i];
+ measurements.push_back(t.to_xyah());
+ }
+
+ for(size_t i = 0; i < track_indices.size(); i++)
+ {
+ auto& track = tracks[track_indices[i]];
+ Eigen::Matrix<float, 1, -1> gating_distance = kf->gating_distance(
+ track->mean, track->covariance, measurements, only_position);
+
+ for (int j = 0; j < gating_distance.cols(); j++)
+ {
+ if (gating_distance(0, j) > gating_threshold) cost_matrix(i, j) = gated_cost;
+ }
+ }
+ return cost_matrix;
+}
+
diff --git a/src/tracker_tools/linear_assignment.h b/src/tracker_tools/linear_assignment.h
new file mode 100644
index 0000000..a7bac99
--- /dev/null
+++ b/src/tracker_tools/linear_assignment.h
@@ -0,0 +1,47 @@
+#ifndef LINEAR_ASSIGNMENT_H
+#define LINEAR_ASSIGNMENT_H
+#include "dataType.h"
+#include "tracker.h"
+
+#define INFTY_COST 1e5
+class tracker;
+//for matching;
+class linear_assignment
+{
+ linear_assignment();
+ linear_assignment(const linear_assignment& );
+ linear_assignment& operator=(const linear_assignment&);
+ static linear_assignment* instance;
+
+public:
+ static linear_assignment* getInstance();
+ ~linear_assignment();
+
+ TRACHER_MATCHD matching_cascade(tracker* distance_metric,
+ tracker::GATED_METRIC_FUNC distance_metric_func,
+ float max_distance,
+ int cascade_depth,
+ std::vector<std::shared_ptr<Track>>& tracks,
+ const DETECTIONS& detections,
+ std::vector<int> &track_indices,
+ std::vector<int> detection_indices = std::vector<int>());
+ TRACHER_MATCHD min_cost_matching(
+ tracker* distance_metric,
+ tracker::GATED_METRIC_FUNC distance_metric_func,
+ float max_distance,
+ std::vector<std::shared_ptr<Track>>& tracks,
+ const DETECTIONS& detections,
+ std::vector<int>& track_indices,
+ std::vector<int>& detection_indices);
+ DYNAMICM gate_cost_matrix(
+ KalmFilter* kf,
+ DYNAMICM& cost_matrix,
+ std::vector<std::shared_ptr<Track>>& tracks,
+ const DETECTIONS& detections,
+ const std::vector<int>& track_indices,
+ const std::vector<int>& detection_indices,
+ float gated_cost = INFTY_COST,
+ bool only_position = true);
+};
+
+#endif // LINEAR_ASSIGNMENT_H
diff --git a/src/tracker_tools/model.cpp b/src/tracker_tools/model.cpp
new file mode 100644
index 0000000..4905a8b
--- /dev/null
+++ b/src/tracker_tools/model.cpp
@@ -0,0 +1,23 @@
+#include "model.h"
+
+
+const float kRatio=0.5;
+enum DETECTBOX_IDX {IDX_X = 0, IDX_Y, IDX_W, IDX_H };
+
+DETECTBOX DETECTION_ROW::to_xyah() const
+{//(centerx, centery, ration, h)
+ DETECTBOX ret = tlwh;
+ ret(0,IDX_X) += (ret(0, IDX_W)*kRatio);
+ ret(0, IDX_Y) += (ret(0, IDX_H)*kRatio);
+ ret(0, IDX_W) /= ret(0, IDX_H);
+ return ret;
+}
+
+DETECTBOX DETECTION_ROW::to_tlbr() const
+{//(x,y,xx,yy)
+ DETECTBOX ret = tlwh;
+ ret(0, IDX_X) += ret(0, IDX_W);
+ ret(0, IDX_Y) += ret(0, IDX_H);
+ return ret;
+}
+
diff --git a/src/tracker_tools/model.h b/src/tracker_tools/model.h
new file mode 100644
index 0000000..d6f2abc
--- /dev/null
+++ b/src/tracker_tools/model.h
@@ -0,0 +1,46 @@
+#ifndef MODEL_H
+#define MODEL_H
+#include "dataType.h"
+#include <algorithm>
+#include "../config.h"
+
+// * Each rect's data structure.
+// * tlwh: topleft point & (w,h).
+// * confidence: detection confidence.
+// * feature: the rect's 128d feature.
+// */
+class DETECTION_ROW
+{
+public:
+ int obj_id;
+ DETECTBOX tlwh;
+ float confidence;
+ FEATURE feature;
+ DETECTBOX to_xyah() const;
+ DETECTBOX to_tlbr() const;
+
+ bool isFall;
+ int fallScore;
+
+ bool isRun;
+ int runScore;
+
+ bool is_hat;
+ int hatScore;
+
+ int helmetScore;
+
+ int headScore;
+
+ bool is_mask;
+ int maskScore;
+
+ bool is_smoke;
+ int smokeScore;
+
+};
+
+typedef std::vector<DETECTION_ROW> DETECTIONS;
+
+
+#endif // MODEL_H
diff --git a/src/tracker_tools/nn_matching.cpp b/src/tracker_tools/nn_matching.cpp
new file mode 100644
index 0000000..0f1d4dc
--- /dev/null
+++ b/src/tracker_tools/nn_matching.cpp
@@ -0,0 +1,159 @@
+#include "nn_matching.h"
+
+
+using namespace Eigen;
+
+NearNeighborDisMetric::NearNeighborDisMetric(
+ NearNeighborDisMetric::METRIC_TYPE metric,
+ float matching_threshold, int budget)
+{
+ if(metric == euclidean)
+ {
+ _metric = &NearNeighborDisMetric::_nneuclidean_distance;
+ } else if (metric == cosine)
+ {
+ _metric = &NearNeighborDisMetric::_nncosine_distance;
+ }
+
+ this->mating_threshold = matching_threshold;
+ this->budget = budget;
+ this->samples.clear();
+}
+
+NearNeighborDisMetric::~NearNeighborDisMetric()
+{
+
+}
+
+DYNAMICM
+NearNeighborDisMetric::distance(
+ const FEATURESS &features,
+ const std::vector<uint64_t>& targets)
+{
+ DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(targets.size(), features.rows());
+ int idx = 0;
+ for(uint64_t target:targets) {
+ cost_matrix.row(idx) = (this->*_metric)(this->samples[target], features);
+ idx++;
+ }
+ return cost_matrix;
+}
+
+DYNAMICM
+NearNeighborDisMetric::cos_distance(const FEATURESS &features, std::map<uint64_t, FEATURESS> cos_samples)
+{
+ DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(cos_samples.size(), features.rows());
+ int idx = 0;
+ std::map<uint64_t, FEATURESS>::iterator iter;
+ for (iter=cos_samples.begin(); iter!= cos_samples.end(); iter++)
+ {
+ cost_matrix.row(idx) = (this->*_metric)(iter->second, features);
+ idx++;
+ }
+
+ return cost_matrix;
+}
+
+void
+NearNeighborDisMetric::partial_fit(
+ std::vector<TRACKER_DATA> &tid_feats,
+ std::vector<uint64_t> &active_targets)
+{
+ /*python code:
+ * let feature(target_id) append to samples;
+ * && delete not comfirmed target_id from samples.
+ * update samples;
+*/
+ for(TRACKER_DATA& data:tid_feats) {
+ uint64_t track_id = data.first;
+ FEATURESS newFeatOne = data.second;
+
+ if(samples.find(track_id) != samples.end()) {//append
+ int oldSize = samples[track_id].rows();
+ int addSize = newFeatOne.rows();
+ int newSize = oldSize + addSize;
+
+ if(newSize <= this->budget) {
+ FEATURESS newSampleFeatures(newSize, 128);
+ newSampleFeatures.block(0,0, oldSize, 128) = samples[track_id];
+ newSampleFeatures.block(oldSize, 0, addSize, 128) = newFeatOne;
+ samples[track_id] = newSampleFeatures;
+ } else {
+ if(oldSize < this->budget) { //original space is not enough;
+ FEATURESS newSampleFeatures(this->budget, 128);
+ if(addSize >= this->budget) {
+ newSampleFeatures = newFeatOne.block(0, 0, this->budget, 128);
+ } else {
+ newSampleFeatures.block(0, 0, this->budget-addSize, 128) =
+ samples[track_id].block(addSize-1, 0, this->budget-addSize, 128).eval();
+ newSampleFeatures.block(this->budget-addSize, 0, addSize, 128) = newFeatOne;
+ }
+ samples[track_id] = newSampleFeatures;
+ } else {//original space is ok;
+ if(addSize >= this->budget) {
+ samples[track_id] = newFeatOne.block(0,0, this->budget, 128);
+ } else {
+ samples[track_id].block(0, 0, this->budget-addSize, 128) =
+ samples[track_id].block(addSize-1, 0, this->budget-addSize, 128).eval();
+ samples[track_id].block(this->budget-addSize, 0, addSize, 128) = newFeatOne;
+ }
+ }
+ }
+ } else {//not exit, create new one;
+ samples[track_id] = newFeatOne;
+ }
+ }//add features;
+
+ //erase the samples which not in active_targets;
+ for(std::map<uint64_t, FEATURESS>::iterator i = samples.begin(); i != samples.end();) {
+ bool flag = false;
+ for(uint64_t j:active_targets) if(j == i->first) { flag=true; break; }
+ if(flag == false) samples.erase(i++);
+ else i++;
+ }
+}
+
+Eigen::VectorXf
+NearNeighborDisMetric::_nncosine_distance(
+ const FEATURESS &x, const FEATURESS &y)
+{
+ MatrixXf distances = _cosine_distance(x,y);
+ VectorXf res = distances.colwise().minCoeff().transpose();
+ return res;
+}
+
+Eigen::VectorXf
+NearNeighborDisMetric::_nneuclidean_distance(
+ const FEATURESS &x, const FEATURESS &y)
+{
+ MatrixXf distances = _pdist(x,y);
+ VectorXf res = distances.colwise().maxCoeff().transpose();
+ res = res.array().max(VectorXf::Zero(res.rows()).array());
+ return res;
+}
+
+Eigen::MatrixXf
+NearNeighborDisMetric::_pdist(const FEATURESS &x, const FEATURESS &y)
+{
+ int len1 = x.rows(), len2 = y.rows();
+ if(len1 == 0 || len2 == 0) {
+ return Eigen::MatrixXf::Zero(len1, len2);
+ }
+ MatrixXf res = x * y.transpose()* -2;
+ res = res.colwise() + x.rowwise().squaredNorm();
+ res = res.rowwise() + y.rowwise().squaredNorm().transpose();
+ res = res.array().max(MatrixXf::Zero(res.rows(), res.cols()).array());
+ return res;
+}
+
+Eigen::MatrixXf
+NearNeighborDisMetric::_cosine_distance(
+ const FEATURESS & a,
+ const FEATURESS& b, bool data_is_normalized) {
+ if(data_is_normalized == true) {
+ //undo:
+ assert(false);
+ }
+ MatrixXf res = 1. - (a*b.transpose()).array();
+ return res;
+}
diff --git a/src/tracker_tools/nn_matching.h b/src/tracker_tools/nn_matching.h
new file mode 100644
index 0000000..9630c56
--- /dev/null
+++ b/src/tracker_tools/nn_matching.h
@@ -0,0 +1,35 @@
+#ifndef NN_MATCHING_H
+#define NN_MATCHING_H
+
+#include "dataType.h"
+
+#include <map>
+
+//A tool to calculate distance;
+class NearNeighborDisMetric{
+public:
+ enum METRIC_TYPE{euclidean=1, cosine};
+ NearNeighborDisMetric(METRIC_TYPE metric,
+ float matching_threshold,
+ int budget);
+ ~NearNeighborDisMetric();
+ DYNAMICM distance(const FEATURESS& features, const std::vector<uint64_t> &targets);
+ DYNAMICM cos_distance(const FEATURESS& features, std::map<uint64_t, FEATURESS> cos_samples);
+ // void partial_fit(FEATURESS& features, std::vector<int> targets, std::vector<int> active_targets);
+ void partial_fit(std::vector<TRACKER_DATA>& tid_feats, std::vector<uint64_t>& active_targets);
+ float mating_threshold;
+
+private:
+ typedef Eigen::VectorXf (NearNeighborDisMetric::*PTRFUN)(const FEATURESS&, const FEATURESS&);
+ Eigen::VectorXf _nncosine_distance(const FEATURESS& x, const FEATURESS& y);
+ Eigen::VectorXf _nneuclidean_distance(const FEATURESS& x, const FEATURESS& y);
+
+ Eigen::MatrixXf _pdist(const FEATURESS& x, const FEATURESS& y);
+ Eigen::MatrixXf _cosine_distance(const FEATURESS & a, const FEATURESS& b, bool data_is_normalized = false);
+private:
+ PTRFUN _metric;
+ int budget;
+ std::map<uint64_t, FEATURESS > samples;
+};
+
+#endif // NN_MATCHING_H
diff --git a/src/tracker_tools/track.cpp b/src/tracker_tools/track.cpp
new file mode 100644
index 0000000..1fca42d
--- /dev/null
+++ b/src/tracker_tools/track.cpp
@@ -0,0 +1,312 @@
+#include "track.h"
+Track::Track()
+{
+
+}
+Track::~Track()
+{
+
+}
+Track::Track(KAL_MEAN& mean, KAL_COVA& covariance, uint64_t track_id, int n_init, int max_age, const FEATURE& feature,char* img_time)
+{
+ this->mean = mean;
+ this->covariance = covariance;
+ this->track_id = track_id;
+ this->hits = 1;
+ this->age = 1;
+ this->time_since_update = 0;
+ this->state = TrackState::Tentative;
+ features = FEATURESS(1, 128);
+ features.row(0) = feature;//features.rows() must = 0;
+
+ this->_n_init = n_init;
+ this->_max_age = max_age;
+ this->init_t = true;
+ this->isCurrent = true;
+
+ //鍒濆鍖栧緲寰婂弬鏁�
+ this->last_time = 0;
+// gettimeofday(&this->last_s_time,0); // 寰樺緤璁℃椂寮�鍚�
+ this->last_s_time = char_2_unix(img_time);
+ std::cout<< "==track img time=%s锛�" << img_time << std::endl;
+
+ this->isWander = false;
+
+ //鍒濆鍖栬穼鍊掔殑鍙傛暟
+ this->rate = 100;
+ this->isFall = false;
+ this->first_Fall = true;
+ this->rateScale = 0;
+ this->minRate = 0;
+ this->center_point.x = 0;
+ this->center_point.y = 0;
+ this->fall_total_time = 0;
+ this->hisMinRate = 100;
+ this->human_h = 0;
+ this->human_w = 0;
+
+
+ //鍒濆鍖栧璺戠殑鍙傛暟
+ this->last_point.x = -1;
+ this->last_point.y = -1;
+ this->is_runing_p = true;
+ this->is_runing_t = true;
+ this->isRuning = false;
+ this->sum_velocity = 0;
+ this->confidence = 0;
+ this->run_rate = 0;
+ this->last_rate = 0;
+ this->rate_area = 0.0;
+
+ this->is_hat = false;
+ this->hatScore = 0;
+ this->headScore = 0;
+ this->helmetScore = 0;
+
+ this->is_mask=false;
+ this->maskScore = 0;
+
+ this->is_smoke=false;
+ this->smokeScore = 0;
+
+ // 娣诲姞褰撳墠鐨剎y鍧愭爣
+// memset(&this->start,0,sizeof(struct this->timeval));
+// memset(&this->stop,0,sizeof(struct this->timeval));
+// memset(&this->diff,0,sizeof(struct this->timeval));
+}
+
+void Track::predit(KalmFilter *kf)
+{
+ /*Propagate the state distribution to the current time step using a
+ Kalman filter prediction step.
+
+ Parameters
+ ----------
+ kf : kalman_filter.KalmFilter
+ The Kalman filter.
+ */
+
+ kf->predict(this->mean, this->covariance);
+ this->age += 1;
+ this->time_since_update += 1;
+}
+
+void Track::update(KalmFilter * const kf, const DETECTION_ROW& detection, char* img_time)
+{
+ // 鍒ゆ柇娑堝け涓夌鍚庡垹闄よ杞ㄨ抗
+ if(this->init_t){
+// gettimeofday(&this->start,0);
+ this->start = char_2_unix(img_time);
+ std::cout << "tracks start:: " << this->start << std::endl;
+ this->init_t = false;
+ }
+
+// gettimeofday(&this->stop,0);
+ this->stop = char_2_unix(img_time);
+// this->time_substract(&this->diff, &this->start, &this->stop);
+ this->diff = this->stop - this->start;
+ std::cout << "tracks curr time:: " << this->stop << " tracks curr diff:: "<<(double)this->diff <<std::endl;
+
+// this->sum_timer = (double)((diff.tv_sec*1000000 + diff.tv_usec)/1000000);
+ this->sum_timer = (double)(this->diff)/1000;
+ printf("----Track update time------%f\n\n\n\n\n\n", this->sum_timer);
+// std::cout<< this->sum_timer << "::" << MAX_OUT_TIME<<std::endl;
+
+ if(this->sum_timer > MAX_OUT_TIME)
+ {
+ this->state = TrackState::Deleted;
+ this->_max_age = 0;
+ this->last_time = 0;
+
+// gettimeofday(&this->last_s_time,0); // 閲嶆柊寰樺緤璁℃椂锛堣繖鏄寮�涓嶅埌涓夌閮界畻鏄緲寰婏級
+ this->last_s_time = char_2_unix(img_time);
+ std::cout<< "wander start "<< this->last_s_time << " : " << img_time <<std::endl;
+// std::cout<< "--------------------------delete -----------------------";
+ }
+// gettimeofday(&this->start,0);
+ this->start = char_2_unix(img_time);
+
+ // 璁＄畻寰樺緤鐨勬椂闀�
+// gettimeofday(&this->last_e_time,0);
+ this->last_e_time = char_2_unix(img_time);
+ std::cout<< "wander end "<< this->last_e_time << " : " << img_time <<std::endl;
+// this->time_substract(&this->last_diff_time,&this->last_s_time,&this->last_e_time);
+ this->last_diff_time = this->last_e_time - this->last_s_time;
+// this->last_time = (int)(this->last_diff_time.tv_sec+this->last_diff_time.tv_usec/1000000);
+ this->last_time = (int)(this->last_diff_time/1000);
+ std::cout<< "wander time "<< this->last_time << "::" << this->last_diff_time << "last_s_time" << this->last_s_time << " e: "<< this->last_e_time <<std::endl;
+
+ // 濂旇窇璁板綍鍗曞抚鏃堕棿
+ if(this->is_runing_t)
+ {
+// gettimeofday(&this->single_s_time, 0);
+ this->single_s_time = char_2_unix(img_time);
+
+ this->is_runing_t = false;
+ }else{
+// this->time_substract(&this->single_diff_time,&this->single_s_time,&this->last_e_time);
+ this->single_diff_time = this->last_e_time - this->single_s_time;
+// this->single_time = (int)(this->single_diff_time.tv_sec*1000 + this->single_diff_time.tv_usec/1000);
+ this->single_time = (int)(this->single_diff_time);
+// gettimeofday(&this->single_s_time, 0);
+ this->single_s_time = char_2_unix(img_time);
+ }
+
+
+ KAL_DATA pa = kf->update(this->mean, this->covariance, detection.to_xyah());
+ this->mean = pa.first;
+ this->covariance = pa.second;
+
+ // 娣诲姞褰撳墠鐨剎y鍧愭爣
+ this->xywh = detection.tlwh;
+ this->confidence = (this->confidence*4+detection.confidence)/5;
+
+ if(this->is_hat)
+ {
+ this->is_hat = detection.is_hat;
+ this->hatScore =(int)((this->hatScore*9 + detection.hatScore)/10);
+ this->helmetScore =(int)((this->helmetScore*9 + detection.helmetScore)/10);
+ this->headScore =(int)((this->headScore*9 + detection.headScore)/10);
+ }else
+ {
+ this->is_hat = detection.is_hat;
+ this->hatScore = detection.hatScore;
+ this->helmetScore = detection.helmetScore;
+ this->headScore = detection.headScore;
+ }
+
+
+ this->is_mask = detection.is_mask;
+ this->maskScore = detection.maskScore;
+ this->is_smoke = detection.is_smoke;
+ this->smokeScore = detection.smokeScore;
+ /***yolo 鐩存帴娴嬭穼鍊掍唬鐮佹洿鏂� s*/
+ if(detection.obj_id==0)
+ {
+ this->fall_total_time = 0 ;
+ this->first_Fall = true;
+ this->rateScale = 0;
+ this->isRuning = false;
+ }else if(detection.obj_id==1)
+ {
+ /**-- start璁＄畻绱Н璺屽�掓椂闂�*/
+ if(this->first_Fall) //鏄惁寮�濮嬭绠楄穼鍊掓椂闂�
+ {
+ this->first_Fall = false;
+// gettimeofday(&this->fall_s_time,0);
+ this->fall_s_time = char_2_unix(img_time);
+ }
+// gettimeofday(&this->fall_e_time,0);
+ this->fall_e_time = char_2_unix(img_time);
+// this->time_substract(&this->fall_diff_time,&this->fall_s_time,&this->fall_e_time); //璺濈鏈�灏忔瘮渚嬬殑鏃堕棿
+ this->fall_diff_time = this->fall_e_time - this->fall_s_time;
+// this->fall_total_time = (int)(this->fall_diff_time.tv_sec + this->fall_diff_time.tv_usec/1000000);
+ this->fall_total_time = (int)(this->fall_diff_time/1000);
+
+ /** -- end璁＄畻绱Н璺屽�掓椂闂�*/
+
+ this->rateScale = detection.confidence;
+ this->isRuning = false;
+
+ }else if(detection.obj_id==2)
+ {
+ this->isRuning = true;
+ }
+ /***yolo 鐩存帴娴嬭穼鍊掍唬鐮佹洿鏂� end*/
+
+ featuresAppendOne(detection.feature);
+ // this->features.row(features.rows()) = detection.feature;
+ this->hits += 1;
+ this->time_since_update = 0;
+ if(this->state == TrackState::Tentative && this->hits >= this->_n_init) {
+ this->state = TrackState::Confirmed;
+ }
+ this->isCurrent = true;
+}
+
+bool Track::mark_missed(char* img_time)
+{
+ if(this->init_t)
+ {
+// gettimeofday(&this->start,0);
+ this->start = char_2_unix(img_time);
+ this->init_t = false;
+ }
+// gettimeofday(&this->stop,0);
+ this->stop = char_2_unix(img_time);
+
+// this->time_substract(&this->diff,&this->start,&this->stop);
+ this->diff = this->stop - this->start;
+
+// this->sum_timer = (double)((diff.tv_sec*1000000 + diff.tv_usec)/1000000);
+ this->sum_timer = (double)(this->diff)/1000;
+
+ if(this->state == TrackState::Tentative) {
+ this->state = TrackState::Deleted;
+ return true;
+ } else if(this->time_since_update > this->_max_age) {
+ this->state = TrackState::Deleted;
+ return true;
+ } else if(this->sum_timer > MAX_OUT_TIME)
+ {
+ this->state = TrackState::Deleted;
+ this->_max_age = 0;
+ return true;
+ }
+ this->isCurrent = false;
+ return false;
+}
+
+bool Track::is_confirmed()
+{
+ return this->state == TrackState::Confirmed;
+}
+
+bool Track::is_deleted()
+{
+ return this->state == TrackState::Deleted;
+}
+
+bool Track::is_tentative()
+{
+ return this->state == TrackState::Tentative;
+}
+
+DETECTBOX Track::to_tlwh()
+{
+ DETECTBOX ret = mean.leftCols(4);
+ ret(2) *= ret(3);
+ ret.leftCols(2) -= (ret.rightCols(2)/2);
+ return ret;
+}
+
+DETECTBOX Track::to_xywh()
+{
+ return this->xywh;
+}
+
+void Track::featuresAppendOne(const FEATURE &f)
+{
+ int size = this->features.rows();
+ FEATURESS newfeatures = FEATURESS(size+1, 128);
+ newfeatures.block(0, 0, size, 128) = this->features;
+ newfeatures.row(size) = f;
+ features = newfeatures;
+// printf("--features size::---%d---%d--\n",size,features.rows());
+}
+
+int Track::time_substract(struct timeval *result, struct timeval *begin, struct timeval *end)
+{
+ if(begin->tv_sec > end->tv_sec) return -1;
+ if((begin->tv_sec == end->tv_sec) && (begin->tv_usec > end->tv_usec)) return -2;
+ result->tv_sec = (end->tv_sec - begin->tv_sec);
+ result->tv_usec = (end->tv_usec - begin->tv_usec);
+
+ if(result->tv_usec < 0)
+ {
+ result->tv_sec--;
+ result->tv_usec += 1000000;
+ }
+ return 0;
+
+}
diff --git a/src/tracker_tools/track.h b/src/tracker_tools/track.h
new file mode 100644
index 0000000..5c0fd3d
--- /dev/null
+++ b/src/tracker_tools/track.h
@@ -0,0 +1,121 @@
+#ifndef TRACK_H
+#define TRACK_H
+
+#include "dataType.h"
+
+#include "kalmfilter.h"
+#include "model.h"
+#include "time.h"
+#include "stdio.h"
+#include "stdlib.h"
+#include <iostream>
+#include<sys/time.h>
+#include "../utils/geometry_util.h"
+#include <queue>
+#include "../utils/queue_util.hpp"
+#include "../utils/log_util.h"
+#include "../utils/time_util.h"
+
+using namespace std;
+
+class Track
+{
+
+ enum TrackState {Tentative = 1, Confirmed, Deleted}; // 杞ㄨ抗鐨勭姸鎬侊紝Tentative 鏄垰寮�濮嬬殑鏃跺�欙紝Confirmed鏄‘璁や簡鏄汉鐨勮建杩癸紝Deleted鏄垹闄ょ殑杞ㄨ抗
+
+public:
+ bool isCurrent;
+ DETECTBOX xywh;
+ Track();
+ ~Track();
+ Track(KAL_MEAN& mean, KAL_COVA& covariance, uint64_t track_id,
+ int n_init, int max_age, const FEATURE& feature, char* img_time);
+ void predit(KalmFilter *kf);
+ void update(KalmFilter * const kf, const DETECTION_ROW &detection, char* img_time);
+ bool mark_missed(char* img_time);
+ bool is_confirmed();
+ bool is_deleted();
+ bool is_tentative();
+ DETECTBOX to_tlwh();
+ DETECTBOX to_xywh();
+ int time_since_update; //绱Н妫�娴嬩笉鍒扮殑娆℃暟
+ uint64_t track_id;
+ FEATURESS features;
+ float confidence;
+ KAL_MEAN mean;
+ KAL_COVA covariance;
+
+ int hits;
+ int age;
+ int _n_init;
+ int _max_age;
+ TrackState state;
+ int time_substract(struct timeval *result, struct timeval *begin,struct timeval *end);
+
+public:
+ // 闄勫姞鐨勫叾浠栫畻娉�
+ // 閫楃暀
+// struct timeval last_s_time, last_e_time, last_diff_time; // 閫楃暀鐨勫紑濮嬫椂闂淬�佺粨鏉熸椂闂达紝鍜屾寔缁椂闂�
+ int last_s_time, last_e_time, last_diff_time; // 閫楃暀鐨勫紑濮嬫椂闂淬�佺粨鏉熸椂闂达紝鍜屾寔缁椂闂�
+ int last_time; //鎸佺画閫楃暀鏃堕棿
+ bool isWander;
+
+ //璺屽��
+ float rate; //浜轰綋韬珮鍙樺寲鐜�
+ bool isFall;
+ bool first_Fall;
+ int rateScale; //鍒ゆ柇鏄惁閫愭璺屽��(鐭椂闂村唴)
+ MinQueue<float> rate_queue;
+// struct timeval fall_s_time, fall_e_time, fall_diff_time; // 璺屽�掔殑鎸佺画鏃堕棿 ,瀛楁fall_diff_time浠ｈ〃浜嗕袱涓惈涔夈�傝窛绂绘渶灏忔瘮渚嬬殑鏃堕棿+绱Н璺屽�掓椂闂�
+// struct timeval min_rate_time; //鏈�澶ф瘮渚嬬殑瀹氫綅鏃堕棿
+
+ int fall_s_time, fall_e_time, fall_diff_time; // 璺屽�掔殑鎸佺画鏃堕棿 ,瀛楁fall_diff_time浠ｈ〃浜嗕袱涓惈涔夈�傝窛绂绘渶灏忔瘮渚嬬殑鏃堕棿+绱Н璺屽�掓椂闂�
+ int min_rate_time; //鏈�澶ф瘮渚嬬殑瀹氫綅鏃堕棿
+
+
+ int fall_total_time;
+ float minRate; // 闃熷垪涓殑鏈�澶ф瘮渚�
+ float hisMinRate;
+ SPoint center_point, min_center_point; //閲嶅績鐨勯珮搴�
+ float human_h;
+ float human_w;
+
+
+
+ //濂旇窇
+ bool is_runing_t;
+ bool is_runing_p;
+ bool isRuning;
+ SPoint last_point;
+// struct timeval single_s_time, single_diff_time; // 鍗曞抚鐨勭粨鏉熸椂闂村彲浠ョ敤閫楃暀閲岀殑鏃堕棿浠ｆ浛
+ int single_s_time, single_diff_time; // 鍗曞抚鐨勭粨鏉熸椂闂村彲浠ョ敤閫楃暀閲岀殑鏃堕棿浠ｆ浛
+ int single_time; // 鍗曞抚鎵�鑺遍攢鐨勬椂闀�(ms)
+ queue<float> velocity_queue;
+ float sum_velocity;
+ double run_rate;
+ double last_rate;
+ double rate_area;
+
+ // 甯藉瓙銆佸彛缃┿�佹娊鐑�
+ bool is_hat;
+ int hatScore;
+ int helmetScore;
+ int headScore;
+
+ bool is_mask;
+ int maskScore;
+
+ bool is_smoke;
+ int smokeScore;
+
+private:
+// struct timeval start,stop,diff;
+ int start,stop,diff;
+ double sum_timer;
+ bool init_t;
+ void init_track_time();
+ void featuresAppendOne(const FEATURE& f);
+
+};
+
+#endif // TRACK_H
diff --git a/src/tracker_tools/tracker.cpp b/src/tracker_tools/tracker.cpp
new file mode 100644
index 0000000..79e59ba
--- /dev/null
+++ b/src/tracker_tools/tracker.cpp
@@ -0,0 +1,343 @@
+#include "tracker.h"
+#include "nn_matching.h"
+#include "model.h"
+#include "linear_assignment.h"
+#include <iostream>
+
+using namespace std;
+
+//#define MY_inner_DEBUG
+#ifdef MY_inner_DEBUG
+#include <string>
+#include <iostream>
+#endif
+
+tracker::tracker(
+ float max_cosine_distance, int nn_budget,
+ float max_iou_distance, int max_age, int n_init)
+{
+ this->metric = new NearNeighborDisMetric(
+ NearNeighborDisMetric::METRIC_TYPE::cosine,
+ max_cosine_distance, nn_budget);
+ this->max_iou_distance = max_iou_distance;
+ this->max_age = max_age;
+ this->n_init = n_init;
+
+ this->kf = new KalmFilter();
+ this->tracks.clear();
+ this->_next_idx = 1;
+}
+
+tracker::~tracker()
+{
+ // delete this->metric;
+ // delete this->kf;
+}
+
+void tracker::predict()
+{
+ for(auto& track:tracks)
+ {
+ track->predit(this->kf);
+ }
+}
+
+void tracker::update(std::vector<std::shared_ptr<tracker>> &CAMERAS_VCT, const DETECTIONS &detections, int cam_id, char* img_time)
+{
+ TRACHER_MATCHD res;
+// for (map<uint64_t, std::shared_ptr<Track>>::iterator iter=candidate_track_map.begin();iter!=candidate_track_map.end();iter++)
+// {
+// this->tracks.push_back(iter->second);
+// printf("-------------------this->tracks.size:%d\n",this->tracks.size());
+// }
+
+// std::cout<< "size1:"<< detections.size() << std::endl;
+// if (detections.size()==2){
+// for (DETECTION_ROW det : detections)
+// {
+// std::cout<< det.feature <<std::endl;
+// }
+// exit(0);
+// }
+
+ _match(detections, res);
+
+ vector<MATCH_DATA>& matches = res.matches;
+ //#ifdef MY_inner_DEBUG
+ // printf("res.matches size = %d:\n", matches.size());
+ //#endif
+ for(MATCH_DATA& data:matches) {
+ int track_idx = data.first;
+ int detection_idx = data.second;
+ //#ifdef MY_inner_DEBUG
+ // printf("\t%d == %d;\n", track_idx, detection_idx);
+ //#endif
+ tracks[track_idx]->update(this->kf, detections[detection_idx], img_time);
+// LOG(INFO) << "=======match-track_idx::" << tracks[track_idx]->track_id;
+// DEBUG( "=======match-track_idx::" + to_string(tracks[track_idx]->track_id));
+ }
+
+ vector<int>& unmatched_tracks = res.unmatched_tracks;
+ //#ifdef MY_inner_DEBUG
+ // printf("res.unmatched_tracks size = %d\n", unmatched_tracks.size());
+ //#endif
+ for(int& track_idx:unmatched_tracks)
+ {
+ this->tracks[track_idx]->mark_missed(img_time);
+ }
+ vector<int>& unmatched_detections = res.unmatched_detections;
+ //#ifdef MY_inner_DEBUG
+ // printf("res.unmatched_detections size = %d\n", unmatched_detections.size());
+ //#endif
+ for(int& detection_idx:unmatched_detections) {
+ this->_initiate_track(detections[detection_idx], cam_id, img_time);
+ }
+ //#ifdef MY_inner_DEBUG
+ // int size = tracks.size();
+ // printf("now track size = %d\n", size);
+ //#endif
+ vector<std::shared_ptr<Track>>::iterator it;
+ for(it = tracks.begin(); it != tracks.end();) {
+ if((*it)->is_deleted()) it = tracks.erase(it);
+ else ++it;
+ }
+ //#ifdef MY_inner_DEBUG
+ // printf("update track size = %d\n", tracks.size());
+ //#endif
+
+ /* old version:
+ //update distance metric:
+ FEATURESS features;
+ vector<int> targets;
+ vector<int> active_targets;
+ int pos = 0;
+ for(Track track:tracks) {
+ if(track.is_confirmed() == false) continue;
+ active_targets.push_back(track.track_id);
+ features.row(pos) = track.features;
+ int rows = track.features.rows();
+ pos += rows;
+ for(int i = 0; i < rows; i++) targets.push_back(track.track_id);
+ //attention!!!
+ //track.features.clear();
+ track.features = Eigen::Matrix<float, -1, 128, Eigen::RowMajor>(0,128);
+ }
+ this->metric->partial_fit(features, targets, active_targets);
+ */
+
+ vector<uint64_t> active_targets;
+ vector<TRACKER_DATA> tid_features;
+ for (auto& track:tracks)
+ {
+ if(track->is_confirmed() == false) continue;
+ active_targets.push_back(track->track_id);
+ tid_features.push_back(std::make_pair(track->track_id, track->features));
+ FEATURESS t = FEATURESS(0, 128);
+ track->features = t;
+ }
+
+ this->metric->partial_fit(tid_features, active_targets);
+
+}
+
+void tracker::_match(const DETECTIONS &detections, TRACHER_MATCHD &res)
+{
+ DEBUG("tracker.cpp _match start 147:: ");
+ vector<int> confirmed_tracks;
+ vector<int> unconfirmed_tracks;
+ int idx = 0;
+ for(auto& t:tracks) {
+ if(t->is_confirmed()) confirmed_tracks.push_back(idx);
+ else unconfirmed_tracks.push_back(idx);
+ idx++;
+ }
+
+ TRACHER_MATCHD matcha = linear_assignment::getInstance()->matching_cascade(
+ this, &tracker::gated_matric,
+ this->metric->mating_threshold,
+ this->max_age,
+ this->tracks,
+ detections,
+ confirmed_tracks);
+ vector<int> iou_track_candidates;
+ iou_track_candidates.assign(unconfirmed_tracks.begin(), unconfirmed_tracks.end());
+ vector<int>::iterator it;
+
+ for(it = matcha.unmatched_tracks.begin(); it != matcha.unmatched_tracks.end();) {
+ int idx = *it;
+ if(tracks[idx]->time_since_update == 1) { //push into unconfirmed
+ iou_track_candidates.push_back(idx);
+ it = matcha.unmatched_tracks.erase(it);
+ continue;
+ }
+ ++it;
+ }
+
+ TRACHER_MATCHD matchb = linear_assignment::getInstance()->min_cost_matching(
+ this, &tracker::iou_cost,
+ this->max_iou_distance,
+ this->tracks,
+ detections,
+ iou_track_candidates,
+ matcha.unmatched_detections);
+
+ //get result:
+ res.matches.assign(matcha.matches.begin(), matcha.matches.end());
+ res.matches.insert(res.matches.end(), matchb.matches.begin(), matchb.matches.end());
+ //unmatched_tracks;
+ res.unmatched_tracks.assign(
+ matcha.unmatched_tracks.begin(),
+ matcha.unmatched_tracks.end());
+ res.unmatched_tracks.insert(
+ res.unmatched_tracks.end(),
+ matchb.unmatched_tracks.begin(),
+ matchb.unmatched_tracks.end());
+ res.unmatched_detections.assign(
+ matchb.unmatched_detections.begin(),
+ matchb.unmatched_detections.end());
+ DEBUG("tracker.cpp _match end 207:: ");
+
+}
+
+
+void tracker::_initiate_track(const DETECTION_ROW &detection, int cam_id, char* img_time)
+{
+ KAL_DATA data = kf->initiate(detection.to_xyah());
+ KAL_MEAN mean = data.first;
+ KAL_COVA covariance = data.second;
+ uint64_t n_track_id;
+ stringstream strValue;
+ strValue << random_int(8);
+ strValue >> n_track_id;
+ // n_track_id = m_staticStruct::human_ids++;
+ auto t = std::make_shared<Track>(mean, covariance, n_track_id, this->n_init, this->max_age, detection.feature, img_time);
+ t->xywh = detection.tlwh;
+ this->tracks.push_back(t);
+
+// auto t = std::make_shared<Track>(mean, covariance, this->_next_idx, this->n_init,this->max_age, detection.feature);
+// this->tracks.push_back(t);
+//
+// _next_idx += 1;
+}
+
+
+DYNAMICM tracker::gated_matric(
+ std::vector<std::shared_ptr<Track>> &tracks,
+ const DETECTIONS &dets,
+ const std::vector<int>& track_indices,
+ const std::vector<int>& detection_indices)
+{
+ // FEATURESS features(detection_indices.size(), 128);
+ // int pos = 0;
+ // for(int i:detection_indices) {
+ // features.row(pos++) = dets[i].feature;
+ // }
+ vector<uint64_t> targets;
+ for(int i:track_indices) {
+ targets.push_back(tracks[i]->track_id);
+ }
+ // DYNAMICM cost_matrix = this->metric->distance(features, targets);
+ DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(targets.size(), detection_indices.size());
+ DYNAMICM res = linear_assignment::getInstance()->gate_cost_matrix(
+ this->kf, cost_matrix, tracks, dets, track_indices,
+ detection_indices);
+ return res;
+}
+
+DYNAMICM
+tracker::cos_matric(
+ std::vector<std::shared_ptr<Track> > &tracks,
+ const DETECTIONS &dets, const std::vector<int>
+ &track_indices, const std::vector<int>
+ &detection_indices)
+{
+ FEATURESS features(detection_indices.size(), 128);
+ int pos = 0;
+ for(int i : detection_indices) {
+ features.row(pos++) = dets[i].feature;
+ }
+ std::map<uint64_t, FEATURESS> simples;
+ for(auto i : track_indices)
+ {
+ simples[tracks[i]->track_id] = tracks[i]->features;
+ }
+
+ DYNAMICM res = this->metric->cos_distance(features, simples);
+
+ return res;
+}
+
+
+DYNAMICM
+tracker::iou_cost(
+ std::vector<std::shared_ptr<Track>> &tracks,
+ const DETECTIONS &dets,
+ const std::vector<int>& track_indices,
+ const std::vector<int>& detection_indices)
+{
+ //!!!python diff: track_indices && detection_indices will never be None.
+ // if(track_indices.empty() == true) {
+ // for(size_t i = 0; i < tracks.size(); i++) {
+ // track_indices.push_back(i);
+ // }
+ // }
+ // if(detection_indices.empty() == true) {
+ // for(size_t i = 0; i < dets.size(); i++) {
+ // detection_indices.push_back(i);
+ // }
+ // }
+ int rows = track_indices.size();
+ int cols = detection_indices.size();
+ DYNAMICM cost_matrix = Eigen::MatrixXf::Zero(rows, cols);
+ for(int i = 0; i < rows; i++) {
+ int track_idx = track_indices[i];
+ if(tracks[track_idx]->time_since_update > 1) {
+ cost_matrix.row(i) = Eigen::RowVectorXf::Constant(cols, INFTY_COST);
+ continue;
+ }
+ DETECTBOX bbox = tracks[track_idx]->to_tlwh();
+ int csize = detection_indices.size();
+ DETECTBOXSS candidates(csize, 4);
+ for(int k = 0; k < csize; k++) candidates.row(k) = dets[detection_indices[k]].tlwh;
+ Eigen::RowVectorXf rowV = (1. - iou(bbox, candidates).array()).matrix().transpose();
+ cost_matrix.row(i) = rowV;
+ }
+ return cost_matrix;
+}
+
+
+Eigen::VectorXf
+tracker::iou(DETECTBOX& bbox, DETECTBOXSS& candidates)
+{
+ float bbox_tl_1 = bbox[0];
+ float bbox_tl_2 = bbox[1];
+ float bbox_br_1 = bbox[0] + bbox[2];
+ float bbox_br_2 = bbox[1] + bbox[3];
+ float area_bbox = bbox[2] * bbox[3];
+
+ Eigen::Matrix<float, -1, 2> candidates_tl;
+ Eigen::Matrix<float, -1, 2> candidates_br;
+ candidates_tl = candidates.leftCols(2) ;
+ candidates_br = candidates.rightCols(2) + candidates_tl;
+
+ int size = int(candidates.rows());
+ // Eigen::VectorXf area_intersection(size);
+ // Eigen::VectorXf area_candidates(size);
+ Eigen::VectorXf res(size);
+ for(int i = 0; i < size; i++) {
+ float tl_1 = std::max(bbox_tl_1, candidates_tl(i, 0));
+ float tl_2 = std::max(bbox_tl_2, candidates_tl(i, 1));
+ float br_1 = std::min(bbox_br_1, candidates_br(i, 0));
+ float br_2 = std::min(bbox_br_2, candidates_br(i, 1));
+
+ float w = br_1 - tl_1; w = (w < 0? 0: w);
+ float h = br_2 - tl_2; h = (h < 0? 0: h);
+ float area_intersection = w * h;
+ float area_candidates = candidates(i, 2) * candidates(i, 3);
+ res[i] = area_intersection/(area_bbox + area_candidates - area_intersection);
+ }
+ //#ifdef MY_inner_DEBUG
+ // std::cout << res << std::endl;
+ //#endif
+ return res;
+}
diff --git a/src/tracker_tools/tracker.h b/src/tracker_tools/tracker.h
new file mode 100644
index 0000000..16a252e
--- /dev/null
+++ b/src/tracker_tools/tracker.h
@@ -0,0 +1,77 @@
+#ifndef TRACKER_H
+#define TRACKER_H
+#include <vector>
+
+#include "kalmfilter.h"
+#include "track.h"
+#include "model.h"
+// #include "../utils/config_util.h"
+#include "feature_util.h"
+#include "../utils/geometry_util.h"
+#include "../std_target.h"
+
+typedef int (*fn)(void* args, const int sender_chan, const char *recver, const char *data, const int len);
+
+class NearNeighborDisMetric;
+
+using CURRENT_ID_REG = std::pair<uint64_t, std::vector<bool>>;
+
+class tracker
+{
+public:
+ NearNeighborDisMetric* metric;
+ float max_iou_distance;
+ int max_age;
+ int n_init;
+
+ KalmFilter* kf;
+
+ uint64_t _next_idx;
+
+private:
+ // bool P_in_Polygon(SPoint p, IntersectReg polygon);
+// std::vector<uint64_t> candidate_track_ids; // 鍊欓�夊垪琛紝涔熷氨鏄柊鍔犲叆鐨勪汉鍛樹俊鎭�:id銆� track淇℃伅锛堢壒寰佷俊鎭�� 鍧愭爣淇℃伅锛�
+// std::vector<uint64_t> current_track_ids; // 璺熻釜鐨刬d 淇℃伅
+
+public:
+ std::vector<std::shared_ptr<Track>> tracks;
+ tracker( /*NearNeighborDisMetric* metric,*/
+ float max_cosine_distance, int nn_budget,
+ float max_iou_distance = MAX_IOU_DISTANCE,
+ int max_age = MAX_AGE, int n_init = N_INIT);
+
+ ~tracker();
+ void predict();
+ void update(std::vector<std::shared_ptr<tracker>> &CAMERAS_VCT, const DETECTIONS& detections, int cam_id, char* img_time);
+
+ typedef DYNAMICM (tracker::* GATED_METRIC_FUNC)(
+ std::vector<std::shared_ptr<Track>>& tracks,
+ const DETECTIONS& dets,
+ const std::vector<int>& track_indices,
+ const std::vector<int>& detection_indices);
+private:
+ void _match(const DETECTIONS& detections, TRACHER_MATCHD& res);
+ void _initiate_track(const DETECTION_ROW& detection, int cam_id, char* img_time);
+public:
+ DYNAMICM gated_matric(
+ std::vector<std::shared_ptr<Track>>& tracks,
+ const DETECTIONS& dets,
+ const std::vector<int>& track_indices,
+ const std::vector<int>& detection_indices);
+
+ DYNAMICM cos_matric(
+ std::vector<std::shared_ptr<Track>>& tracks,
+ const DETECTIONS& dets,
+ const std::vector<int>& track_indices,
+ const std::vector<int>& detection_indices);
+
+ DYNAMICM iou_cost(
+ std::vector<std::shared_ptr<Track>>& tracks,
+ const DETECTIONS& dets,
+ const std::vector<int>& track_indices,
+ const std::vector<int>& detection_indices);
+ Eigen::VectorXf iou(DETECTBOX& bbox,
+ DETECTBOXSS &candidates);
+};
+
+#endif // TRACKER_H
diff --git a/src/utils/draw_util.cpp b/src/utils/draw_util.cpp
index 5e2c5a7..4da8fcf 100644
--- a/src/utils/draw_util.cpp
+++ b/src/utils/draw_util.cpp
@@ -70,26 +70,26 @@
cv::imwrite(file_path + name + "_" + to_string(type) + ".jpg", aaaa);
}
-void draw_SDK_result(cv::Mat mat_img, Target& target)
+void draw_SDK_result(cv::Mat mat_img, Target* target)
{
- cv::Rect tmp_rect = cv::Rect(target.rect.left,target.rect.top,target.rect.right-target.rect.left,target.rect.bottom-target.rect.top);
+ cv::Rect tmp_rect = cv::Rect(target->rect.left,target->rect.top,target->rect.right-target->rect.left,target->rect.bottom-target->rect.top);
cv::rectangle(mat_img, tmp_rect , cv::Scalar(50, 200, 50), 2);
// cv::resize(mat_img, mat_img, frameSize);
// cv::imwrite("1111.jpg", mat_img);
cv::imshow(to_string(1), mat_img);
- cv::waitKey(0);
+ cv::waitKey(1);
}
cv::VideoWriter writer("./01_baseDetector.avi", cv::VideoWriter::fourcc('M','J','P','G'),24, cv::Size(800,500), true);
// cv::VideoWriter writer("/opt/vasystem/valog/01_Scheanve/01_baseDetector.avi", cv::VideoWriter::fourcc('M','J','P','G'),24, cv::Size(800,500), true);
-void draw_SDK_result(const int cam_id, cv::Mat mat_img, TResult& t_result)
+void draw_SDK_result(const int cam_id, cv::Mat mat_img, TResult* t_result)
{
cv::Rect tmp_rect;
- for (int i = 0; i < t_result.count; ++i)
+ for (int i = 0; i < t_result->count; ++i)
{
- auto &result = t_result.targets[i];
+ auto &result = t_result->targets[i];
tmp_rect = cv::Rect(result.rect.left,result.rect.top,result.rect.right-result.rect.left,result.rect.bottom-result.rect.top);
cv::rectangle(mat_img, tmp_rect , cv::Scalar(50, 200, 50), 2);
// cv::putText(mat_img, std::to_string(result.id), Point((result.rect.left+result.rect.right)/2,result.rect.top+150), CV_FONT_HERSHEY_SIMPLEX, 0.8, Scalar(255,255,0), 2);
diff --git a/src/utils/draw_util.h b/src/utils/draw_util.h
index 8a916b1..fac3353 100644
--- a/src/utils/draw_util.h
+++ b/src/utils/draw_util.h
@@ -11,8 +11,8 @@
void create_foldert(const char *path);
-void draw_SDK_result(cv::Mat mat_img, Target& target);
-void draw_SDK_result(const int cam_id, cv::Mat mat_img, TResult& t_result);
+void draw_SDK_result(cv::Mat mat_img, Target* target);
+void draw_SDK_result(const int cam_id, cv::Mat mat_img, TResult* t_result);
diff --git a/src/utils/geometry_util.cpp b/src/utils/geometry_util.cpp
new file mode 100644
index 0000000..8371ce9
--- /dev/null
+++ b/src/utils/geometry_util.cpp
@@ -0,0 +1,445 @@
+//
+// Created by Scheaven on 2020/5/31.
+//
+
+#include "geometry_util.h"
+using namespace std;
+
+
+SPoint::_Point() {}
+SPoint::_Point(float x, float y)
+{
+ this->x = x;
+ this->y = y;
+ this->z = 0;
+}
+
+SPoint::_Point(double a, double b, double c)
+{
+ x = a; y = b; z = c;
+}
+
+float calDistance(SPoint A, SPoint B)
+{
+ float x_d, y_d;
+ x_d = A.x - B.x;
+ y_d = A.y - B.y;
+ float len = sqrt(x_d * x_d + 4*y_d * y_d);
+ return len;
+}
+
+//bool Pt_in_Polygon(SPoint point, RegInfo* polygon)
+//{
+// int nCross = 0;
+// for (int i = 0; i < polygon->count; ++i)
+// {
+// SPoint p1 = polygon[i];
+// SPoint p2 = polygon[(i+1)%polygon->count];
+//
+// if ( p1.y == p2.y )
+// continue;
+// if ( p.y < min(p1.y, p2.y) )
+// continue;
+// if ( p.y >= max(p1.y, p2.y) )
+// continue;
+//
+// float x = (float)(p.y - p1.y) * (float)(p2.x - p1.x) / (float)(p2.y - p1.y) + p1.x;
+//
+// if ( x > p.x )
+// {
+// nCross++;
+// }
+//
+// }
+//
+// if ((nCross % 2) == 1)
+// {
+// return true;
+// }
+// else
+// {
+// return false;
+// }
+
+//}
+
+
+float computeArea(const SPoint * pt,int n ) {
+ float area0 = 0.f;
+ for (int i = 0 ; i < n ; i++ )
+ {
+ int j = (i+1)%n;
+ area0 += pt[i].x * pt[j].y;
+ area0 -= pt[i].y * pt[j].x;
+ }
+ return 0.5f * fabs(area0);
+}
+
+struct PointAngle{
+ SPoint p;
+ float angle;
+};
+
+float Polygon::area() const {
+ return computeArea(pt,n);
+}
+
+static inline int cross(const SPoint* a,const SPoint* b) {
+ return a->x * b->y - a->y * b->x;
+}
+
+static inline SPoint* vsub(const SPoint* a,const SPoint* b, SPoint* res) {
+ res->x = a->x - b->x;
+ res->y = a->y - b->y;
+ return res;
+}
+
+
+static int line_sect(const SPoint* x0,const SPoint* x1,const SPoint* y0,const SPoint* y1, SPoint* res) {
+ SPoint dx, dy, d;
+ vsub(x1, x0, &dx);
+ vsub(y1, y0, &dy);
+ vsub(x0, y0, &d);
+ float dyx = (float)cross(&dy, &dx);
+ if (!dyx) return 0;
+ dyx = cross(&d, &dx) / dyx;
+ if (dyx <= 0 || dyx >= 1) return 0;
+ res->x = int(y0->x + dyx * dy.x);
+ res->y = int(y0->y + dyx * dy.y);
+ return 1;
+}
+
+static int left_of(const SPoint* a,const SPoint* b,const SPoint* c) {
+ SPoint tmp1, tmp2;
+ int x;
+ vsub(b, a, &tmp1);
+ vsub(c, b, &tmp2);
+ x = cross(&tmp1, &tmp2);
+ return x < 0 ? -1 : x > 0;
+}
+
+static void poly_edge_clip(const Polygon* sub,const SPoint* x0,const SPoint* x1, int left, Polygon* res) {
+ int i, side0, side1;
+ SPoint tmp;
+ const SPoint* v0 = sub->pt+ sub->n - 1;
+ const SPoint* v1;
+ res->clear();
+
+ side0 = left_of(x0, x1, v0);
+ if (side0 != -left) res->add(*v0);
+
+ for (i = 0; i < sub->n; i++) {
+ v1 = sub->pt + i;
+ side1 = left_of(x0, x1, v1);
+ if (side0 + side1 == 0 && side0)
+ /* last point and current straddle the edge */
+ if (line_sect(x0, x1, v0, v1, &tmp))
+ res->add(tmp);
+ if (i == sub->n - 1) break;
+ if (side1 != -left) res->add(*v1);
+ v0 = v1;
+ side0 = side1;
+ }
+}
+
+static int poly_winding(const Polygon* p) {
+ return left_of(p->pt, p->pt + 1, p->pt + 2);
+}
+
+
+void intersectPolygonSHPC(const Polygon * sub, const Polygon* clip, Polygon* res)
+{
+ int i;
+ Polygon P1,P2;
+ Polygon *p1 = &P1;
+ Polygon *p2 = &P2;
+ Polygon *tmp;
+
+ int dir = poly_winding(clip);
+ poly_edge_clip(sub, clip->pt + clip->n - 1, clip->pt, dir, p2);
+ for (i = 0; i < clip->n - 1; i++) {
+ tmp = p2; p2 = p1; p1 = tmp;
+ if(p1->n == 0) {
+ p2->n = 0;
+ break;
+ }
+ poly_edge_clip(p1, clip->pt + i, clip->pt + i + 1, dir, p2);
+ }
+ res->clear();
+ for (i = 0 ; i < p2->n ; i++) res->add(p2->pt[i]);
+}
+
+void intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Polygon& res) {
+ intersectPolygonSHPC(&sub, &clip, &res);
+}
+
+Line::Line(SPoint a, SPoint b, bool _is_seg)
+{
+ s = a; e = b; is_seg = _is_seg;
+}
+
+Line::Line() {}
+
+
+bool PointCmp(const SPoint &a,const SPoint &b,const SPoint ¢er)
+{
+ if (a.x >= 0 && b.x < 0)
+ return true;
+ if (a.x == 0 && b.x == 0)
+ return a.y > b.y;
+ int det = (a.x - center.x) * (b.y - center.y) - (b.x - center.x) * (a.y - center.y);
+ if (det < 0)
+ return true;
+ if (det > 0)
+ return false;
+ int d1 = (a.x - center.x) * (a.x - center.x) + (a.y - center.y) * (a.y - center.y);
+ int d2 = (b.x - center.x) * (b.x - center.y) + (b.y - center.y) * (b.y - center.y);
+ return d1 > d2;
+}
+
+SPoint ClockwiseSortPoints(std::vector<SPoint> &vPoints)
+{
+ SPoint center;
+ double x = 0,y = 0;
+ for (int i = 0;i < vPoints.size();i++)
+ {
+ x += vPoints[i].x;
+ y += vPoints[i].y;
+ }
+ center.x = (int)x/vPoints.size();
+ center.y = (int)y/vPoints.size();
+
+ for(int i = 0;i < vPoints.size() - 1;i++)
+ {
+ for (int j = 0;j < vPoints.size() - i - 1;j++)
+ {
+ if (PointCmp(vPoints[j],vPoints[j+1],center))
+ {
+ SPoint tmp = vPoints[j];
+ vPoints[j] = vPoints[j + 1];
+ vPoints[j + 1] = tmp;
+ }
+ }
+ }
+ return center;
+}
+
+SPoint add(const SPoint& lhs, const SPoint& rhs)
+{
+ SPoint res;
+
+ res.x = lhs.x + rhs.x;
+ res.y = lhs.y + rhs.y;
+ res.z = lhs.z + rhs.z;
+
+ return res;
+}
+SPoint sub(const SPoint& lhs, const SPoint& rhs)
+{
+ SPoint res;
+
+ res.x = lhs.x - rhs.x;
+ res.y = lhs.y - rhs.y;
+ res.z = lhs.z - rhs.z;
+
+ return res;
+}
+SPoint mul(const SPoint& p, double ratio)
+{
+ SPoint res;
+
+ res.x = p.x * ratio;
+ res.y = p.y * ratio;
+ res.z = p.z * ratio;
+
+ return res;
+}
+SPoint div(const SPoint& p, double ratio)
+{
+ SPoint res;
+
+ res.x = p.x / ratio;
+ res.y = p.y / ratio;
+ res.z = p.z / ratio;
+
+ return res;
+}
+bool equal(const SPoint& lhs, const SPoint& rhs)
+{
+ return(lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z);
+}
+
+double length(const SPoint& vec)
+{
+ return (sqrt(pow(vec.x, 2) + pow(vec.y, 2) + pow(vec.z, 2)));
+}
+
+SPoint normalize(const SPoint& vec)
+{
+ SPoint res;
+
+ res = div(vec, length(vec));
+
+ return res;
+}
+
+double dotMultiply(const SPoint& op, const SPoint& p1, const SPoint& p2)
+{
+ return ((p1.x - op.x) * (p2.x - op.x) + (p1.y - op.y) * (p2.y - op.y) + (p1.z - op.z) * (p2.z - op.z));
+}
+double dotMultiply(const SPoint& vec1, const SPoint& vec2)
+{
+ return(vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z);
+}
+
+SPoint multiply(const SPoint& op, const SPoint& p1, const SPoint& p2)
+{
+ SPoint result;
+
+ result.x = (p1.y - op.y) * (p2.z - op.z) - (p2.y - op.y) * (p1.z - op.z);
+ result.y = (p1.z - op.z) * (p2.x - op.x) - (p2.z - op.z) * (p1.x - op.x);
+ result.z = (p1.x - op.x) * (p2.y - op.y) - (p2.x - op.x) * (p1.y - op.y);
+
+ return result;
+}
+
+SPoint multiply(const SPoint& vec1, const SPoint& vec2)
+{
+ SPoint result;
+
+ result.x = vec1.y * vec2.z - vec2.y * vec1.z;
+ result.y = vec1.z * vec2.x - vec2.z * vec1.x;
+ result.z = vec1.x * vec2.y - vec2.x * vec1.y;
+
+ return result;
+}
+
+bool isponl(const SPoint& p, const Line& l)
+{
+ SPoint line_vec = sub(l.e, l.s);
+ SPoint point_vec1 = sub(p, l.s);
+ SPoint point_vec2 = sub(p, l.e);
+
+ SPoint mul_vec = multiply(line_vec, point_vec1);
+ double dot = dotMultiply(point_vec1, point_vec2);
+ if (l.is_seg)
+ {
+ if (equal(p,l.s) || equal(p,l.e))
+ return true;
+ return (0.0 == length(mul_vec) && dot < 0.0);
+
+ }
+ else
+ {
+ return (0.0 == length(mul_vec));
+ }
+}
+
+double Cos(const SPoint& vec1, const SPoint& vec2)
+{
+ SPoint unit_vec1 = normalize(vec1);
+ SPoint unit_vec2 = normalize(vec2);
+
+ return dotMultiply(unit_vec1, unit_vec2);
+}
+
+double Cos(const SPoint& op, const SPoint& p1, const SPoint& p2)
+{
+ SPoint vec1 = sub(p1, op);
+ SPoint vec2 = sub(p2, op);
+
+ return Cos(vec1, vec2);
+}
+
+
+bool isSegIntersect(const Line& l1, const Line& l2, SPoint& inter_p)
+{
+ SPoint line1 = sub(l1.e, l1.s);
+ SPoint line2 = sub(l2.e, l2.s);
+ SPoint norm1 = normalize(line1);
+ SPoint norm2 = normalize(line2);
+ if (l1.is_seg)
+ {
+ if (isponl(l1.s, l2))
+ {
+ inter_p = l1.s;
+ return true;
+ }
+ if (isponl(l1.e, l2))
+ {
+ inter_p = l1.e;
+ return true;
+ }
+ if (isponl(l2.s, l1))
+ {
+ inter_p = l2.s;
+ return true;
+ }
+ if (isponl(l2.e, l1))
+ {
+ inter_p = l2.e;
+ return true;
+ }
+ double dot1 = dotMultiply(multiply(sub(l2.s, l1.s), line1), multiply(sub(l2.e, l1.s), line1));
+ double dot2 = dotMultiply(multiply(sub(l1.s, l2.s), line2), multiply(sub(l1.e, l2.s), line2));
+ if (dot1 < 0.0 && dot2 < 0.0)
+ {
+ double t1 = length(multiply(sub(l1.s, l2.s), norm2)) / length(multiply(norm2, norm1));
+ double t2 = length(multiply(sub(l2.s, l1.s), norm1)) / length(multiply(norm1, norm2));
+
+ inter_p = add(l1.s, mul(norm1, t1));
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+
+ }
+ else
+ {
+ if (Cos(line1, line2) == 1.0)
+ return false;
+
+ double t1 = length(multiply(sub(l1.s, l2.s), norm2)) / length(multiply(norm2, norm1));
+ double t2 = length(multiply(sub(l2.s, l1.s), norm1)) / length(multiply(norm1, norm2));
+
+ inter_p = add(l1.s, mul(norm1, t1));
+ return true;
+ }
+}
+
+// bool p_inPolygon(Point p, vector<Point> point_vec)
+// {
+// int nCross = 0;
+// for (int i = 0; i < point_vec.size(); ++i)
+// {
+// Point p1(point_vec[i].x, point_vec[i].y);
+// Point p2(point_vec[(i+1)%point_vec.size()].x, point_vec[(i+1)%point_vec.size()].y);
+
+// if ( p1.y == p2.y )
+// continue;
+// if ( p.y < min(p1.y, p2.y) )
+// continue;
+// if ( p.y >= max(p1.y, p2.y) )
+// continue;
+
+// float x = (float)(p.y - p1.y) * (float)(p2.x - p1.x) / (float)(p2.y - p1.y) + p1.x;
+
+// if ( x > p.x )
+// {
+// nCross++;
+// }
+
+// }
+
+// if ((nCross % 2) == 1)
+// {
+// return true;
+// }
+// else
+// {
+// return false;
+// }
+// }
+
diff --git a/src/utils/geometry_util.h b/src/utils/geometry_util.h
new file mode 100644
index 0000000..b9ba09c
--- /dev/null
+++ b/src/utils/geometry_util.h
@@ -0,0 +1,129 @@
+//
+// Created by Scheaven on 2020/5/31.
+//
+#ifndef GEOMETRY_UTIL_H
+#define GEOMETRY_UTIL_H
+
+#include <vector>
+#include <map>
+#include <string>
+#include <list>
+#include <fstream>
+#include <stdint.h>
+#include "stdio.h"
+#include <stdlib.h>
+#include <math.h>
+
+#include <iostream>
+#include <cmath>
+#include <cstring>
+#include "assert.h"
+
+const double PI = 3.14159265;
+
+typedef struct _Point
+{
+public:
+ float x;
+ float y;
+ float z;
+
+public:
+ _Point(float x, float y);
+ _Point();
+ _Point(double a , double b , double c );
+}SPoint;
+
+// 澶氳竟褰㈠尯鍩熺粨鏋�
+typedef struct _PInfo
+{
+ SPoint* point;
+ int count;
+}RegInfo;
+
+float calDistance(SPoint A, SPoint B);
+
+
+bool Pt_in_Polygon(SPoint point, RegInfo* polygon);
+
+#define MAX_POINT_POLYGON 64
+
+struct Polygon {
+ SPoint pt[MAX_POINT_POLYGON];
+ int n;
+
+ Polygon(int n_ = 0 ) { assert(n_>= 0 && n_ < MAX_POINT_POLYGON); n = n_;}
+ virtual ~Polygon() {}
+
+ void clear() { n = 0; }
+ void add(const SPoint &p) {if(n < MAX_POINT_POLYGON) pt[n++] = p;}
+ void push_back(const SPoint &p) {add(p);}
+ int size() const { return n;}
+ SPoint getCenter() const ;
+ const SPoint & operator[] (int index) const { assert(index >= 0 && index < n); return pt[index];}
+ SPoint& operator[] (int index) { assert(index >= 0 && index < n); return pt[index]; }
+ void pointsOrdered() ;
+ float area() const ;
+ bool pointIsInPolygon(SPoint p) const ;
+};
+
+
+void intersectPolygonSHPC(const Polygon * sub,const Polygon* clip,Polygon* res) ;
+void intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Polygon& res) ;
+
+
+//struct Point
+//{
+// double x; // x鍧愭爣
+// double y; // y鍧愭爣
+// double z; // z鍧愭爣锛堥粯璁や负0锛屽鏋滈渶瑕佷笁缁寸偣鍒欑粰z璧嬪�硷級
+//
+// Point(double a , double b , double c = 0); // 鏋勯�犲嚱鏁�
+// Point(); // 鏋勯�犲嚱鏁�
+//};
+
+struct Line
+{
+ SPoint s;
+ SPoint e;
+ bool is_seg;
+
+ Line();
+ Line(SPoint a, SPoint b, bool _is_seg = true);
+};
+
+bool PointCmp(const SPoint &a,const SPoint &b,const SPoint ¢er);
+
+SPoint ClockwiseSortPoints(std::vector<SPoint> &vPoints); // 澶氳竟褰㈢殑鐐规寜鐓ч�嗘椂閽堟帓搴�
+
+SPoint add(const SPoint& lhs, const SPoint& rhs);
+
+SPoint sub(const SPoint& lhs, const SPoint& rhs);
+
+SPoint mul(const SPoint& p, double ratio);
+
+SPoint div(const SPoint& p, double ratio);
+
+bool equal(const SPoint& lhs, const SPoint& rhs);
+
+double length(const SPoint& vec);
+
+SPoint normalize(const SPoint& vec);
+
+double dotMultiply(const SPoint& op, const SPoint& p1, const SPoint& p2);
+
+double dotMultiply(const SPoint& vec1, const SPoint& vec2);
+
+SPoint multiply(const SPoint& op, const SPoint& p1, const SPoint& p2);
+
+SPoint multiply(const SPoint& vec1, const SPoint& vec2);
+
+bool isponl(const SPoint& p, const Line& l);
+
+double Cos(const SPoint& vec1, const SPoint& vec2);
+
+double Cos(const SPoint& op, const SPoint& p1, const SPoint& p2);
+
+bool isSegIntersect(const Line& l1, const Line& l2, SPoint& inter_p);
+
+#endif //GEOMETRY_UTIL_H
diff --git a/src/utils/queue_util.hpp b/src/utils/queue_util.hpp
new file mode 100644
index 0000000..5dde819
--- /dev/null
+++ b/src/utils/queue_util.hpp
@@ -0,0 +1,168 @@
+//
+// Created by Scheaven on 2020/6/8.
+//
+
+#ifndef _QUEUE_UTIL_H
+#define _QUEUE_UTIL_H
+
+#include "stack_util.hpp"
+
+using namespace std;
+
+template <typename T>
+class MaxQueue
+{
+private:
+ MaxStack<T> stackA;
+ MaxStack<T> stackB;
+
+public:
+ MaxQueue(){};
+ ~MaxQueue(){};
+
+ void push(T ele)
+ {
+ stackA.push(ele);
+ };
+
+ void pop()
+ {
+ if(stackB.isEmpty())
+ {
+// if(stackA.isEmpty())
+// {
+// return ;
+// }
+ while (!stackA.isEmpty())
+ {
+ stackB.push(stackA.top());
+ stackA.pop();
+ }
+ }
+
+ stackB.pop();
+ };
+
+
+ T front()
+ {
+ if(stackB.isEmpty())
+ {
+// if(stackA.isEmpty())
+// {
+// return ;
+// }
+ while (!stackA.isEmpty())
+ {
+ stackB.push(stackA.top());
+ stackA.pop();
+ }
+ }
+ return stackB.top();
+ };
+
+ T max()
+ {
+ if(!stackA.isEmpty()&&!stackB.isEmpty())
+ {
+ return stackA.maxItem()>stackB.maxItem()?stackA.maxItem():stackB.maxItem();
+ } else if(!stackA.isEmpty()&&stackB.isEmpty())
+ {
+ return stackA.maxItem();
+ } else
+ {
+ return stackB.maxItem();
+ }
+ };
+
+ int size()
+ {
+ return stackA.size()+stackB.size();
+ }
+
+ bool isEmpty()
+ {
+ return stackA.isEmpty()&&stackB.isEmpty();
+ };
+
+};
+
+template <typename T>
+class MinQueue
+{
+private:
+ MinStack<T> stackA;
+ MinStack<T> stackB;
+
+public:
+ MinQueue(){};
+ ~MinQueue(){};
+
+ void push(T ele)
+ {
+ stackA.push(ele);
+ };
+
+ void pop()
+ {
+ if(stackB.isEmpty())
+ {
+// if(stackA.isEmpty())
+// {
+// return ;
+// }
+ while (!stackA.isEmpty())
+ {
+ stackB.push(stackA.top());
+ stackA.pop();
+ }
+ }
+
+ stackB.pop();
+ };
+
+
+ T front()
+ {
+ if(stackB.isEmpty())
+ {
+// if(stackA.isEmpty())
+// {
+// return ;
+// }
+ while (!stackA.isEmpty())
+ {
+ stackB.push(stackA.top());
+ stackA.pop();
+ }
+ }
+ return stackB.top();
+ };
+
+ T min()
+ {
+ if(!stackA.isEmpty()&&!stackB.isEmpty())
+ {
+ return stackA.minItem()<stackB.minItem()?stackA.minItem():stackB.minItem();
+ } else if(!stackA.isEmpty()&&stackB.isEmpty())
+ {
+ return stackA.minItem();
+ } else
+ {
+ return stackB.minItem();
+ }
+ };
+
+ int size()
+ {
+ return stackA.size()+stackB.size();
+ }
+
+ bool isEmpty()
+ {
+ return stackA.isEmpty()&&stackB.isEmpty();
+ };
+
+};
+
+#endif //INC_05_MAX_STACK_QUEUE_QUEUE_UTIL_H
diff --git a/src/utils/stack_util.hpp b/src/utils/stack_util.hpp
new file mode 100644
index 0000000..598b563
--- /dev/null
+++ b/src/utils/stack_util.hpp
@@ -0,0 +1,127 @@
+//
+// Created by Scheaven on 2020/6/8.
+//
+
+#ifndef _STACK_UTIL_H
+#define _STACK_UTIL_H
+
+#include <algorithm>
+#include <memory.h>
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <iomanip>
+#include <stdlib.h>
+#include <vector>
+#include <stack>
+#include <cstdio>
+
+using namespace std;
+
+
+template <typename T>
+class MaxStack
+{
+private:
+ stack<T> max_stack;
+ stack<T> _data;
+
+public:
+ MaxStack(){};
+ ~MaxStack(){};
+
+
+ void push(T ele)
+ {
+ _data.push(ele);
+ if(max_stack.empty() || ele>=max_stack.top())
+ {
+ max_stack.push(ele);
+ }
+ };
+
+ void pop()
+ {
+ T tem = _data.top();
+ _data.pop();
+ if(tem==max_stack.top())
+ {
+ max_stack.pop();
+ }
+ };
+
+ T top()
+ {
+ return _data.top();
+
+ };
+
+ bool isEmpty()
+ {
+ return _data.empty();
+ };
+
+ T maxItem()
+ {
+ return max_stack.top();
+ };
+
+ int size()
+ {
+ return _data.size();
+ };
+};
+
+
+template <typename T>
+class MinStack
+{
+private:
+ stack<T> min_stack;
+ stack<T> _data;
+
+public:
+ MinStack(){};
+ ~MinStack(){};
+
+ void push(T ele)
+ {
+ _data.push(ele);
+ if(min_stack.empty() || ele<=min_stack.top())
+ {
+ min_stack.push(ele);
+ }
+ };
+
+ void pop(){
+ T tem = _data.top();
+ _data.pop();
+ if(tem==min_stack.top())
+ {
+ min_stack.pop();
+ }
+ };
+
+ T top()
+ {
+ return _data.top();
+
+ };
+
+ bool isEmpty()
+ {
+ return _data.empty();
+ };
+
+ T minItem()
+ {
+ return min_stack.top();
+ };
+
+ int size()
+ {
+ return _data.size();
+ };
+};
+
+#endif //INC_05_MAX_STACK_QUEUE_STACK_UTIL_H
diff --git a/src/utils/timer_utils.hpp b/src/utils/timer_utils.hpp
new file mode 100644
index 0000000..dafccbb
--- /dev/null
+++ b/src/utils/timer_utils.hpp
@@ -0,0 +1,38 @@
+#pragma once
+
+#include <istream>
+#include <string>
+#include <chrono>
+#include "log_util.h"
+#include <boost/format.hpp>
+
+class Timer
+{
+ public:
+ Timer():beg_(clock_::now()){}
+ void reset()
+ {
+ beg_ = clock_::now();
+ }
+ double elapsed() const
+ {
+ return std::chrono::duration_cast<second_>(clock_::now() - beg_).count();
+ }
+ void out(std::string message="")
+ {
+ double t = elapsed();
+ // std::cout << message << " elasped time:" << t << "ms" << std::endl;
+ DEBUG((boost::format("%e lasped time:%f ms")%message %t).str());
+ reset();
+ }
+ // DEBUG((boost::format("nhao%d")%1).str());
+
+ double get_duration() const
+ {
+ return elapsed();
+ }
+ private:
+ using clock_ = std::chrono::high_resolution_clock;
+ using second_=std::chrono::duration<double,std::milli>;
+ std::chrono::time_point<clock_>beg_;
+};
--
Gitblit v1.8.0