diff --git a/CMakeLists.txt b/CMakeLists.txt index b0c0baa..8a2abb1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,7 +80,8 @@ endif() # add the vendor folder to the include path target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE vendor) -target_sources(${CMAKE_PROJECT_NAME} PRIVATE src/plugin-main.c src/detect-filter.cpp src/detect-filter-info.c - src/obs-utils/obs-utils.cpp src/edgeyolo/edgeyolo_onnxruntime.cpp) +target_sources( + ${CMAKE_PROJECT_NAME} PRIVATE src/plugin-main.c src/detect-filter.cpp src/detect-filter-info.c + src/obs-utils/obs-utils.cpp src/edgeyolo/edgeyolo_onnxruntime.cpp src/sort/Sort.cpp) set_target_properties_plugin(${CMAKE_PROJECT_NAME} PROPERTIES OUTPUT_NAME ${_name}) diff --git a/README.md b/README.md index 040874a..6d5180f 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ If you like this work, which is given to you completely free of charge, please c - https://github.com/sponsors/royshil - https://github.com/sponsors/umireon -This work uses the great contributions from [EdgeYOLO-ROS](https://github.com/fateshelled/EdgeYOLO-ROS) and [PINTO-Model-Zoo](https://github.com/PINTO0309/PINTO_model_zoo). +This work uses the great contributions from [EdgeYOLO-ROS](https://github.com/fateshelled/EdgeYOLO-ROS) and [PINTO-Model-Zoo](https://github.com/PINTO0309/PINTO_model_zoo). The Hungarian algorithm is taken from https://github.com/Gluttton/munkres-cpp under the GPLv2 license. ## Usage diff --git a/cmake/FetchOpenCV.cmake b/cmake/FetchOpenCV.cmake index cccdc25..467e5dc 100644 --- a/cmake/FetchOpenCV.cmake +++ b/cmake/FetchOpenCV.cmake @@ -19,7 +19,7 @@ else() endif() if(USE_PREDEFINED_OPENCV) - set(OpenCV_VERSION "v4.8.1-1") + set(OpenCV_VERSION "v4.9.0-1") set(OpenCV_BASEURL "https://github.com/obs-ai/obs-backgroundremoval-dep-opencv/releases/download/${OpenCV_VERSION}") if(${CMAKE_BUILD_TYPE} STREQUAL Release OR ${CMAKE_BUILD_TYPE} STREQUAL RelWithDebInfo) @@ -31,26 +31,26 @@ if(USE_PREDEFINED_OPENCV) if(APPLE) if(OpenCV_BUILD_TYPE STREQUAL Debug) set(OpenCV_URL "${OpenCV_BASEURL}/opencv-macos-${OpenCV_VERSION}-Debug.tar.gz") - set(OpenCV_HASH SHA256=2930e335a19cc03a3d825e2b76eadd0d5cf08d8baf6537747d43f503dff32454) + set(OpenCV_HASH SHA256=BE85C8224F71C52162955BEE4EC9FFBE41CBED636D7989843CA75AD42657B121) else() set(OpenCV_URL "${OpenCV_BASEURL}/opencv-macos-${OpenCV_VERSION}-Release.tar.gz") - set(OpenCV_HASH SHA256=b0c4fe2370b0bd5aa65c408e875b1ab18508ba31b93083805d7e398a3ecafdac) + set(OpenCV_HASH SHA256=5DB4FCFBD8C7CDBA136657B4D149821A670DF9A7C71120F5A4D34FA35A58D07B) endif() elseif(MSVC) if(OpenCV_BUILD_TYPE STREQUAL Debug) set(OpenCV_URL "${OpenCV_BASEURL}/opencv-windows-${OpenCV_VERSION}-Debug.zip") - set(OpenCV_HASH SHA256=0c5ef12cf4b4e4db7ea17a24db156165b6f01759f3f1660b069d0722e5d5dc37) + set(OpenCV_HASH SHA256=0A1BBC898DCE5F193427586DA84D7A34BBB783127957633236344E9CCD61B9CE) else() set(OpenCV_URL "${OpenCV_BASEURL}/opencv-windows-${OpenCV_VERSION}-Release.zip") - set(OpenCV_HASH SHA256=5e468f71d41d3a3ea46cc4f247475877f65d3655a2764a2c01074bda3b3e6864) + set(OpenCV_HASH SHA256=56A5E042F490B8390B1C1819B2B48C858F10CD64E613BABBF11925A57269C3FA) endif() else() if(OpenCV_BUILD_TYPE STREQUAL Debug) set(OpenCV_URL "${OpenCV_BASEURL}/opencv-linux-${OpenCV_VERSION}-Debug.tar.gz") - set(OpenCV_HASH SHA256=e2e246d6b4f279be80e7fd0a78cba8a0eeee7b53ae807f2f57428d6876306422) + set(OpenCV_HASH SHA256=840A7D80B661CFF7B7300272A2A2992D539672ECECA01836B85F68BD8CAF07F5) else() set(OpenCV_URL "${OpenCV_BASEURL}/opencv-linux-${OpenCV_VERSION}-Release.tar.gz") - set(OpenCV_HASH SHA256=809922a7cc9f344a2d82a232ed7b02e122c82e77cba94b4047e666a0527cc00e) + set(OpenCV_HASH SHA256=73652C2155B477B5FD95FCD8EA7CE35D313543ECE17BDFA3A2B217A0239D74C6) endif() endif() else() @@ -68,13 +68,14 @@ add_library(OpenCV INTERFACE) if(MSVC) target_link_libraries( OpenCV - INTERFACE ${opencv_SOURCE_DIR}/x64/vc17/staticlib/opencv_imgproc481.lib - ${opencv_SOURCE_DIR}/x64/vc17/staticlib/opencv_core481.lib + INTERFACE ${opencv_SOURCE_DIR}/x64/vc17/staticlib/opencv_imgproc490.lib + ${opencv_SOURCE_DIR}/x64/vc17/staticlib/opencv_core490.lib + ${opencv_SOURCE_DIR}/x64/vc17/staticlib/opencv_video490.lib ${opencv_SOURCE_DIR}/x64/vc17/staticlib/zlib.lib) target_include_directories(OpenCV SYSTEM INTERFACE ${opencv_SOURCE_DIR}/include) else() target_link_libraries( OpenCV INTERFACE ${opencv_SOURCE_DIR}/lib/libopencv_imgproc.a ${opencv_SOURCE_DIR}/lib/libopencv_core.a - ${opencv_SOURCE_DIR}/lib/opencv4/3rdparty/libzlib.a) + ${opencv_SOURCE_DIR}/lib/libopencv_video.a ${opencv_SOURCE_DIR}/lib/opencv4/3rdparty/libzlib.a) target_include_directories(OpenCV SYSTEM INTERFACE ${opencv_SOURCE_DIR}/include/opencv4) endif() diff --git a/cmake/linux/compilerconfig.cmake b/cmake/linux/compilerconfig.cmake index 67d2b74..d19305b 100644 --- a/cmake/linux/compilerconfig.cmake +++ b/cmake/linux/compilerconfig.cmake @@ -21,6 +21,7 @@ set(_obs_gcc_c_options -Wformat -Wformat-security -Wno-conversion + -Wno-error=shadow -Wno-float-conversion -Wno-implicit-fallthrough -Wno-missing-braces diff --git a/data/locale/en-US.ini b/data/locale/en-US.ini index 890e723..24b30a6 100644 --- a/data/locale/en-US.ini +++ b/data/locale/en-US.ini @@ -29,5 +29,8 @@ ZoomObject="Zoom Object" SingleFirst="Single (First)" ZoomSpeed="Zoom Speed" DetectedObject="Detected Object" +SORTTracking="Continuous Tracking" +MaxUnseenFrames="Max Unseen Frames" ExternalModel="External Model" ModelPath="Model Path" +ShowUnseenObjects="Show Currently Undetected Objects" diff --git a/src/FilterData.h b/src/FilterData.h index 0b70a45..c73c409 100644 --- a/src/FilterData.h +++ b/src/FilterData.h @@ -3,6 +3,7 @@ #include #include "edgeyolo/edgeyolo_onnxruntime.hpp" +#include "sort/Sort.h" /** * @brief The filter_data struct @@ -28,6 +29,11 @@ struct filter_data { obs_source_t *trackingFilter; cv::Rect2f trackingRect; int lastDetectedObjectId; + bool sortTracking; + bool showUnseenObjects; + + // create SORT tracker + Sort tracker; obs_source_t *source; gs_texrender_t *texrender; diff --git a/src/detect-filter.cpp b/src/detect-filter.cpp index 79d8fb2..aa189f6 100644 --- a/src/detect-filter.cpp +++ b/src/detect-filter.cpp @@ -247,15 +247,25 @@ obs_properties_t *detect_filter_properties(void *data) obs_property_set_modified_callback(advanced, enable_advanced_settings); // add a text input for the currently detected object - obs_properties_add_text(props, "detected_object", obs_module_text("DetectedObject"), - OBS_TEXT_DEFAULT); + obs_property_t *detected_obj_prop = obs_properties_add_text( + props, "detected_object", obs_module_text("DetectedObject"), OBS_TEXT_DEFAULT); // disable the text input by default - obs_property_set_enabled(obs_properties_get(props, "detected_object"), false); + obs_property_set_enabled(detected_obj_prop, false); // add threshold slider obs_properties_add_float_slider(props, "threshold", obs_module_text("ConfThreshold"), 0.0, 1.0, 0.025); + // add SORT tracking enabled checkbox + obs_properties_add_bool(props, "sort_tracking", obs_module_text("SORTTracking")); + + // add parameter for number of missing frames before a track is considered lost + obs_properties_add_int(props, "max_unseen_frames", obs_module_text("MaxUnseenFrames"), 1, + 30, 1); + + // add option to show unseen objects + obs_properties_add_bool(props, "show_unseen_objects", obs_module_text("ShowUnseenObjects")); + /* GPU, CPU and performance Props */ obs_property_t *p_use_gpu = obs_properties_add_list(props, "useGPU", obs_module_text("InferenceDevice"), @@ -355,6 +365,9 @@ void detect_filter_defaults(obs_data_t *settings) // Linux obs_data_set_default_string(settings, "useGPU", USEGPU_CPU); #endif + obs_data_set_default_bool(settings, "sort_tracking", false); + obs_data_set_default_int(settings, "max_unseen_frames", 10); + obs_data_set_default_bool(settings, "show_unseen_objects", true); obs_data_set_default_int(settings, "numThreads", 1); obs_data_set_default_bool(settings, "preview", true); obs_data_set_default_double(settings, "threshold", 0.5); @@ -389,6 +402,12 @@ void detect_filter_update(void *data, obs_data_t *settings) tf->zoomFactor = (float)obs_data_get_double(settings, "zoom_factor"); tf->zoomSpeedFactor = (float)obs_data_get_double(settings, "zoom_speed_factor"); tf->zoomObject = obs_data_get_string(settings, "zoom_object"); + tf->sortTracking = obs_data_get_bool(settings, "sort_tracking"); + size_t maxUnseenFrames = (size_t)obs_data_get_int(settings, "max_unseen_frames"); + if (tf->tracker.getMaxUnseenFrames() != maxUnseenFrames) { + tf->tracker.setMaxUnseenFrames(maxUnseenFrames); + } + tf->showUnseenObjects = obs_data_get_bool(settings, "show_unseen_objects"); // check if tracking state has changed if (tf->trackingEnabled != newTrackingEnabled) { @@ -748,6 +767,18 @@ void detect_filter_video_tick(void *data, float seconds) objects = filtered_objects; } + if (tf->sortTracking) { + objects = tf->tracker.update(objects); + } + + if (!tf->showUnseenObjects) { + objects.erase(std::remove_if(objects.begin(), objects.end(), + [](const edgeyolo_cpp::Object &obj) { + return obj.unseenFrames > 0; + }), + objects.end()); + } + if (tf->preview || tf->maskingEnabled) { if (tf->preview && objects.size() > 0) { edgeyolo_cpp::utils::draw_objects(frame, objects, tf->classNames); diff --git a/src/edgeyolo/core.hpp b/src/edgeyolo/core.hpp index 2d3e0e9..6fe470f 100644 --- a/src/edgeyolo/core.hpp +++ b/src/edgeyolo/core.hpp @@ -4,6 +4,8 @@ #include #include +#include "types.hpp" + namespace edgeyolo_cpp { /** * @brief Define names based depends on Unicode path support @@ -18,18 +20,6 @@ namespace edgeyolo_cpp { #define imread_t cv::imread -struct Object { - cv::Rect_ rect; - int label; - float prob; -}; - -struct GridAndStride { - int grid0; - int grid1; - int stride; -}; - class AbcEdgeYOLO { public: AbcEdgeYOLO() {} @@ -243,6 +233,7 @@ class AbcEdgeYOLO { proposals[picked[i]].rect.y = y0; proposals[picked[i]].rect.width = x1 - x0; proposals[picked[i]].rect.height = y1 - y0; + proposals[picked[i]].id = objects.size() + 1; objects.push_back(proposals[picked[i]]); } diff --git a/src/edgeyolo/types.hpp b/src/edgeyolo/types.hpp new file mode 100644 index 0000000..94d612f --- /dev/null +++ b/src/edgeyolo/types.hpp @@ -0,0 +1,26 @@ +#ifndef EDGEYOLO_TYPES_HPP +#define EDGEYOLO_TYPES_HPP + +#include +#include + +namespace edgeyolo_cpp { + +struct Object { + cv::Rect_ rect; + int label; + float prob; + uint64_t id; + uint64_t unseenFrames; + cv::KalmanFilter kf; +}; + +struct GridAndStride { + int grid0; + int grid1; + int stride; +}; + +} // namespace edgeyolo_cpp + +#endif diff --git a/src/edgeyolo/utils.hpp b/src/edgeyolo/utils.hpp index a72d2d3..e9704ab 100644 --- a/src/edgeyolo/utils.hpp +++ b/src/edgeyolo/utils.hpp @@ -70,6 +70,11 @@ static void draw_objects(cv::Mat bgr, const std::vector &objects, cv::putText(bgr, text, cv::Point(x, y + label_size.height), cv::FONT_HERSHEY_SIMPLEX, 0.4, txt_color, 1); + + // write the id of the object + snprintf(text, sizeof(text), "ID: %d", (int)obj.id); + cv::putText(bgr, text, cv::Point(x, y + label_size.height + 15), + cv::FONT_HERSHEY_SIMPLEX, 0.4, txt_color, 1); } } } // namespace utils diff --git a/src/plugin-support.c.in b/src/plugin-support.c.in index 44a2c1c..9173f45 100644 --- a/src/plugin-support.c.in +++ b/src/plugin-support.c.in @@ -21,6 +21,8 @@ with this program. If not, see const char *PLUGIN_NAME = "@CMAKE_PROJECT_NAME@"; const char *PLUGIN_VERSION = "@CMAKE_PROJECT_VERSION@"; +extern void blogva(int log_level, const char *format, va_list args); + void obs_log(int log_level, const char *format, ...) { size_t length = 4 + strlen(PLUGIN_NAME) + strlen(format); diff --git a/src/plugin-support.h b/src/plugin-support.h index 8ffb57c..6959fcf 100644 --- a/src/plugin-support.h +++ b/src/plugin-support.h @@ -31,7 +31,6 @@ extern const char *PLUGIN_NAME; extern const char *PLUGIN_VERSION; void obs_log(int log_level, const char *format, ...); -extern void blogva(int log_level, const char *format, va_list args); #ifdef __cplusplus } diff --git a/src/sort/Sort.cpp b/src/sort/Sort.cpp new file mode 100644 index 0000000..cebee2a --- /dev/null +++ b/src/sort/Sort.cpp @@ -0,0 +1,215 @@ +#include "Sort.h" + +#include "sort/munkres-cpp/matrix_base.h" +#include "sort/munkres-cpp/adapters/matrix_std_2d_vector.h" +#include "sort/munkres-cpp/munkres.h" + +#include +#include +#include + +#include "plugin-support.h" + +#include + +#define INF std::numeric_limits::infinity() + +// Constructor +Sort::Sort(size_t maxUnseenFrames_) : nextTrackID(0), maxUnseenFrames(maxUnseenFrames_) {} + +// Destructor +Sort::~Sort() {} + +// Initialize the Kalman filter for a new object +void Sort::initializeKalmanFilter(cv::KalmanFilter &kf, const cv::Rect_ &bbox) +{ + // Linear motion model with dimension: [x, y, width, height, dx, dy, dwidth, dheight] + kf.init(8, 4, 0); + + // State vector: [x, y, width, height, dx, dy, dwidth, dheight] + kf.statePre.at(0) = bbox.x; + kf.statePre.at(1) = bbox.y; + kf.statePre.at(2) = bbox.width; + kf.statePre.at(3) = bbox.height; + + // Transition matrix + kf.transitionMatrix = (cv::Mat_(8, 8) << 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, + 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 0, 0, 1); + + // Measurement matrix + kf.measurementMatrix = cv::Mat::zeros(4, 8, CV_32F); + for (int i = 0; i < 4; ++i) { + kf.measurementMatrix.at(i, i) = 1; + } + + // Process noise covariance matrix + const float q = 1e-1f; + kf.processNoiseCov = (cv::Mat_(8, 8) << 0.25, 0, 0, 0, 0.5, 0, 0, 0, 0, 0.25, 0, 0, + 0, 0.5, 0, 0, 0, 0, 0.25, 0, 0, 0, 0.5, 0, 0, 0, 0, 0.25, 0, 0, 0, + 0.5, 0.5, 0, 0, 0, 1, 0, 0, 0, 0, 0.5, 0, 0, 0, 1, 0, 0, 0, 0, 0.5, 0, + 0, 0, 1, 0, 0, 0, 0, 0.5, 0, 0, 0, 1) * + q; + + // Measurement noise covariance matrix + kf.measurementNoiseCov = + (cv::Mat_(4, 4) << 4, 0, 0, 0, 0, 4, 0, 0, 0, 0, 10, 0, 0, 0, 0, 10); + + // Error covariance matrix + cv::setIdentity(kf.errorCovPost, cv::Scalar::all(1e3)); + + // Correct the state vector with the initial measurement + cv::Mat measurement(4, 1, CV_32F); + measurement.at(0) = bbox.x; + measurement.at(1) = bbox.y; + measurement.at(2) = bbox.width; + measurement.at(3) = bbox.height; + kf.correct(measurement); +} + +// Predict the next state of the object +cv::Rect_ Sort::predict(cv::KalmanFilter &kf) +{ + cv::Mat prediction = kf.predict(); + return cv::Rect_(prediction.at(0), prediction.at(1), + prediction.at(2), prediction.at(3)); +} + +// Update the Kalman filter with a new measurement +cv::Rect_ Sort::updateKalmanFilter(cv::KalmanFilter &kf, const cv::Rect_ &bbox) +{ + cv::Mat measurement(4, 1, CV_32F); + measurement.at(0) = bbox.x; + measurement.at(1) = bbox.y; + measurement.at(2) = bbox.width; + measurement.at(3) = bbox.height; + const auto corrected = kf.correct(measurement); + return cv::Rect_(corrected.at(0), corrected.at(1), + corrected.at(2), corrected.at(3)); +} + +// Compute the Intersection over Union (IoU) between two rectangles +float computeIoU(const cv::Rect_ &rect1, const cv::Rect_ &rect2) +{ + float intersectionArea = (rect1 & rect2).area(); + float unionArea = rect1.area() + rect2.area() - intersectionArea; + return intersectionArea / unionArea; +} + +// Update the tracking with detected objects +std::vector Sort::update(const std::vector &detections) +{ + if (detections.empty()) { + std::vector newTrackedObjects; + + // No detections, predict the next state of the existing tracks and update unseen frames + for (size_t i = 0; i < trackedObjects.size(); ++i) { + trackedObjects[i].rect = predict(trackedObjects[i].kf); + trackedObjects[i].unseenFrames++; + + // Remove lost tracks + if (trackedObjects[i].unseenFrames < this->maxUnseenFrames) { + newTrackedObjects.push_back(trackedObjects[i]); + } + } + trackedObjects = newTrackedObjects; + return trackedObjects; + } + + if (trackedObjects.empty()) { + // No existing tracks, create new tracks for all detections + for (const auto &detection : detections) { + cv::KalmanFilter kf; + initializeKalmanFilter(kf, detection.rect); + trackedObjects.push_back(detection); + trackedObjects.back().kf = kf; // store the Kalman filter in the object + trackedObjects.back().id = nextTrackID++; + trackedObjects.back().unseenFrames = 0; + } + return trackedObjects; + } + + // Predict new locations of existing tracked objects + for (size_t i = 0; i < trackedObjects.size(); ++i) { + trackedObjects[i].rect = predict(trackedObjects[i].kf); + } + + // Build the cost matrix for the Hungarian algorithm + size_t numDetections = detections.size(); + std::vector> costMatrix(trackedObjects.size(), + std::vector(numDetections, 0)); + + for (size_t i = 0; i < trackedObjects.size(); ++i) { + for (size_t j = 0; j < numDetections; ++j) { + const float iou = computeIoU(trackedObjects[i].rect, detections[j].rect); + costMatrix[i][j] = iou > 0.0f ? 1.0f - iou : 10.0f; + } + } + + // Solve the assignment problem using the Hungarian algorithm + munkres_cpp::matrix_std_2d_vector costMatrixAdapter(costMatrix); + munkres_cpp::Munkres solver(costMatrixAdapter); + + // Update Kalman filters with associated detections + std::vector detectionUsed(numDetections, false); + std::vector trackedObjectUsed(trackedObjects.size(), false); + for (size_t i = 0; i < trackedObjects.size(); ++i) { + for (size_t j = 0; j < numDetections; ++j) { + if (costMatrix[i][j] == 0) { + const float iou = + computeIoU(trackedObjects[i].rect, detections[j].rect); + if (iou == 0) { + // prevent matching detections without any overlap + continue; + } + // update the tracked object with the new detection + trackedObjects[i].rect = updateKalmanFilter(trackedObjects[i].kf, + detections[j].rect); + trackedObjects[i].unseenFrames = 0; + trackedObjects[i].label = detections[j].label; + trackedObjects[i].prob = detections[j].prob; + // mark the detection and the tracked object as used + detectionUsed[j] = true; + trackedObjectUsed[i] = true; + break; + } + } + } + + // Create new tracks for unmatched detections + for (size_t j = 0; j < numDetections; ++j) { + if (!detectionUsed[j]) { + cv::KalmanFilter kf; + initializeKalmanFilter(kf, detections[j].rect); + trackedObjects.push_back(detections[j]); + trackedObjects.back().kf = kf; // store the Kalman filter in the object + trackedObjects.back().id = nextTrackID++; + trackedObjects.back().unseenFrames = 0; + // resize trackedObjectUsed to match the new size of trackedObjects + trackedObjectUsed.resize(trackedObjects.size(), true); + } + } + + // Remove lost tracks + std::vector newTrackedObjects; + std::vector newTrackIDs; + for (size_t i = 0; i < trackedObjects.size(); ++i) { + if (trackedObjectUsed[i] || + trackedObjects[i].unseenFrames < this->maxUnseenFrames) { + newTrackedObjects.push_back(trackedObjects[i]); + if (!trackedObjectUsed[i]) { + newTrackedObjects.back().unseenFrames++; + } + } + } + trackedObjects = newTrackedObjects; + + return trackedObjects; +} + +// Get the current tracked objects and their tracking id +std::vector Sort::getTrackedObjects() const +{ + return trackedObjects; +} diff --git a/src/sort/Sort.h b/src/sort/Sort.h new file mode 100644 index 0000000..9c17f11 --- /dev/null +++ b/src/sort/Sort.h @@ -0,0 +1,47 @@ +#ifndef SORT_H +#define SORT_H + +#include +#include +#include + +#include "edgeyolo/types.hpp" + +class Sort { +public: + // Constructor + Sort(size_t maxUnseenFrames = 5); + + // Destructor + ~Sort(); + + // Update the tracking with detected objects + std::vector + update(const std::vector &detections); + + // Get the current tracked objects and their classes + std::vector getTrackedObjects() const; + + // Set Max Unseen Frames + void setMaxUnseenFrames(size_t maxUnseenFrames_) + { + this->maxUnseenFrames = maxUnseenFrames_; + } + + // Get Max Unseen Frames + size_t getMaxUnseenFrames() const { return this->maxUnseenFrames; } + +private: + // Private methods for the Kalman filter and other internal workings + void initializeKalmanFilter(cv::KalmanFilter &kf, const cv::Rect_ &bbox); + cv::Rect_ predict(cv::KalmanFilter &kf); + cv::Rect_ updateKalmanFilter(cv::KalmanFilter &kf, const cv::Rect_ &bbox); + + // Data members for tracking + std::vector trackedObjects; + uint64_t nextTrackID; + size_t maxUnseenFrames; +}; + +#endif +// Path: src/sort-cpp/Sort.cpp diff --git a/src/sort/munkres-cpp/adapters/matrix_std_2d_vector.h b/src/sort/munkres-cpp/adapters/matrix_std_2d_vector.h new file mode 100644 index 0000000..75a207e --- /dev/null +++ b/src/sort/munkres-cpp/adapters/matrix_std_2d_vector.h @@ -0,0 +1,84 @@ +/* + * Copyright (c) 2016 Gluttton + * + * 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_ADAPTERS_STD_2D_VECTOR_H_) +#define _MUNKRES_ADAPTERS_STD_2D_VECTOR_H_ + +#include "sort/munkres-cpp/munkres.h" +#include + +namespace munkres_cpp { + +template class matrix_std_2d_vector : public matrix_base { +public: + matrix_std_2d_vector(std::vector> &data) : data_handler{data} {} + + matrix_std_2d_vector(size_t rows, size_t columns) + : data_storage{}, + data_handler{data_storage} + { + resize(rows, columns); + } + + matrix_std_2d_vector(const matrix_std_2d_vector &that) + : data_storage{}, + data_handler{data_storage} + { + this->data_storage = that.data_storage; + } + + matrix_std_2d_vector &operator=(const matrix_std_2d_vector &that) + { + this->data_storage = that.data_storage; + return *this; + } + + const T &operator()(size_t row, size_t column) const override + { + return data_handler[row][column]; + }; + + T &operator()(size_t row, size_t column) override { return data_handler[row][column]; } + + size_t columns() const override + { + size_t columns = data_handler.size() ? data_handler[0].size() : 0; + for (size_t i = 0; i < data_handler.size(); ++i) { + columns = std::min(columns, data_handler[i].size()); + } + return columns; + } + + size_t rows() const override { return data_handler.size(); } + + void resize(size_t rows, size_t columns, T value = T(0)) override + { + data_handler.resize(rows); + for (size_t i = 0; i < rows; ++i) { + data_handler[i].resize(columns, value); + } + } + +private: + std::vector> data_storage; + std::vector> &data_handler; +}; + +} // namespace munkres_cpp + +#endif /* !defined(_MUNKRES_ADAPTERS_STD_2D_VECTOR_H_) */ diff --git a/src/sort/munkres-cpp/matrix.h b/src/sort/munkres-cpp/matrix.h new file mode 100644 index 0000000..f8cc771 --- /dev/null +++ b/src/sort/munkres-cpp/matrix.h @@ -0,0 +1,165 @@ +/* + * Copyright (c) 2007 John Weaver + * Copyright (c) 2016 Gluttton + * + * 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(_MATRIX_H_) +#define _MATRIX_H_ + +#include "munkres-cpp/matrix_base.h" +#include +#include + +namespace munkres_cpp { + +template class Matrix : public matrix_base { +public: + Matrix() = default; + Matrix(size_t, size_t); + Matrix(const std::initializer_list> &); + Matrix(const Matrix &); + Matrix &operator=(const Matrix &); + ~Matrix() override; + // All operations modify the matrix in-place. + void resize(size_t, size_t, T default_value = T(0)) override; + const T &operator()(size_t x, size_t y) const override + { + assert(x < m_rows); + assert(y < m_columns); + assert(m_matrix != nullptr); + return m_matrix[x][y]; + }; + T &operator()(size_t x, size_t y) override + { + assert(x < m_rows); + assert(y < m_columns); + assert(m_matrix != nullptr); + return m_matrix[x][y]; + }; + size_t columns() const override { return m_columns; } + size_t rows() const override { return m_rows; } + +private: + T **m_matrix{nullptr}; + size_t m_rows{0}; + size_t m_columns{0}; +}; + +template +Matrix::Matrix(const std::initializer_list> &init) : Matrix() +{ + m_rows = init.size(); + if (m_rows) { + m_columns = init.begin()->size(); + if (m_columns > 0) { + resize(m_rows, m_columns); + } + } + + size_t i = 0; + for (auto row = init.begin(); row != init.end(); ++row, ++i) { + assert(row->size() == m_columns); + size_t j = 0; + for (auto value = row->begin(); value != row->end(); ++value, ++j) { + m_matrix[i][j] = *value; + } + } +} + +template Matrix::Matrix(const Matrix &other) : Matrix() +{ + if (other.m_matrix) { + resize(other.m_rows, other.m_columns); + std::copy(other.m_matrix[0], other.m_matrix[0] + m_rows * m_columns, m_matrix[0]); + } +} + +template Matrix::Matrix(size_t rows, size_t columns) : Matrix() +{ + resize(rows, columns); +} + +template Matrix &Matrix::operator=(const Matrix &other) +{ + if (this == &other) { + return *this; + } + + if (other.m_matrix != nullptr) { + resize(other.m_rows, other.m_columns); + std::copy(other.m_matrix[0], other.m_matrix[0] + m_rows * m_columns, m_matrix[0]); + } else { + // Free arrays. + delete[] m_matrix[0]; + delete[] m_matrix; + + m_matrix = nullptr; + m_rows = 0; + m_columns = 0; + } + + return *this; +} + +template Matrix::~Matrix() +{ + if (m_matrix != nullptr) { + delete[] m_matrix[0]; + delete[] m_matrix; + } + m_matrix = nullptr; +} + +template void Matrix::resize(size_t rows, size_t columns, T default_value) +{ + T **new_matrix = new T *[rows]; // Row pointers. + try { + new_matrix[0] = new T[rows * columns]; // All data in one stripe. + } catch (std::bad_alloc &) { + delete[] new_matrix; + throw; + } + for (size_t i = 1; i < rows; i++) { + new_matrix[i] = new_matrix[0] + i * columns; + } + std::fill(new_matrix[0], new_matrix[0] + rows * columns, default_value); + + if (m_matrix) { + // Copy data from saved pointer to new arrays. + const size_t minrows = std::min(rows, m_rows); + const size_t mincols = std::min(columns, m_columns); + for (size_t y = 0; y < mincols; y++) { + for (size_t x = 0; x < minrows; x++) { + new_matrix[x][y] = m_matrix[x][y]; + } + } + + // Delete old arrays. + if (m_matrix != nullptr) { + delete[] m_matrix[0]; + delete[] m_matrix; + } + } + + m_rows = rows; + m_columns = columns; + m_matrix = new_matrix; +} + +} // namespace munkres_cpp + +#endif /* !defined(_MATRIX_H_) */ diff --git a/src/sort/munkres-cpp/matrix_base.h b/src/sort/munkres-cpp/matrix_base.h new file mode 100644 index 0000000..092845f --- /dev/null +++ b/src/sort/munkres-cpp/matrix_base.h @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2016 Gluttton + * + * 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_CPP_MATRIX_BASE_H__) +#define __MUNKRES_CPP_MATRIX_BASE_H__ + +#include + +namespace munkres_cpp { + +template struct matrix_base { + // Types. + using value_type = T; + + // Interface. + virtual const value_type &operator()(size_t, size_t) const = 0; + virtual value_type &operator()(size_t, size_t) = 0; + virtual size_t columns() const = 0; + virtual size_t rows() const = 0; + virtual void resize(size_t, size_t, value_type = value_type(0)) = 0; + + // Default implementation. + virtual ~matrix_base() = default; + + // Allow to use standard algorithms. + template struct iterator { + iterator(M &m, size_t r, size_t c) : m{m}, r{r}, c{c} {} + typename std::conditional::value, const typename M::value_type, + typename M::value_type>::type & + operator*() const + { + return m(r, c); + } + bool operator==(const iterator &that) + { + return this->r == that.r && this->c == that.c; + } + bool operator!=(const iterator &that) { return !operator==(that); } + iterator &operator++() + { + r += ++c / m.columns(); + c = c % m.columns(); + return *this; + } + + M &m; + size_t r, c; + + using difference_type = std::ptrdiff_t; + using value_type = T; + using pointer = T *; + using reference = T &; + using iterator_category = std::input_iterator_tag; + }; + + template using const_iterator = iterator; + + iterator begin() { return iterator{*this, 0, 0}; } + iterator end() { return iterator{*this, rows(), 0}; } + const_iterator begin() const + { + return const_iterator{*this, 0, 0}; + } + const_iterator end() const + { + return const_iterator{*this, rows(), 0}; + } +}; + +} // namespace munkres_cpp + +#endif /* !defined(__MUNKRES_CPP_MATRIX_BASE_H__) */ diff --git a/src/sort/munkres-cpp/munkres.h b/src/sort/munkres-cpp/munkres.h new file mode 100644 index 0000000..5e85b9e --- /dev/null +++ b/src/sort/munkres-cpp/munkres.h @@ -0,0 +1,303 @@ +/* + * Copyright (c) 2007 John Weaver + * Copyright (c) 2016 Gluttton + * + * 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 +#include +#include +#include + +namespace munkres_cpp { + +template +static constexpr typename std::enable_if::value, bool>::type is_zero(const T &v) +{ + return v == 0; +} + +template +static constexpr typename std::enable_if::value, bool>::type +is_zero(const T &v) +{ + return FP_ZERO == std::fpclassify(v); +} + +template class M> class Munkres { +public: + Munkres(M &); + Munkres(const Munkres &) = delete; + Munkres &operator=(const Munkres &) = delete; + +private: + bool find_uncovered_in_matrix(size_t &, size_t &) const; + int step1(); + int step2(); + int step3(); + int step4(); + int step5(); + int step6(); + + const size_t size; + M &matrix; + M mask_matrix; + bool *const row_mask; + bool *const col_mask; + size_t saverow; + size_t savecol; + enum MASK : char { + NORMAL, + STAR // starred, + , + PRIME // primed. + }; +}; + +template class M> +void minimize_along_direction(M &matrix, bool over_columns) +{ + // Look for a minimum value to subtract from all values along the "outer" direction. + size_t i = 0, j = 0, size = matrix.rows(); + size_t &r = over_columns ? j : i; + size_t &c = over_columns ? i : j; + for (; i < size; i++, j = 0) { + T min = matrix(r, c); + + // 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 (j = 1; j < size && min > 0; j++) + min = std::min(min, matrix(r, c)); + + if (min > 0) + for (j = 0; j < size; j++) + matrix(r, c) -= min; + } +} + +template class M> +bool Munkres::find_uncovered_in_matrix(size_t &row, size_t &col) const +{ + for (col = 0; col < size; col++) + if (!col_mask[col]) + for (row = 0; row < size; row++) + if (!row_mask[row]) + if (is_zero(matrix(row, col))) + return true; + + return false; +} + +template class M> int Munkres::step1() +{ + for (size_t row = 0; row < size; row++) { + for (size_t col = 0; col < size; col++) { + if (is_zero(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; +} + +template class M> int Munkres::step2() +{ + size_t covercount = 0; + + for (size_t col = 0; col < size; col++) + for (size_t row = 0; row < size; row++) + if (STAR == mask_matrix(row, col)) { + col_mask[col] = true; + covercount++; + } + + return covercount >= size ? 0 : 3; +} + +template class M> int Munkres::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(saverow, savecol)) { + mask_matrix(saverow, savecol) = PRIME; // Prime it. + for (size_t ncol = 0; ncol < size; 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. + } + + return 5; +} + +template class M> int Munkres::step4() +{ + // Seq contains pairs of row/column values where we have found + // either a star or a prime that is part of the ``alternating sequence``. + // Use saverow, savecol from step 3. + std::forward_list> seq{{saverow, 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. + size_t dim[] = {0, savecol}; + const char mask[] = {STAR, PRIME}; + for (size_t i = 0; dim[i] < size; ++dim[i]) { + if (mask_matrix(dim[0], dim[1]) == mask[i]) { + // We have to find these two pairs: z1 and z2n. + seq.push_front({dim[0], dim[1]}); + i = (i + 1) & 1; // Switch dimension. + dim[i] = -1; // After increment this value becames zero. + } + } + + for (const auto &i : seq) { + // 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, + std::replace(mask_matrix.begin(), mask_matrix.end(), PRIME, NORMAL); + std::fill_n(row_mask, size, false); + std::fill_n(col_mask, size, false); + + // and return to Step 2. + return 2; +} + +template class M> int Munkres::step5() +{ + // 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. + T h = std::numeric_limits::max(); + for (size_t col = 0; col < size; col++) + if (!col_mask[col]) + for (size_t row = 0; row < size; row++) + if (!row_mask[row]) + if (h > matrix(row, col) && !is_zero(matrix(row, col))) + h = matrix(row, col); + + for (size_t row = 0; row < size; row++) + if (row_mask[row]) + for (size_t col = 0; col < size; col++) + matrix(row, col) += h; + + for (size_t col = 0; col < size; col++) + if (!col_mask[col]) + for (size_t row = 0; row < size; row++) + matrix(row, col) -= h; + + return 3; +} + +// 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) +template class M> +Munkres::Munkres(M &matrix) + : size{std::max(matrix.rows(), matrix.columns())}, + matrix{matrix}, + mask_matrix{size, size}, + row_mask{new bool[size]}, + col_mask{new bool[size]}, + saverow{0}, + savecol{0} +{ + const size_t rows = matrix.rows(); + const size_t columns = matrix.columns(); + + std::fill_n(row_mask, size, false); + std::fill_n(col_mask, size, false); + + if (rows != columns) + // If the input matrix isn't square, make it square and fill + // the empty values with the maximum possible value. + matrix.resize(size, size, std::numeric_limits::max()); + + // Prepare the matrix values... + 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 + // Fallthrough. + 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 col = 0; col < size; col++) + for (size_t row = 0; row < size; row++) + matrix(row, col) = (T)(mask_matrix(row, col) == STAR ? 0 : 1); + + // Remove the excess rows or columns that we added to fit the input to a square matrix. + matrix.resize(rows, columns, 1); + + delete[] row_mask; + delete[] col_mask; +} + +} // namespace munkres_cpp + +#endif /* !defined(_MUNKRES_H_) */ diff --git a/src/sort/munkres-cpp/utils.h b/src/sort/munkres-cpp/utils.h new file mode 100644 index 0000000..b82eee2 --- /dev/null +++ b/src/sort/munkres-cpp/utils.h @@ -0,0 +1,53 @@ +#if !defined(__MUNKRES_CPP_UTILS_H__) +#define __MUNKRES_CPP_UTILS_H__ + +#include +#include + +namespace munkres_cpp { + +template class M> +typename std::enable_if::value>::type replace_infinites(M &matrix) +{ + std::replace_if( + matrix.begin(), matrix.end(), [](const T &v) { return std::isinf(v); }, + std::numeric_limits::max()); +} + +template +typename std::enable_if::value && std::is_unsigned::value, bool>::type +is_data_invalid(const T &) +{ + return false; +} + +template +typename std::enable_if::value && std::is_signed::value, bool>::type +is_data_invalid(const T &value) +{ + return std::signbit(value); +} + +template +typename std::enable_if::value, bool>::type is_data_invalid(const T &value) +{ + return value < T(0) || + !(std::fpclassify(value) == FP_ZERO || std::fpclassify(value) == FP_NORMAL); +} + +template class M> +typename std::enable_if::value, bool>::type is_data_valid(const M &) +{ + return true; +} + +template class M> +typename std::enable_if::value, bool>::type is_data_valid(const M &matrix) +{ + return !std::any_of(matrix.begin(), matrix.end(), + [](const T &v) { return is_data_invalid(v); }); +} + +} // namespace munkres_cpp + +#endif /* !defined(__MUNKRES_CPP_UTILS_H__) */