├── CMakeLists.txt ├── LICENSE ├── README.md ├── configs ├── config.yaml ├── homography_001.yaml ├── homography_006.yaml └── kalman_param.yaml ├── images ├── area_cam_001.png ├── area_cam_006.png ├── planview.png └── readme_image.png ├── src ├── apps │ ├── CMakeLists.txt │ └── src │ │ ├── homography_app.cpp │ │ └── multi_camera_tracker.cpp ├── config │ ├── CMakeLists.txt │ ├── include │ │ ├── camera_param.h │ │ ├── configmanager.h │ │ ├── detector_param.h │ │ ├── kalman_param.h │ │ └── yamlmanager.h │ └── src │ │ ├── configmanager.cpp │ │ └── yamlmanager.cpp ├── homography │ ├── CMakeLists.txt │ ├── include │ │ └── homography.h │ └── src │ │ └── homography.cpp ├── object_detection │ ├── CMakeLists.txt │ ├── include │ │ └── object_detector.h │ └── src │ │ └── object_detector.cpp ├── segmentation │ ├── CMakeLists.txt │ ├── include │ │ └── bgsubtraction.h │ └── src │ │ └── bgsubtraction.cpp ├── tracker │ ├── CMakeLists.txt │ ├── include │ │ ├── detection.h │ │ ├── entity.h │ │ ├── hungarianAlg.h │ │ ├── hypothesis.h │ │ ├── kalman.h │ │ ├── lap.h │ │ ├── track.h │ │ └── tracker.h │ └── src │ │ ├── hungarianAlg.cpp │ │ ├── hypothesis.cpp │ │ ├── kalman.cpp │ │ ├── lap.cpp │ │ ├── track.cpp │ │ └── tracker.cpp └── utils │ ├── CMakeLists.txt │ ├── include │ ├── camera.h │ ├── camerastack.h │ ├── drawing.h │ ├── utility.h │ └── utils.h │ └── src │ ├── camera.cpp │ ├── camerastack.cpp │ ├── drawing.cpp │ └── utility.cpp └── videos ├── View_001.mp4 └── View_006.mp4 /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | cmake_policy(SET CMP0060 NEW) 3 | set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -Wall -O3 -g") 4 | 5 | project( mctracker ) 6 | 7 | 8 | find_package(OpenCV REQUIRED) 9 | message(STATUS "OpenCV library status:") 10 | message(STATUS " version: ${OpenCV_VERSION}") 11 | message(STATUS " libraries: ${OpenCV_LIBS}") 12 | message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}") 13 | 14 | find_package(yaml-cpp REQUIRED) 15 | message(STATUS "YAML-CPP library status:") 16 | message(STATUS " include path: ${YAML_CPP_INCLUDE_DIR}") 17 | message(STATUS " libraries: ${YAML_CPP_LIBRARIES}") 18 | 19 | 20 | add_definitions(-Wuninitialized) 21 | add_definitions(-Wreturn-type) 22 | add_definitions(-Wsign-compare) 23 | add_definitions(-Wuninitialized) 24 | add_definitions(-Wunused-function) 25 | add_definitions(-Wunused-label) 26 | add_definitions(-Wunused-value) 27 | add_definitions(-Wunused-variable) 28 | 29 | include(CheckCXXCompilerFlag) 30 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 31 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 32 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 33 | if(COMPILER_SUPPORTS_CXX14) 34 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 35 | elseif(COMPILER_SUPPORTS_CXX11) 36 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 37 | elseif(COMPILER_SUPPORTS_CXX0X) 38 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 39 | else() 40 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 41 | endif() 42 | 43 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 44 | 45 | set(LIBRARY_OUTPUT_PATH ../lib) 46 | set(EXECUTABLE_OUTPUT_PATH ../bin) 47 | 48 | include(${CMAKE_CURRENT_SOURCE_DIR}/src/utils/CMakeLists.txt) 49 | utils() 50 | 51 | include(${CMAKE_CURRENT_SOURCE_DIR}/src/config/CMakeLists.txt) 52 | config() 53 | 54 | include(${CMAKE_CURRENT_SOURCE_DIR}/src/object_detection/CMakeLists.txt) 55 | object_detection() 56 | 57 | include(${CMAKE_CURRENT_SOURCE_DIR}/src/segmentation/CMakeLists.txt) 58 | segmentation() 59 | 60 | include(${CMAKE_CURRENT_SOURCE_DIR}/src/homography/CMakeLists.txt) 61 | homography() 62 | 63 | include(${CMAKE_CURRENT_SOURCE_DIR}/src/tracker/CMakeLists.txt) 64 | tracker() 65 | 66 | include(${CMAKE_CURRENT_SOURCE_DIR}/src/apps//CMakeLists.txt) 67 | applications() 68 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Andrea Pennisi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Multi Camera Tracker (MCTracker) 2 | 3 |

4 | 5 |

6 |
7 | 8 | MCTracker is a multi camera tracker based on homography and costs. The costs are computed according to the Mahalanobis distance. Then, a Linear Assignment approach together with a Munkres algorithm have been used for assigning the current detections to the previous tracks. 9 | 10 | # Requirements 11 | * C++14 12 | * OpenCV 13 | * Darknet ([https://github.com/AlexeyAB/darknet](https://github.com/AlexeyAB/darknet)) 14 | * Boost 15 | * CUDA (not mandatory) 16 | * Yaml-cpp 17 | 18 | # How to build 19 | 20 | 1. Clone *darknet* repository into the main folder (mctracker). 21 | 2. Modify the Makefile in order to use GPU (if any), to use OPENCV, and to build the library: 22 | - GPU=1, LIBSO=1 and OPENCV=1 23 | 3. If necessary, modify the cuda path as well as the architecture (inside the makefile) 24 | 4. Inside the darknet folder, go into the forlder include and edit the file yolo_v2_class.hpp 25 | - Comment Lines 107 and 179 (_#ifdef_ _OPENCV_ and _#endif_) 26 | 5. Compile it with: ```make``` 27 | 6. Download the COCO weights: 28 | - [https://pjreddie.com/media/files/yolov3.weights](https://pjreddie.com/media/files/yolov3.weights) 29 | - Move the downloaded file into the folder *weights* 30 | 7. In the main folder *mctracker*, create a folder build 31 | 8. navigate into the folder build and compile it with the following commands: ```cmake .. && make``` 32 | 33 | # How it works 34 | 35 | 1. To launch the application: ```./multi_camera_tracker ../configs/config.yaml``` 36 | 2. To create a new homography file: ```./homography_app /path/to/the/source/image /path/to/the/destination/image /path/to/yaml/file (where you save the homography)``` 37 | 38 | # LICENSE 39 | MIT 40 | -------------------------------------------------------------------------------- /configs/config.yaml: -------------------------------------------------------------------------------- 1 | Number: 2 #camera number 2 | 3 | #Plan View Params 4 | Planview: ../images/planview.png 5 | Show Planview: false 6 | 7 | #Camera Params 8 | Camera1: ../videos/View_001.mp4 9 | Homography1: ../configs/homography_001.yaml 10 | FOV1: ../images/area_cam_001.png 11 | Proximity1: false 12 | 13 | Camera2: ../videos/View_006.mp4 14 | Homography2: ../configs/homography_006.yaml 15 | FOV2: ../images/area_cam_006.png 16 | Proximity2: true 17 | 18 | #Tracker 19 | Kalman: ../configs/kalman_param.yaml 20 | 21 | #Detector Params 22 | Config: ../darknet/cfg/yolov3.cfg 23 | Weights: ../weights/yolov3.weights 24 | MinThreshold: 0.8 25 | -------------------------------------------------------------------------------- /configs/homography_001.yaml: -------------------------------------------------------------------------------- 1 | Matrix: [3.775749870135431, 17.21097181186204, -986.1518665019161, 0.5492166781072961, 23.11494421915456, -1125.57892913749, 0.0007707595352949053, 0.01702218406881415, 1] 2 | Error: [5.316614] -------------------------------------------------------------------------------- /configs/homography_006.yaml: -------------------------------------------------------------------------------- 1 | Matrix: [-0.6652451349651806, -9.620972855792273, 1144.561469988118, -0.2425185157379307, -10.66978068477464, 1315.254889450699, -0.000266117218104874, -0.01007590124612281, 1] 2 | Error: [5.846917] -------------------------------------------------------------------------------- /configs/kalman_param.yaml: -------------------------------------------------------------------------------- 1 | ACD: 50 #Association Dummy Cost 2 | NHDC: 2 #New Hypothesis Dummy Cost 3 | MINPROPAGATE: 15 #NUMBER AFTER WHICH A TRACK IS ACCEPTED 4 | MAXMISSED: 15 #NUMBER AFTER WHICH A TRACK IS DELETED 5 | DT: 0.5 #INITIAL DT OF THE KALMAN FILTER 6 | -------------------------------------------------------------------------------- /images/area_cam_001.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apennisi/mctracker/29a89046f796f84eac922b2dc9cf4cd6c7aebb6b/images/area_cam_001.png -------------------------------------------------------------------------------- /images/area_cam_006.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apennisi/mctracker/29a89046f796f84eac922b2dc9cf4cd6c7aebb6b/images/area_cam_006.png -------------------------------------------------------------------------------- /images/planview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apennisi/mctracker/29a89046f796f84eac922b2dc9cf4cd6c7aebb6b/images/planview.png -------------------------------------------------------------------------------- /images/readme_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apennisi/mctracker/29a89046f796f84eac922b2dc9cf4cd6c7aebb6b/images/readme_image.png -------------------------------------------------------------------------------- /src/apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(applications) 2 | file(GLOB TRACKER_SRC "src/apps/src/multi_camera_tracker.cpp") 3 | add_executable( multi_camera_tracker ${TRACKER_SRC}) 4 | target_link_libraries( multi_camera_tracker ${OpenCV_LIBS} objectdetector segmentation config utils homography tracker) 5 | 6 | file(GLOB HOMOGRAPHY_SRC "src/apps/src/homography_app.cpp") 7 | add_executable( homography_app ${HOMOGRAPHY_SRC}) 8 | target_link_libraries( homography_app ${OpenCV_LIBS} homography config) 9 | 10 | endfunction() 11 | -------------------------------------------------------------------------------- /src/apps/src/homography_app.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "homography.h" 5 | 6 | std::vector mapPoints; 7 | std::vector rgbPoints; 8 | std::string filename; 9 | 10 | cv::Mat rgb; 11 | cv::Mat map; 12 | 13 | using namespace mctracker::geometry; 14 | 15 | static void add(cv::Mat& img, std::vector& points, int x, int y) 16 | { 17 | cv::circle(img, cv::Point2f(x, y), 3, cv::Scalar(0, 0, 255), -1); 18 | points.push_back(cv::Point2f(x, y)); 19 | } 20 | 21 | static void compute(const std::vector& points_1, const std::vector& points_2, cv::Mat& img_1, cv::Mat& img_2) 22 | { 23 | if(points_1.size() == points_2.size()) 24 | { 25 | Homography h; 26 | h.setPoints(points_1, points_2); 27 | cv::Mat H = h.calcHomography(); 28 | h.draw(img_1, img_2, H); 29 | h.writeToFile(filename); 30 | } 31 | else 32 | { 33 | std::cout << "ERROR!!" << std::endl; 34 | } 35 | } 36 | 37 | void 38 | mouseHandlerRobot(int event, int x, int y, int flags, void *param) 39 | { 40 | if(event == CV_EVENT_LBUTTONDOWN) 41 | { 42 | add(map, mapPoints, x, y); 43 | cv::imshow("map", map); 44 | } 45 | 46 | if(event == CV_EVENT_RBUTTONDOWN) 47 | { 48 | compute(rgbPoints, mapPoints, rgb, map); 49 | } 50 | } 51 | 52 | void mouseHandlerRGB(int event, int x, int y, int flags, void *param) 53 | { 54 | if(event == CV_EVENT_LBUTTONDOWN) 55 | { 56 | add(rgb, rgbPoints, x, y); 57 | cv::imshow("rgb", rgb); 58 | } 59 | 60 | if(event == CV_EVENT_RBUTTONDOWN) 61 | { 62 | compute(rgbPoints, mapPoints, rgb, map); 63 | } 64 | } 65 | 66 | 67 | 68 | int main(int argc, char **argv) { 69 | 70 | if(argc != 4) 71 | { 72 | std::cout << "ERROR: " << argv[0] << " rgb_image map_image save_file_name" << std::endl; 73 | exit(-1); 74 | } 75 | 76 | rgb = cv::imread(argv[1]); 77 | map = cv::imread(argv[2]); 78 | filename = std::string(argv[3]); 79 | 80 | 81 | cv::namedWindow("map", CV_WINDOW_AUTOSIZE); 82 | cv::namedWindow("rgb", CV_WINDOW_AUTOSIZE); 83 | 84 | cv::setMouseCallback("map", &mouseHandlerRobot, NULL); 85 | cv::setMouseCallback("rgb", &mouseHandlerRGB, NULL); 86 | 87 | 88 | cv::imshow("rgb", rgb); 89 | cv::imshow("map", map); 90 | 91 | cv::waitKey(0); 92 | 93 | } 94 | -------------------------------------------------------------------------------- /src/apps/src/multi_camera_tracker.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "object_detector.h" 5 | #include "camerastack.h" 6 | #include "bgsubtraction.h" 7 | #include "track.h" 8 | #include "tracker.h" 9 | #include "kalman_param.h" 10 | #include "object_detector.h" 11 | #include "configmanager.h" 12 | #include "homography.h" 13 | #include "utility.h" 14 | 15 | 16 | using namespace mctracker; 17 | using namespace mctracker::config; 18 | using namespace mctracker::utils; 19 | using namespace mctracker::objectdetection; 20 | using namespace mctracker::segmentation; 21 | 22 | 23 | auto main(int argc, char **argv) -> int 24 | { 25 | if(argc != 2) 26 | { 27 | std::cout << "Error: too few/much arguments!" << std::endl; 28 | std::cout << "Usage: " << argv[0] << " /path/to/the/config/file" << std::endl; 29 | exit(-1); 30 | } 31 | 32 | ConfigManager config; 33 | config.read(std::string(argv[1])); 34 | config.print(); 35 | 36 | const int w = config.getPlaview().cols; 37 | const int h = config.getPlaview().rows; 38 | 39 | //initialize all the main objects 40 | CameraStack streams(config.getCameraParam()); 41 | ObjectDetector detector(config.getDetectorParam()); 42 | std::vector bgSub(config.getCameraNumber()); 43 | Tracker tr(config.getKalmanParam(), streams.getCameraStack()); 44 | 45 | //set tracker space 46 | tr.setSize(w, h); 47 | 48 | //storing variables 49 | std::vector frames; 50 | std::vector detectionFrames(config.getCameraNumber()); 51 | std::vector trackingFrames(config.getCameraNumber()); 52 | std::vector fgMasks(config.getCameraNumber()); 53 | std::vector< std::vector > detections(config.getCameraNumber()); 54 | bool compute = false; 55 | cv::Mat imageTracks; 56 | 57 | auto& cameras = streams.getCameraStack(); 58 | 59 | while(streams.getFrame(frames)) 60 | { 61 | imageTracks = config.getPlaview().clone(); 62 | 63 | std::stringstream ss; 64 | auto i = 0; 65 | 66 | for(const auto& frame : frames) 67 | { 68 | detectionFrames.at(i) = frame.clone(); 69 | trackingFrames.at(i) = frame.clone(); 70 | cv::Mat fgMask; 71 | bgSub.at(i).process(frame); 72 | compute = bgSub.at(i).getFgMask(fgMask); 73 | if(compute) 74 | { 75 | fgMasks.at(i) = fgMask.clone(); 76 | detector.classify(detectionFrames.at(i), false); 77 | detections.at(i) = detector.detections(); 78 | } 79 | i++; 80 | } 81 | 82 | if(compute) 83 | { 84 | auto observations = 85 | Utility::dets2Obs(detections, frames, fgMasks, cameras); 86 | 87 | tr.track(observations, w, h); 88 | 89 | const auto& tracks = tr.getTracks(); 90 | for(auto& track : tracks) 91 | { 92 | track->drawTrack(trackingFrames, cameras); 93 | if(config.showPlanView()) 94 | track->drawTrackPlanView(imageTracks); 95 | } 96 | 97 | cv::imshow("TRACKING", Utility::makeMosaic(trackingFrames)); 98 | if(config.showPlanView()) 99 | cv::imshow("TRACKS", imageTracks); 100 | 101 | cv::waitKey(1); 102 | } 103 | } 104 | return 0; 105 | } 106 | -------------------------------------------------------------------------------- /src/config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(config) 2 | include_directories(${PROJECT_BINARY_DIR}/../src/config/include) 3 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../src/confg/include) 4 | 5 | file(GLOB_RECURSE CONFIG_SRC "src/config/src/*.cpp") 6 | 7 | add_library(config SHARED ${CONFIG_SRC}) 8 | target_link_libraries(config ${OpenCV_LIBS} ${YAML_CPP_LIBRARIES}) 9 | 10 | endfunction() 11 | -------------------------------------------------------------------------------- /src/config/include/camera_param.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _CAMERA_PARAM_H_ 6 | #define _CAMERA_PARAM_H_ 7 | 8 | #include 9 | 10 | namespace mctracker 11 | { 12 | namespace config 13 | { 14 | class CameraParam 15 | { 16 | public: 17 | /** 18 | * @brief Constructor class KalmanParam 19 | */ 20 | CameraParam() { ; } 21 | 22 | /** 23 | * @brief set the path to the stream device 24 | * @param s string containing the path 25 | */ 26 | void 27 | setStream(const std::string& s) 28 | { 29 | stream = s; 30 | } 31 | 32 | /** 33 | * @brief get the path to the stream device 34 | * @return string containing the path 35 | */ 36 | inline const std::string 37 | getStream() const 38 | { 39 | return stream; 40 | } 41 | 42 | /** 43 | * @brief set the path to the homography file 44 | * @param h string containing the path 45 | */ 46 | void 47 | setHomography(const std::string& h) 48 | { 49 | homography = h; 50 | } 51 | 52 | /** 53 | * @brief get the path to the homography file 54 | * @return a string containing the path 55 | */ 56 | inline const std::string 57 | getHomography() const 58 | { 59 | return homography; 60 | } 61 | 62 | /** 63 | * @brief set the path to the fov image 64 | * @param fov string containing the path 65 | */ 66 | void 67 | setFOV(const std::string& fov) 68 | { 69 | fieldOfView = fov; 70 | } 71 | 72 | /** 73 | * @brief set the path to the fov image 74 | * @return string containing the path 75 | */ 76 | inline const std::string 77 | getFOV() const 78 | { 79 | return fieldOfView; 80 | } 81 | 82 | /** 83 | * @brief set if a camera is close to the scene to monitor 84 | * @param prox a boolean value where true means that the camera is close to the scene, false otherwise 85 | */ 86 | void 87 | setProximity(const bool prox) 88 | { 89 | proximity = prox; 90 | } 91 | 92 | /** 93 | * @brief get the proximity valuer 94 | * @return a boolean value where true means that the camera is close to the scene, false otherwise 95 | */ 96 | inline const float 97 | getProximity() const 98 | { 99 | return proximity; 100 | } 101 | 102 | /** 103 | * @brief copy operator 104 | * @param _param object to copy 105 | * @return the pointer to the current instance 106 | */ 107 | CameraParam& operator=(const CameraParam& _param) 108 | { 109 | stream = _param.getStream(); 110 | homography = _param.getHomography(); 111 | fieldOfView = _param.getFOV(); 112 | proximity = _param.getProximity(); 113 | return *this; 114 | } 115 | 116 | /** 117 | * @brief print all the parameters 118 | */ 119 | void 120 | print() 121 | { 122 | std::cout << "[STREAM]: " << stream << std::endl; 123 | std::cout << "[HOMOGRAPHY]: " << homography << std::endl; 124 | std::cout << "[FOV]: " << fieldOfView << std::endl; 125 | std::cout << "[PROXIMITY]: " << proximity << std::endl; 126 | } 127 | 128 | private: 129 | std::string stream; 130 | std::string homography; 131 | std::string fieldOfView; 132 | bool proximity; 133 | }; 134 | } 135 | } 136 | 137 | #endif 138 | -------------------------------------------------------------------------------- /src/config/include/configmanager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _CONFIG_MANAGER_H_ 6 | #define _CONFIG_MANAGER_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include "detector_param.h" 13 | #include "camera_param.h" 14 | #include "kalman_param.h" 15 | #include "yamlmanager.h" 16 | 17 | namespace mctracker 18 | { 19 | namespace config 20 | { 21 | class ConfigManager 22 | { 23 | public: 24 | /** 25 | * @brief Constructor class ConfigManager 26 | */ 27 | ConfigManager() { ; } 28 | public: 29 | /** 30 | * @brief read all the parameters included in a configuration files 31 | * @param config a string containing the path to the configuration file 32 | * @return true if the configuration file is correctly readed, false otherwise 33 | */ 34 | bool read(const std::string& config); 35 | /** 36 | * @brief print all the values read from the configuration file 37 | */ 38 | void print(); 39 | public: 40 | /** 41 | * @brief get the number of cameras of the system specified in the file 42 | * @return the the number of cameras of the system specified in the file 43 | */ 44 | inline const int 45 | getCameraNumber() const 46 | { 47 | return cameraNum; 48 | } 49 | 50 | /** 51 | * @brief get the kalman parameters specified in the configuration file 52 | * @return the kalman parameters specified in the configuration file 53 | */ 54 | inline const KalmanParam 55 | getKalmanParam() const 56 | { 57 | return kalmanParam; 58 | } 59 | 60 | /** 61 | * @brief get the detector parameters specified in the configuration file 62 | * @return the detector parameters specified in the configuration file 63 | */ 64 | inline const DetectorParam 65 | getDetectorParam() const 66 | { 67 | return detectorParam; 68 | } 69 | 70 | /** 71 | * @brief get the camera parameters specified in the configuration file 72 | * @return the camera parameters specified in the configuration file 73 | */ 74 | inline const std::vector 75 | getCameraParam() const 76 | { 77 | return cameraParam; 78 | } 79 | 80 | /** 81 | * @brief get the plan view of the monitored environment 82 | * @return a cv::Mat containing the image of the plan view 83 | */ 84 | inline const cv::Mat 85 | getPlaview() const 86 | { 87 | return planView; 88 | } 89 | 90 | /** 91 | * @brief get if the planview has to be showd 92 | * @return a bool value: true if the planview has to be showed, false otherwise 93 | */ 94 | inline const bool 95 | showPlanView() const 96 | { 97 | return show; 98 | } 99 | private: 100 | /** 101 | * @brief check if a file exists on the hd 102 | * @return true if exists, false otherwise 103 | */ 104 | bool exist(const std::string& name) const; 105 | private: 106 | KalmanParam kalmanParam; 107 | DetectorParam detectorParam; 108 | std::vector cameraParam; 109 | YamlManager yamlManager; 110 | cv::Mat planView; 111 | bool show; 112 | int cameraNum; 113 | }; 114 | } 115 | } 116 | 117 | #endif 118 | -------------------------------------------------------------------------------- /src/config/include/detector_param.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _DETECTOR_PARAM_H_ 6 | #define _DETECTOR_PARAM_H_ 7 | 8 | namespace mctracker 9 | { 10 | namespace config 11 | { 12 | class DetectorParam 13 | { 14 | public: 15 | /** 16 | * @brief Constructor class DetectorParam 17 | */ 18 | DetectorParam() { ; } 19 | 20 | /** 21 | * @brief set the path to the weight file 22 | * @param w path to the weight file 23 | */ 24 | void 25 | setWeights(const std::string& w) 26 | { 27 | weights = w; 28 | } 29 | 30 | /** 31 | * @brief get the path to the file containing the weights 32 | * @return a string containing the path 33 | */ 34 | inline const std::string 35 | getWeights() const 36 | { 37 | return weights; 38 | } 39 | 40 | /** 41 | * @brief set the path to the file containing the network configuration 42 | * @param c value containing the path to the config file 43 | */ 44 | void 45 | setConfig(const std::string& c) 46 | { 47 | config = c; 48 | } 49 | 50 | /** 51 | * @brief get the path to the file containing the network configuration 52 | * @return a string containing the path to the file containing the network configuration 53 | */ 54 | inline const std::string 55 | getConfig() const 56 | { 57 | return config; 58 | } 59 | 60 | /** 61 | * @brief set the minimum threshold above which a detection is considered 62 | * @param thres value contianing the minimum threshold above which a detection is considered 63 | */ 64 | void 65 | setThreshold(const float& thres) 66 | { 67 | minThreshold = thres; 68 | } 69 | 70 | /** 71 | * @brief get the minimum threshold above which a detection is considered 72 | * @return a float containing the minimum threshold above which a detection is considered 73 | */ 74 | inline const float 75 | getThreshold() const 76 | { 77 | return minThreshold; 78 | } 79 | 80 | /** 81 | * @brief copy operator 82 | * @param _param object to copy 83 | * @return the pointer to the current instance 84 | */ 85 | DetectorParam& operator=(const DetectorParam& _param) 86 | { 87 | config = _param.getConfig(); 88 | weights = _param.getWeights(); 89 | minThreshold = _param.getThreshold(); 90 | return *this; 91 | } 92 | 93 | /** 94 | * @brief print all the parameters 95 | */ 96 | void 97 | print() 98 | { 99 | std::cout << "[CONFIG]: " << config << std::endl; 100 | std::cout << "[WEIGHTS]: " << weights << std::endl; 101 | std::cout << "[THRESHOLD]: " << minThreshold << std::endl; 102 | } 103 | 104 | private: 105 | std::string config; 106 | std::string weights; 107 | float minThreshold; 108 | }; 109 | } 110 | } 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /src/config/include/kalman_param.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _KALMAN_PARAM_H_ 6 | #define _KALMAN_PARAM_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace mctracker 14 | { 15 | namespace config 16 | { 17 | class KalmanParam 18 | { 19 | public: 20 | /** 21 | * @brief Constructor class KalmanParam 22 | */ 23 | KalmanParam() { ; } 24 | 25 | /** 26 | * @brief set the number of times after which a track is accepted 27 | * @param propagate unsigned integer containing the value of the propagation 28 | */ 29 | void 30 | setMinPropagate(uint propagate) 31 | { 32 | min_propagate = propagate; 33 | } 34 | 35 | /** 36 | * @brief get the number of times after which a track is accepted 37 | * @return the number of times after which a track is accepted 38 | */ 39 | inline const uint 40 | getMinpropagate() const 41 | { 42 | return min_propagate; 43 | } 44 | 45 | /** 46 | * @brief set the dt value for the transition matrix 47 | * @param dt float containing the value of the sample frequency 48 | */ 49 | void 50 | setDt(const float& dt) 51 | { 52 | d_t = dt; 53 | } 54 | 55 | /** 56 | * @brief get the dt value of the transition matrix 57 | * @return the dt value 58 | */ 59 | inline const float 60 | getDt() const 61 | { 62 | return d_t; 63 | } 64 | 65 | /** 66 | * @brief get the association dummy cost value for the lienar assignment cost 67 | * @return the value of the association cost 68 | */ 69 | inline const uint 70 | getAssocdummycost() const 71 | { 72 | return assoc_dummy_cost; 73 | } 74 | 75 | /** 76 | * @brief set the association dummy cost for the lienar assignment cost 77 | * @param ass unsigned integer containing the association value 78 | */ 79 | void 80 | setAssociationDummyCost(uint ass) 81 | { 82 | assoc_dummy_cost = ass; 83 | } 84 | 85 | /** 86 | * @brief set the dummy cost for new hypothesis in the linear assignment cost 87 | * @param nydc value containing the cost 88 | */ 89 | void 90 | setNewHypDummyCost(uint nydc) 91 | { 92 | new_hyp_dummy_cost = nydc; 93 | } 94 | 95 | /** 96 | * @brief get the dymmy cost for the new hypothesis in the linear assignment cost 97 | * @return the value for the new hypothesis cost 98 | */ 99 | inline const uint 100 | getNewhypdummycost() const 101 | { 102 | return new_hyp_dummy_cost; 103 | } 104 | 105 | /** 106 | * @brief set the maximum number of times after which a track is deleted 107 | * @param missed the value containing the maximum number of times after which a track is deleted 108 | */ 109 | void 110 | setMaxMissed(uint missed) 111 | { 112 | max_missed = missed; 113 | } 114 | 115 | /** 116 | * @brief get the maximum number of times after which a track is deleted 117 | * @return the value containing the maximum number of times after which a track is deleted 118 | */ 119 | inline const uint 120 | getMaxmissed() const 121 | { 122 | return max_missed; 123 | } 124 | 125 | /** 126 | * @brief copy operator 127 | * @param _param object to copy 128 | * @return the pointer to the current instance 129 | */ 130 | KalmanParam& operator=(const KalmanParam& _param) 131 | { 132 | this->min_propagate = _param.getMinpropagate(); 133 | this->d_t = _param.getDt(); 134 | this->assoc_dummy_cost = _param.getAssocdummycost(); 135 | this->new_hyp_dummy_cost = _param.getNewhypdummycost(); 136 | this->max_missed = _param.getMaxmissed(); 137 | return *this; 138 | } 139 | 140 | /** 141 | * @brief print all the parameters 142 | */ 143 | void 144 | print() 145 | { 146 | std::cout << "[MINPROPAGATE]: " << min_propagate << std::endl; 147 | std::cout << "[MAXMISSED]: " << max_missed << std::endl; 148 | std::cout << "[ASSOCDUMMYCOST]: " << assoc_dummy_cost << std::endl; 149 | std::cout << "[NEW_HYP_DUMMY_COST]: " << new_hyp_dummy_cost << std::endl; 150 | std::cout << "[DT]: " << d_t << std::endl; 151 | } 152 | private: 153 | uint max_missed; 154 | uint min_propagate; 155 | float d_t; 156 | uint assoc_dummy_cost; 157 | uint new_hyp_dummy_cost; 158 | }; 159 | } 160 | } 161 | 162 | #endif 163 | -------------------------------------------------------------------------------- /src/config/include/yamlmanager.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _YAML_MANAGER_H_ 6 | #define _YAML_MANAGER_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace mctracker 16 | { 17 | namespace config 18 | { 19 | class YamlManager 20 | { 21 | public: 22 | /** 23 | * @brief Constructor class YamlManager 24 | */ 25 | YamlManager(); 26 | /** 27 | * @brief Write a new yaml file 28 | * @param filepath the path where you have to save your file, including the filename 29 | * @return true if the file is correctly written, false otherwise 30 | */ 31 | bool write(const std::string& filepath); 32 | /** 33 | * @brief Read a yaml file 34 | * @param filepath the path where your file is stored 35 | * @return true if the fiel is correctly read, false otherwise 36 | */ 37 | bool read(const std::string& filepath); 38 | /** 39 | * @brief Read an integer element from the yaml file given the key 40 | * @param name key to read 41 | * @param elem variable where the value is stored 42 | * @return true if the element exists and is correctly read, false otherwise 43 | */ 44 | bool getElem(const std::string& name, int& elem); 45 | /** 46 | * @brief Read a float element from the yaml file given the key 47 | * @param name key to read 48 | * @param elem variable where the value is stored 49 | * @return true if the element exists and is correctly read, false otherwise 50 | */ 51 | bool getElem(const std::string& name, float& elem); 52 | /** 53 | * @brief Read a double element from the yaml file given the key 54 | * @param name key to read 55 | * @param elem variable where the value is stored 56 | * @return true if the element exists and is correctly read, false otherwise 57 | */ 58 | bool getElem(const std::string& name, double& elem); 59 | /** 60 | * @brief Read a boolean element from the yaml file given the key 61 | * @param name key to read 62 | * @param elem variable where the value is stored 63 | * @return true if the element exists and is correctly read, false otherwise 64 | */ 65 | bool getElem(const std::string& name, bool& elem); 66 | /** 67 | * @brief Read a cv::Point2i element from the yaml file given the key 68 | * @param name key to read 69 | * @param elem variable where the value is stored 70 | * @return true if the element exists and is correctly read, false otherwise 71 | */ 72 | bool getElem(const std::string& name, cv::Point2i& elem); 73 | /** 74 | * @brief Read a cv::Point2f element from the yaml file given the key 75 | * @param name key to read 76 | * @param elem variable where the value is stored 77 | * @return true if the element exists and is correctly read, false otherwise 78 | */ 79 | bool getElem(const std::string& name, cv::Point2f& elem); 80 | /** 81 | * @brief Read a cv::Point2d element from the yaml file given the key 82 | * @param name key to read 83 | * @param elem variable where the value is stored 84 | * @return true if the element exists and is correctly read, false otherwise 85 | */ 86 | bool getElem(const std::string& name, cv::Point2d& elem); 87 | /** 88 | * @brief Read a string element from the yaml file given the key 89 | * @param name key to read 90 | * @param elem variable where the value is stored 91 | * @return true if the element exists and is correctly read, false otherwise 92 | */ 93 | bool getElem(const std::string& name, std::string& elem); 94 | /** 95 | * @brief Read a vector of integers from the yaml file given the key 96 | * @param name key to read 97 | * @param elem variable where the value is stored 98 | * @return true if the element exists and is correctly read, false otherwise 99 | */ 100 | bool getElem(const std::string& name, std::vector& elem); 101 | /** 102 | * @brief Read a vector of boolean from the yaml file given the key 103 | * @param name key to read 104 | * @param elem variable where the value is stored 105 | * @return true if the element exists and is correctly read, false otherwise 106 | */ 107 | bool getElem(const std::string& name, std::vector& elem); 108 | /** 109 | * @brief Read a vector of floats from the yaml file given the key 110 | * @param name key to read 111 | * @param elem variable where the value is stored 112 | * @return true if the element exists and is correctly read, false otherwise 113 | */ 114 | bool getElem(const std::string& name, std::vector& elem); 115 | /** 116 | * @brief Read a vector of doubles from the yaml file given the key 117 | * @param name key to read 118 | * @param elem variable where the value is stored 119 | * @return true if the element exists and is correctly read, false otherwise 120 | */ 121 | bool getElem(const std::string& name, std::vector& elem); 122 | /** 123 | * @brief Read a vector of cv::Point from the yaml file given the key 124 | * @param name key to read 125 | * @param elem variable where the value is stored 126 | * @return true if the element exists and is correctly read, false otherwise 127 | */ 128 | bool getElem(const std::string& name, std::vector& elem); 129 | /** 130 | * @brief Read a vector of cv::Point2f from the yaml file given the key 131 | * @param name key to read 132 | * @param elem variable where the value is stored 133 | * @return true if the element exists and is correctly read, false otherwise 134 | */ 135 | bool getElem(const std::string& name, std::vector& elem); 136 | /** 137 | * @brief Read a cv::Mat from the yaml file given the key 138 | * @param name key to read 139 | * @param elem variable where the value is stored 140 | * @param type the type of cv::Mat: CV_FC321 or CV_FC64U1 141 | * @return true if the element exists and is correctly read, false otherwise 142 | */ 143 | bool getElem(const std::string& name, cv::Mat& elem, int type); 144 | /** 145 | * @brief Write a float into the yaml file given the key 146 | * @param name key to write 147 | * @param elem variable to write 148 | * @return true if the is correctly written, false otherwise 149 | */ 150 | void writeElem(const std::string& key, const float& elem); 151 | /** 152 | * @brief Write a cv::Point into the yaml file given the key 153 | * @param name key to write 154 | * @param elem variable to write 155 | * @return true if the is correctly written, false otherwise 156 | */ 157 | void writeElem(const std::string& key, const cv::Point& elem); 158 | /** 159 | * @brief Write a cv::Mat into the yaml file given the key 160 | * @param name key to write 161 | * @param elem variable to write 162 | * @return true if the is correctly written, false otherwise 163 | */ 164 | void writeElem(const std::string& key, const cv::Mat& elem); 165 | /** 166 | * @brief Check if a key exists 167 | * @param name name of the key to check 168 | * @return true if exists, false otherwise 169 | */ 170 | bool keyExists(const std::string& key); 171 | 172 | private: 173 | /** 174 | * @brief Check if a file exists on your local HD 175 | * @param name path of the file name to check 176 | * @return true if exists, false otherwise 177 | */ 178 | inline bool 179 | exists(const std::string& name) const 180 | { 181 | if(name.find("://")!=std::string::npos) return true; 182 | struct stat buffer; 183 | return (stat (name.c_str(), &buffer) == 0); 184 | } 185 | 186 | private: 187 | YAML::Emitter out; 188 | YAML::Node in; 189 | }; 190 | } 191 | } 192 | 193 | 194 | #endif 195 | -------------------------------------------------------------------------------- /src/config/src/configmanager.cpp: -------------------------------------------------------------------------------- 1 | #include "configmanager.h" 2 | 3 | using namespace mctracker::config; 4 | 5 | bool 6 | ConfigManager::read(const std::string& config) 7 | { 8 | if(!exist(config)) 9 | { 10 | throw std::invalid_argument("Invalid file name: " + config); 11 | } 12 | yamlManager.read(config); 13 | 14 | if(!yamlManager.getElem("Number", cameraNum)) 15 | { 16 | std::cout << "Camera Number is not specified!" << std::endl; 17 | return false; 18 | } 19 | 20 | std::string planViewName; 21 | if(!yamlManager.getElem("Planview", planViewName)) 22 | { 23 | std::cout << "Planview is not specified!" << std::endl; 24 | return false; 25 | } 26 | 27 | planView = cv::imread(planViewName); 28 | 29 | if(!yamlManager.getElem("Show Planview", show)) 30 | { 31 | show = false; 32 | } 33 | 34 | std::stringstream ss; 35 | for(auto i = 0; i < cameraNum; ++i) 36 | { 37 | CameraParam param; 38 | ss << "Camera" << i+1; 39 | std::string tmpVar; 40 | if(!yamlManager.getElem(ss.str(), tmpVar)) 41 | { 42 | std::cout << "Camera"<< i+1 << " Stream is not specified!" << std::endl; 43 | return false; 44 | } 45 | param.setStream(tmpVar); 46 | 47 | ss.str(""); 48 | ss << "Homography" << i+1; 49 | if(!yamlManager.getElem(ss.str(), tmpVar)) 50 | { 51 | std::cout << "Homography"<< i+1 << " is not specified!" << std::endl; 52 | return false; 53 | } 54 | param.setHomography(tmpVar); 55 | 56 | ss.str(""); 57 | ss << "FOV" << i+1; 58 | if(!yamlManager.getElem(ss.str(), tmpVar)) 59 | { 60 | std::cout << "FOV"<< i+1 << " is not specified!" << std::endl; 61 | return false; 62 | } 63 | param.setFOV(tmpVar); 64 | 65 | ss.str(""); 66 | ss << "Proximity" << i+1; 67 | bool prox; 68 | if(!yamlManager.getElem(ss.str(), prox)) 69 | { 70 | std::cout << "Proximity"<< i+1 << " is not specified!" << std::endl; 71 | return false; 72 | } 73 | ss.str(""); 74 | param.setProximity(prox); 75 | cameraParam.push_back(param); 76 | } 77 | 78 | 79 | std::string kalman_file; 80 | if(!yamlManager.getElem("Kalman", kalman_file)) 81 | { 82 | std::cout << "No Kalman file found, predefined values will be use." << std::endl; 83 | kalmanParam.setAssociationDummyCost(50); 84 | kalmanParam.setNewHypDummyCost(2); 85 | kalmanParam.setDt(1.0); 86 | kalmanParam.setMaxMissed(15); 87 | kalmanParam.setMinPropagate(15); 88 | } 89 | else 90 | { 91 | if(!exist(kalman_file)) 92 | { 93 | throw std::invalid_argument("Invalid file name: " + kalman_file); 94 | } 95 | 96 | YamlManager kalmanReader; 97 | kalmanReader.read(kalman_file); 98 | 99 | 100 | int tmpValue; 101 | if(!kalmanReader.getElem("ACD", tmpValue)) 102 | { 103 | kalmanParam.setAssociationDummyCost(50); 104 | } 105 | kalmanParam.setAssociationDummyCost(tmpValue); 106 | 107 | if(!kalmanReader.getElem("NHDC", tmpValue)) 108 | { 109 | kalmanParam.setNewHypDummyCost(2); 110 | } 111 | kalmanParam.setNewHypDummyCost(uint(tmpValue) ); 112 | 113 | if(!kalmanReader.getElem("MINPROPAGATE", tmpValue)) 114 | { 115 | kalmanParam.setMinPropagate(15); 116 | } 117 | kalmanParam.setMinPropagate(uint(tmpValue) ); 118 | 119 | if(!kalmanReader.getElem("MAXMISSED", tmpValue)) 120 | { 121 | kalmanParam.setMaxMissed(15); 122 | } 123 | kalmanParam.setMaxMissed(uint(tmpValue) ); 124 | 125 | float dt; 126 | if(!kalmanReader.getElem("DT", dt)) 127 | { 128 | kalmanParam.setMaxMissed(1.0); 129 | } 130 | kalmanParam.setDt( dt ); 131 | } 132 | 133 | std::string detectorTmpVal; 134 | 135 | if(!yamlManager.getElem("Config", detectorTmpVal)) 136 | { 137 | std::cout << "Config is not specified!" << std::endl; 138 | return false; 139 | } 140 | 141 | if(!exist(detectorTmpVal)) 142 | { 143 | throw std::invalid_argument("Invalid file name: " + detectorTmpVal); 144 | } 145 | 146 | detectorParam.setConfig(detectorTmpVal); 147 | 148 | if(!yamlManager.getElem("Weights", detectorTmpVal)) 149 | { 150 | std::cout << "Weights is not specified!" << std::endl; 151 | return false; 152 | } 153 | 154 | if(!exist(detectorTmpVal)) 155 | { 156 | throw std::invalid_argument("Invalid file name: " + detectorTmpVal); 157 | } 158 | 159 | detectorParam.setWeights(detectorTmpVal); 160 | 161 | float thresh; 162 | 163 | if(!yamlManager.getElem("MinThreshold", thresh)) 164 | { 165 | thresh = .8; 166 | } 167 | 168 | detectorParam.setThreshold(thresh); 169 | 170 | return true; 171 | } 172 | 173 | void 174 | ConfigManager::print() 175 | { 176 | auto i = 0; 177 | for(auto& param : cameraParam) 178 | { 179 | std::cout << "CAMERA " << i+1 << std::endl; 180 | param.print(); 181 | i++; 182 | std::cout << std::endl; 183 | } 184 | 185 | std::cout << std::endl; 186 | std::cout << "KALMAN" << std::endl; 187 | kalmanParam.print(); 188 | 189 | std::cout << std::endl; 190 | std::cout << "DETECTOR" << std::endl; 191 | detectorParam.print(); 192 | } 193 | 194 | bool 195 | ConfigManager::exist(const std::string& name) const 196 | { 197 | if(name.find("://")!=std::string::npos) return true; 198 | 199 | struct stat buffer; 200 | return (stat (name.c_str(), &buffer) == 0); 201 | } 202 | -------------------------------------------------------------------------------- /src/config/src/yamlmanager.cpp: -------------------------------------------------------------------------------- 1 | #include "yamlmanager.h" 2 | 3 | using namespace mctracker::config; 4 | 5 | YamlManager 6 | ::YamlManager() 7 | { 8 | out << YAML::BeginMap; 9 | } 10 | 11 | bool 12 | YamlManager::keyExists(const std::string& key) 13 | { 14 | if(in.IsDefined()) 15 | { 16 | if(in[key]) 17 | { 18 | return true; 19 | } 20 | } 21 | else 22 | { 23 | throw std::invalid_argument("File YAML is not open!"); 24 | } 25 | return false; 26 | } 27 | 28 | 29 | 30 | bool 31 | YamlManager::write(const std::string& filepath) 32 | { 33 | out << YAML::EndMap; 34 | std::ofstream fout(filepath); 35 | try 36 | { 37 | fout << out.c_str(); 38 | } 39 | catch(const std::runtime_error& e) 40 | { 41 | std::cout << "ERROR: Cannot write the yaml file: " << filepath << std::endl; 42 | return false; 43 | } 44 | fout.close(); 45 | return true; 46 | } 47 | 48 | bool 49 | YamlManager::read(const std::string& filepath) 50 | { 51 | if(!exists(filepath)) 52 | { 53 | throw std::invalid_argument("The File " + filepath + " does not exist!!"); 54 | exit(-1); 55 | } 56 | try 57 | { 58 | in = YAML::LoadFile(filepath); 59 | } 60 | catch(const std::runtime_error& e) 61 | { 62 | std::cout << "ERROR: Cannot read the yaml file: " << filepath << std::endl; 63 | return false; 64 | } 65 | return true; 66 | } 67 | 68 | void 69 | YamlManager::writeElem(const std::string& key, const cv::Mat& elem) 70 | { 71 | try 72 | { 73 | out << YAML::Key << key; 74 | } 75 | catch(const std::runtime_error& e) 76 | { 77 | std::cout << "ERROR: Cannot write the key: " << key << std::endl; 78 | } 79 | 80 | 81 | auto wirteMatrix = [](auto vec, YAML::Emitter& out) 82 | { 83 | try 84 | { 85 | out << YAML::Flow << vec; 86 | } 87 | catch(const std::runtime_error& e) 88 | { 89 | std::cout << "ERROR: Cannot write the matrix" << std::endl; 90 | return false; 91 | } 92 | return true; 93 | }; 94 | 95 | const int depth = elem.type(); 96 | 97 | switch (depth) 98 | { 99 | case CV_32FC1: 100 | { 101 | std::vector vec(elem.begin(), elem.end()); 102 | if(!wirteMatrix(vec, out)) 103 | throw std::invalid_argument("ERROR: Cannot write the Key : " + key); 104 | break; 105 | } 106 | case CV_64FC1: 107 | { 108 | std::vector vec(elem.begin(), elem.end()); 109 | if(!wirteMatrix(vec, out)) 110 | throw std::invalid_argument("ERROR: Cannot write the Key : " + key); 111 | break; 112 | } 113 | 114 | default: 115 | throw std::invalid_argument("Invalid depth type of matrix!"); 116 | } 117 | } 118 | 119 | 120 | void 121 | YamlManager::writeElem(const std::string& key, const float& elem) 122 | { 123 | try 124 | { 125 | out << YAML::Key << key; 126 | } 127 | catch(const std::runtime_error& e) 128 | { 129 | std::cout << "ERROR: Cannot write the key: " << key << std::endl; 130 | } 131 | try 132 | { 133 | out << YAML::Flow; 134 | out << YAML::BeginSeq << elem << YAML::EndSeq; 135 | } 136 | catch(const std::runtime_error& e) 137 | { 138 | std::cout << "ERROR: Cannot write the elem: " << elem << std::endl; 139 | } 140 | 141 | } 142 | 143 | void 144 | YamlManager::writeElem(const std::string& key, const cv::Point& elem) 145 | { 146 | try 147 | { 148 | out << YAML::Key << key; 149 | } 150 | catch(const std::runtime_error& e) 151 | { 152 | std::cout << "ERROR: Cannot write the key: " << key << std::endl; 153 | } 154 | try 155 | { 156 | out << YAML::Flow; 157 | out << YAML::BeginSeq << elem.x << elem.y << YAML::EndSeq; 158 | } 159 | catch(const std::runtime_error& e) 160 | { 161 | std::cout << "ERROR: Cannot write the elem: " << elem << std::endl; 162 | } 163 | 164 | } 165 | 166 | bool 167 | YamlManager::getElem(const std::string& name, bool& elem) 168 | { 169 | if(in[name]) 170 | { 171 | try 172 | { 173 | elem = in[name].as(); 174 | } 175 | catch(const std::runtime_error& e) 176 | { 177 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 178 | return false; 179 | } 180 | } 181 | else 182 | { 183 | return false; 184 | } 185 | return true; 186 | } 187 | 188 | 189 | bool 190 | YamlManager::getElem(const std::string& name, int& elem) 191 | { 192 | if(in[name]) 193 | { 194 | try 195 | { 196 | elem = in[name].as(); 197 | } 198 | catch(const std::runtime_error& e) 199 | { 200 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 201 | return false; 202 | } 203 | } 204 | else 205 | { 206 | return false; 207 | } 208 | return true; 209 | } 210 | 211 | 212 | bool 213 | YamlManager::getElem(const std::string& name, float& elem) 214 | { 215 | if(in[name]) 216 | { 217 | try 218 | { 219 | elem = in[name].as(); 220 | } 221 | catch(const std::runtime_error& e) 222 | { 223 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 224 | return false; 225 | } 226 | } 227 | else 228 | { 229 | return false; 230 | } 231 | return true; 232 | } 233 | 234 | bool 235 | YamlManager::getElem(const std::string& name, double& elem) 236 | { 237 | if(in[name]) 238 | { 239 | try 240 | { 241 | elem = in[name].as(); 242 | } 243 | catch(const std::runtime_error& e) 244 | { 245 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 246 | return false; 247 | } 248 | } 249 | else 250 | { 251 | return false; 252 | } 253 | return true; 254 | } 255 | 256 | 257 | bool 258 | YamlManager::getElem(const std::string& name, std::string& elem) 259 | { 260 | if(in[name]) 261 | { 262 | try 263 | { 264 | elem = in[name].as(); 265 | } 266 | catch(const std::runtime_error& e) 267 | { 268 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 269 | return false; 270 | } 271 | } 272 | else 273 | { 274 | return false; 275 | } 276 | return true; 277 | } 278 | 279 | bool 280 | YamlManager::getElem(const std::string& name, std::vector& elem) 281 | { 282 | if(in[name] && in[name].IsSequence()) 283 | { 284 | uint i = 0; 285 | for(const auto& e : in[name]) 286 | { 287 | try 288 | { 289 | elem.push_back(e.as()); 290 | } 291 | catch(const std::runtime_error& e) 292 | { 293 | std::cout << "ERROR: Cannot read element " << i << e.what() << std::endl; 294 | return false; 295 | } 296 | } 297 | } 298 | else 299 | { 300 | return true; 301 | } 302 | return false; 303 | } 304 | 305 | bool 306 | YamlManager::getElem(const std::string& name, std::vector& elem) 307 | { 308 | if(in[name] && in[name].IsSequence()) 309 | { 310 | uint i = 0; 311 | for(const auto& e : in[name]) 312 | { 313 | try 314 | { 315 | elem.push_back(e.as()); 316 | } 317 | catch(const std::runtime_error& e) 318 | { 319 | std::cout << "ERROR: Cannot read element " << i << e.what() << std::endl; 320 | return false; 321 | } 322 | } 323 | } 324 | else 325 | { 326 | return true; 327 | } 328 | return false; 329 | } 330 | 331 | 332 | bool 333 | YamlManager::getElem(const std::string& name, std::vector& elem) 334 | { 335 | if(in[name] && in[name].IsSequence()) 336 | { 337 | uint i = 0; 338 | for(const auto& e : in[name]) 339 | { 340 | try 341 | { 342 | elem.push_back(e.as()); 343 | } 344 | catch(const std::runtime_error& e) 345 | { 346 | std::cout << "ERROR: Cannot read element " << i << e.what() << std::endl; 347 | return false; 348 | } 349 | } 350 | } 351 | else 352 | { 353 | return true; 354 | } 355 | return false; 356 | } 357 | 358 | bool 359 | YamlManager::getElem(const std::string& name, std::vector< cv::Point2f >& elem) 360 | { 361 | if(in[name] && in[name].IsSequence()) 362 | { 363 | for (YAML::iterator it = in[name].begin(); it != in[name].end(); ++it) 364 | { 365 | try 366 | { 367 | elem.push_back(cv::Point2f((*it).as(), (*(++it)).as())); 368 | } 369 | catch(const std::runtime_error& e) 370 | { 371 | std::cout << "ERROR: Cannot read element " << e.what() << std::endl; 372 | return false; 373 | } 374 | } 375 | return true; 376 | } 377 | return false; 378 | } 379 | 380 | 381 | 382 | bool 383 | YamlManager::getElem(const std::string& name, cv::Mat& elem, int type) 384 | { 385 | if(in[name] && in[name].IsSequence()) 386 | { 387 | elem = cv::Mat(cv::Size(3, 3), type); 388 | uint i = 0; 389 | for(const auto& e : in[name]) 390 | { 391 | try 392 | { 393 | if(type == CV_32FC1) 394 | elem.at(i++) = e.as(); 395 | else if(type == CV_64FC1) 396 | elem.at(i++) = e.as(); 397 | } 398 | catch(const std::runtime_error& e) 399 | { 400 | std::cout << "ERROR: Cannot read element " << i << e.what() << std::endl; 401 | return false; 402 | } 403 | } 404 | return true; 405 | } 406 | else 407 | { 408 | return true; 409 | } 410 | return false; 411 | } 412 | 413 | bool 414 | YamlManager::getElem(const std::string& name, cv::Point2i& elem) 415 | { 416 | if(in[name] && in[name].IsSequence() && in[name].size() == 2) 417 | { 418 | try 419 | { 420 | elem.x = in[name][0].as(); 421 | elem.y = in[name][1].as(); 422 | } 423 | catch(const std::runtime_error& e) 424 | { 425 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 426 | return false; 427 | } 428 | } 429 | else 430 | { 431 | return false; 432 | } 433 | return true; 434 | } 435 | 436 | bool 437 | YamlManager::getElem(const std::string& name, cv::Point2f& elem) 438 | { 439 | if(in[name] && in[name].IsSequence() && in[name].size() == 2) 440 | { 441 | try 442 | { 443 | elem.x = in[name][0].as(); 444 | elem.y = in[name][1].as(); 445 | } 446 | catch(const std::runtime_error& e) 447 | { 448 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 449 | return false; 450 | } 451 | } 452 | else 453 | { 454 | return false; 455 | } 456 | return true; 457 | } 458 | 459 | bool 460 | YamlManager::getElem(const std::string& name, cv::Point2d& elem) 461 | { 462 | if(in[name] && in[name].IsSequence() && in[name].size() == 2) 463 | { 464 | try 465 | { 466 | elem.x = in[name][0].as(); 467 | elem.y = in[name][1].as(); 468 | } 469 | catch(const std::runtime_error& e) 470 | { 471 | std::cout << "ERROR: Cannot read element " << name << e.what() << std::endl; 472 | return false; 473 | } 474 | } 475 | else 476 | { 477 | return false; 478 | } 479 | return true; 480 | } 481 | 482 | 483 | -------------------------------------------------------------------------------- /src/homography/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(homography) 2 | include_directories(${PROJECT_BINARY_DIR}/../src/homography/include) 3 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../src/homography/include) 4 | 5 | file(GLOB_RECURSE HOMOGRAPHY_SRC "src/homography/src/*.cpp") 6 | 7 | add_library(homography SHARED ${HOMOGRAPHY_SRC}) 8 | target_link_libraries(homography ${OpenCV_LIBS} utils) 9 | 10 | endfunction() 11 | -------------------------------------------------------------------------------- /src/homography/include/homography.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _HOMOGRAPHY_H_ 6 | #define _HOMOGRAPHY_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include "yamlmanager.h" 19 | 20 | namespace mctracker 21 | { 22 | namespace geometry 23 | { 24 | class Homography { 25 | public: 26 | /** 27 | * @brief Constructor class Homography 28 | */ 29 | Homography() { ; } 30 | /** 31 | * @brief set the source and destination points 32 | * @param src a vecotor of points of the source image 33 | * @param dst a vecotor of points of the destination image 34 | */ 35 | void setPoints(const std::vector& src, const std::vector& dst); 36 | /** 37 | * @brief calculate the homography 38 | * @return a cv::Mat 3x3 containing the homography 39 | */ 40 | cv::Mat calcHomography(); 41 | /** 42 | * @brief draw the original points and the reprojected into the images 43 | * @param srcImage image representing the source to be reprojected 44 | * @param dstImage image representing the detestination image 45 | * @param H the homography matrix 46 | */ 47 | void draw(const cv::Mat& srcImage, const cv::Mat& dstImage, const cv::Mat& H); 48 | /** 49 | * @brief write the matrix into a yaml filepath 50 | * @param filepath path to the file in which the homography will be stored 51 | */ 52 | void writeToFile(const std::string& filepath = "../config/homography.yaml"); 53 | public: 54 | /** 55 | * @brief calculate the projection of point give the homography matrix 56 | * @param point point to be converted 57 | * @param H homography matrix 58 | * @param x variable where the x-coordinate of the converted point will be stored 59 | * @param y variable where the y-coordinate of the converted point will be stored 60 | */ 61 | static void calcProjection(const cv::Point2f &point, const cv::Mat &H, double &x, double &y); 62 | private: 63 | std::vector srcPoints; 64 | std::vector dstPoints; 65 | cv::Mat H; 66 | float error; 67 | config::YamlManager configFileWriter; 68 | }; 69 | } 70 | } 71 | 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /src/homography/src/homography.cpp: -------------------------------------------------------------------------------- 1 | #include "homography.h" 2 | 3 | using namespace mctracker; 4 | using namespace mctracker::geometry; 5 | 6 | void 7 | Homography::setPoints(const std::vector& src, const std::vector& dst) 8 | { 9 | auto itPts = srcPoints.begin(); 10 | 11 | for(const auto& point : src) 12 | { 13 | srcPoints.push_back(point); 14 | } 15 | 16 | for(const auto& point : dst) 17 | { 18 | dstPoints.push_back(point); 19 | } 20 | } 21 | 22 | 23 | cv::Mat 24 | Homography::calcHomography() 25 | { 26 | 27 | std::cout << "\nsrc points: " << std::endl; 28 | 29 | for(const auto& point : srcPoints) 30 | { 31 | std::cout << point << std::endl; 32 | } 33 | 34 | std::cout << "\ndst points: " << std::endl; 35 | 36 | for(const auto& point : dstPoints) 37 | { 38 | std::cout << point << std::endl; 39 | } 40 | 41 | H = cv::findHomography( 42 | cv::Mat(srcPoints), cv::Mat(dstPoints), // corresponding points 43 | //inliers, // outputed inliers matches 44 | 0, //force all corrispondences to be used 45 | 0.5); // max distance to reprojection point 46 | 47 | std::cout << std::endl; 48 | std::cout << "MATRIX H" << std::endl; 49 | std::cout << H << std::endl; 50 | 51 | cv::Mat invH = H.inv(); 52 | 53 | std::cout << std::endl; 54 | std::cout << "INV MATRIX H" << std::endl; 55 | std::cout << invH << std::endl; 56 | 57 | return H; 58 | } 59 | 60 | 61 | void 62 | Homography::draw(const cv::Mat& srcImage, const cv::Mat& dstImage, const cv::Mat& H) { 63 | 64 | cv::Mat dstMat; 65 | dstImage.copyTo(dstMat); 66 | cv::Mat srcMat; 67 | srcImage.copyTo(srcMat); 68 | 69 | // Draw the inlier points 70 | std::vector::const_iterator itPts = dstPoints.begin(); 71 | 72 | for(const auto& point : dstPoints) 73 | { 74 | cv::circle(dstMat, point, 6, cv::Scalar(120,120,120), 2); 75 | } 76 | 77 | for(const auto& point : srcPoints) 78 | { 79 | cv::circle(srcMat, point, 6, cv::Scalar(120,120,120), 2); 80 | ++itPts; 81 | } 82 | 83 | // Display the images with points 84 | cv::namedWindow("Dst Points"); 85 | cv::imshow("Dst Points", dstMat); 86 | cv::namedWindow("Src Points"); 87 | cv::imshow("Src Points",srcMat); 88 | 89 | //reprojection 90 | std::cout << "Reprojection..." << std::endl; 91 | 92 | cv::Mat reprojectionImage = dstMat.clone(); 93 | 94 | int cnt = 0; 95 | auto dist = [] (const cv::Point2f& p1, const cv::Point2f& p2) 96 | { 97 | const auto& p = p1 - p2; 98 | return sqrt(p.x * p.x + p.y * p.y); 99 | }; 100 | 101 | error = 0.0; 102 | auto ii = 0; 103 | for(const auto& point : srcPoints) 104 | { 105 | double x, y; 106 | cv::Point2f p(point.x, point.y); 107 | calcProjection(p, H, x, y); 108 | 109 | std::cout << "(" << p.x << "," << p.y << ") --> (" << x << "," << y << ")" << std::endl; 110 | error += dist(dstPoints.at(ii++), cv::Point2f(x, y)); 111 | 112 | if(x < 0 || y < 0) 113 | { 114 | std::cout << "WARNING: Negative reprojection values" << std::endl; 115 | } 116 | else { 117 | cv::line(reprojectionImage, cv::Point2f(x, y), 118 | cv::Point2f(x+10, y), CV_RGB(0,0,255), 2); 119 | cv::line(reprojectionImage, cv::Point2f(x, y), 120 | cv::Point2f(x-10, y), CV_RGB(0,0,255), 2); 121 | cv::line(reprojectionImage, cv::Point2f(x, y), 122 | cv::Point2f(x, y+10), CV_RGB(0,0,255), 2); 123 | cv::line(reprojectionImage, cv::Point2f(x, y), 124 | cv::Point2f(x, y-10), CV_RGB(0,0,255), 2); 125 | 126 | std::string s; 127 | std::stringstream out; 128 | out << cnt; 129 | s.assign(out.str()); 130 | cv::putText(reprojectionImage, s, cv::Point2f(x+5,y-5), 131 | CV_FONT_HERSHEY_SIMPLEX, 0.6, CV_RGB(0, 0, 255)); 132 | } 133 | } 134 | 135 | error /= srcPoints.size(); 136 | 137 | std::cout << "Reprojection error: " << error << std::endl; 138 | 139 | cv::namedWindow("Reprojected Points"); 140 | cv::imshow("Reprojected Points", reprojectionImage); 141 | 142 | cv::Mat warpImage = cv::Mat::zeros(dstMat.size(), dstMat.type()); 143 | cv::warpPerspective(srcImage, warpImage, H, dstMat.size()); 144 | 145 | 146 | cv::Mat total; 147 | cv::addWeighted(warpImage, 0.7, dstImage, 1., 0, total); 148 | 149 | cv::imshow("Total", total); 150 | 151 | cv::namedWindow("Warp Image"); 152 | cv::imshow("Warp Image", warpImage); 153 | 154 | cv::waitKey(0); 155 | } 156 | 157 | void 158 | Homography::calcProjection(const cv::Point2f &point, const cv::Mat &H, double &x, double &y) 159 | { 160 | if(H.empty()) 161 | { 162 | throw std::invalid_argument("Invaling homography"); 163 | } 164 | 165 | x = (point.x*H.at(0,0)+point.y*H.at(0,1)+H.at(0,2))/ 166 | (point.x*H.at(2,0)+point.y*H.at(2,1)+H.at(2,2)); 167 | y = (point.x*H.at(1,0)+point.y*H.at(1,1)+H.at(1,2))/ 168 | (point.x*H.at(2,0)+point.y*H.at(2,1)+H.at(2,2)); 169 | } 170 | 171 | void 172 | Homography::writeToFile(const std::string& filepath) 173 | { 174 | if(H.empty()) 175 | { 176 | throw std::invalid_argument("Homography Matrix is empty"); 177 | } 178 | 179 | configFileWriter.writeElem("Matrix", H); 180 | configFileWriter.writeElem("Error", error); 181 | configFileWriter.write(filepath); 182 | } 183 | 184 | 185 | 186 | -------------------------------------------------------------------------------- /src/object_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(object_detection) 2 | add_library(darknet SHARED IMPORTED) 3 | set_property(TARGET darknet PROPERTY IMPORTED_LOCATION ${CMAKE_SOURCE_DIR}/darknet/libdarknet.so) 4 | 5 | 6 | include_directories(${PROJECT_BINARY_DIR}/darknet/include) 7 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/darknet/include) 8 | include_directories(${PROJECT_BINARY_DIR}/../src/object_detection/include) 9 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../src/object_detection/include) 10 | 11 | file(GLOB_RECURSE DETECTOR "src/object_detection/src/*.cpp") 12 | 13 | #compiling libraries 14 | add_library(objectdetector SHARED ${DETECTOR}) 15 | target_link_libraries(objectdetector darknet) 16 | endfunction() 17 | -------------------------------------------------------------------------------- /src/object_detection/include/object_detector.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _OBJECT_DETECTOR_H_ 6 | #define _OBJECT_DETECTOR_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include "detector_param.h" 12 | #include "yolo_v2_class.hpp" 13 | 14 | 15 | using namespace mctracker::config; 16 | namespace mctracker 17 | { 18 | namespace objectdetection 19 | { 20 | class ObjectDetector 21 | { 22 | public: 23 | /** 24 | * @brief Constructor class Homography 25 | * @param params object detector params extracted from the configuration file 26 | */ 27 | ObjectDetector(const DetectorParam& params); 28 | /** 29 | * @brief classify the current frame 30 | * @param frame cv::Mat containing the image to classify 31 | * @param draw boolean which if is true allow the function to draw the detections 32 | * @return a cv::Mat containing the original frame with the detections drawed on (if draw is true) 33 | */ 34 | cv::Mat classify(cv::Mat& frame, bool draw = true); 35 | public: 36 | /** 37 | * @brief return the detections of the last classified frame 38 | * @brief a vector of bbo_t contianing all the detections 39 | */ 40 | inline const std::vector 41 | detections() const 42 | { 43 | return final_dets; 44 | } 45 | private: 46 | /** 47 | * @brief draw a bounding box around the detections 48 | * @param img the cv::Mat where the detections are drawed on 49 | * @param result_vec the vector of bbox_t containing all the detections 50 | */ 51 | void draw_boxes(cv::Mat& mat_img, std::vector& result_vec); 52 | /** 53 | * @brief refine all the detections according to the thresholds specified in the config file 54 | * @param dets the vector of bbox_t containing all the detections 55 | */ 56 | void refining(const std::vector& dets); 57 | private: 58 | std::string cfg, weights; 59 | float minimum_thresh; 60 | std::shared_ptr detector; 61 | std::vector dets; 62 | std::vector final_dets; 63 | }; 64 | } 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /src/object_detection/src/object_detector.cpp: -------------------------------------------------------------------------------- 1 | #include "object_detector.h" 2 | 3 | using namespace mctracker; 4 | using namespace mctracker::objectdetection; 5 | 6 | ObjectDetector 7 | ::ObjectDetector(const DetectorParam& params) 8 | { 9 | cfg = params.getConfig(); 10 | weights = params.getWeights(); 11 | minimum_thresh = params.getThreshold(); 12 | detector = std::shared_ptr(new Detector(cfg, weights)); 13 | } 14 | 15 | 16 | cv::Mat 17 | ObjectDetector::classify(cv::Mat& frame, bool draw) 18 | { 19 | final_dets.clear(); 20 | dets = detector->detect(frame); 21 | refining(dets); 22 | if(draw) 23 | { 24 | draw_boxes(frame, final_dets); 25 | } 26 | return frame; 27 | } 28 | 29 | 30 | void 31 | ObjectDetector::refining(const std::vector& dets) 32 | { 33 | for(const auto& det : dets) 34 | { 35 | //0 corresponds to PERSON and minimum_thresh is the minimum threshold I consider for being a person 36 | if(det.obj_id == 0 && det.prob >= minimum_thresh) 37 | { 38 | final_dets.push_back(det); 39 | } 40 | } 41 | } 42 | 43 | 44 | 45 | void 46 | ObjectDetector::draw_boxes(cv::Mat& mat_img, std::vector& result_vec) 47 | { 48 | static const cv::Scalar color(0, 0, 255); 49 | static const std::string obj_name = "person"; 50 | 51 | for (auto &i : result_vec) 52 | { 53 | cv::rectangle(mat_img, cv::Rect(i.x, i.y, i.w, i.h), color, 2); 54 | 55 | cv::Size const text_size = cv::getTextSize(obj_name, cv::FONT_HERSHEY_COMPLEX_SMALL, 1.2, 2, 0); 56 | int const max_width = (text_size.width > (int)i.w + 2) ? text_size.width : (i.w + 2); 57 | cv::rectangle(mat_img, cv::Point2f(std::max((int)i.x - 1, 0), std::max((int)i.y - 30, 0)), 58 | cv::Point2f(std::min((int)i.x + max_width, mat_img.cols-1), std::min((int)i.y, mat_img.rows-1)), 59 | color, CV_FILLED, 8, 0); 60 | cv::putText(mat_img, obj_name, cv::Point2f(i.x, i.y - 10), cv::FONT_HERSHEY_COMPLEX_SMALL, 1.2, cv::Scalar(0, 0, 0), 2); 61 | 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /src/segmentation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(segmentation) 2 | include_directories(${PROJECT_BINARY_DIR}/../src/segmentation/include) 3 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../src/segmentation/include) 4 | 5 | file(GLOB_RECURSE SEGMENTATION_SRC "src/segmentation/src/*.cpp") 6 | 7 | add_library(segmentation SHARED ${SEGMENTATION_SRC}) 8 | target_link_libraries(segmentation ${OpenCV_LIBS}) 9 | 10 | endfunction() 11 | -------------------------------------------------------------------------------- /src/segmentation/include/bgsubtraction.h: -------------------------------------------------------------------------------- 1 | #ifndef _BGSUBTRACTION_H_ 2 | #define _BGSUBTRACTION_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace mctracker 8 | { 9 | namespace segmentation 10 | { 11 | class BgSubtraction 12 | { 13 | public: 14 | /** 15 | * @brief Constructor class BgSubtraction 16 | */ 17 | BgSubtraction(); 18 | /** 19 | * @brief compute the background model 20 | * @param frame cv::Mat containing the current frame 21 | * @param morph a boolean which if is true enable morphology operations on the resulting foreground mask 22 | */ 23 | void process(const cv::Mat& frame, bool morph = true); 24 | public: 25 | /** 26 | * @brief get the foreground mask 27 | * @param mask cv::Mat variable in which the mask will be stored 28 | * @return true if the mask is ready, false otherwise 29 | */ 30 | inline bool 31 | getFgMask(cv::Mat& mask) const 32 | { 33 | if(frameNum >= 2) 34 | { 35 | mask = fgMask.getMat(cv::ACCESS_WRITE).clone(); 36 | return true; 37 | } 38 | return false; 39 | } 40 | private: 41 | cv::Ptr pMOG2; 42 | double learningRate; 43 | int frameNum; 44 | cv::UMat fgMask; 45 | cv::UMat ref_frame; 46 | }; 47 | } 48 | } 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /src/segmentation/src/bgsubtraction.cpp: -------------------------------------------------------------------------------- 1 | #include "bgsubtraction.h" 2 | 3 | using namespace mctracker; 4 | using namespace mctracker::segmentation; 5 | 6 | BgSubtraction 7 | ::BgSubtraction() 8 | { 9 | pMOG2 = cv::createBackgroundSubtractorKNN(); 10 | frameNum = 0; 11 | } 12 | 13 | void 14 | BgSubtraction::process(const cv::Mat& frame, bool morph) 15 | { 16 | //Decrease the learning rate when the background model becomes more reliable 17 | if (frameNum < 100) 18 | { 19 | learningRate = -1; 20 | } 21 | else if (frameNum < 1000) 22 | { 23 | learningRate = -.1; 24 | } 25 | else 26 | { 27 | learningRate = -0.001; 28 | } 29 | 30 | cv::UMat frameUMat = frame.getUMat(cv::ACCESS_WRITE); 31 | cv::blur(frameUMat, ref_frame, cv::Size(15, 15)); 32 | 33 | pMOG2->apply(ref_frame, fgMask, learningRate); 34 | cv::compare(fgMask, cv::Scalar(255), fgMask, cv::CMP_EQ); 35 | 36 | if(morph) 37 | { 38 | cv::erode(fgMask, fgMask, cv::Mat(), cv::Point(-1, -1), 1); 39 | cv::dilate(fgMask, fgMask, cv::Mat(), cv::Point(-1, -1), 2); 40 | } 41 | 42 | frameUMat.release(); 43 | 44 | frameNum++; 45 | } 46 | -------------------------------------------------------------------------------- /src/tracker/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(tracker) 2 | include_directories(${PROJECT_BINARY_DIR}/../src/tracker/include) 3 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../src/tracker/include) 4 | 5 | file(GLOB_RECURSE TRACKER_SRC "src/tracker/src/*.cpp") 6 | 7 | add_library(tracker SHARED ${TRACKER_SRC}) 8 | target_link_libraries(tracker ${OpenCV_LIBS} config) 9 | 10 | endfunction() 11 | -------------------------------------------------------------------------------- /src/tracker/include/detection.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _DETECTION_H_ 6 | #define _DETECTION_H_ 7 | 8 | #include 9 | #include 10 | 11 | namespace mctracker 12 | { 13 | namespace tracker 14 | { 15 | class Detection 16 | { 17 | public: 18 | /** 19 | * @brief Constructor class Detection 20 | */ 21 | Detection() { ; } 22 | /** 23 | * @brief Constructor class Detection 24 | * @param _x the x-coordinate of the top left point 25 | * @param _y the y-coordinate of the top left point 26 | * @param _w the width of the detection 27 | * @param _h the height of the detection 28 | * @param hist the hsv histogram of the observation 29 | */ 30 | Detection(const float& _x, const float& _y, const float& _w, const float& _h, const cv::Mat& hist) 31 | : m_x(_x), m_y(_y) , m_w(_w), m_h(_h), m_hist(hist) 32 | { 33 | point = cv::Point2f(m_x, m_y); 34 | } 35 | 36 | /** 37 | * @brief get the x-coordinate of the top left point 38 | * @return the x value of the x-coordinate of the top left point 39 | */ 40 | inline const float 41 | x() const 42 | { 43 | return m_x; 44 | } 45 | 46 | /** 47 | * @brief get the y-coordinate of the top left point 48 | * @return the y value of the y-coordinate of the top left point 49 | */ 50 | inline const float 51 | y() const 52 | { 53 | return m_y; 54 | } 55 | 56 | /** 57 | * @brief get the width of the detection 58 | * @return the width value of the detection 59 | */ 60 | inline const float 61 | w() const 62 | { 63 | return m_w; 64 | } 65 | 66 | /** 67 | * @brief get the height of the detection 68 | * @return the height value of the detection 69 | */ 70 | inline const float 71 | h() const 72 | { 73 | return m_h; 74 | } 75 | 76 | /** 77 | * @brief get the hsv histogram of the detection 78 | * @return the hsv histogram of the detection 79 | */ 80 | inline const cv::Mat 81 | hist() const 82 | { 83 | return m_hist; 84 | } 85 | 86 | /** 87 | * @brief set the x-coordinate of the top left point 88 | * @param x containing the value of the x-coordinate of the top left point 89 | */ 90 | void 91 | setX(const float& x) 92 | { 93 | m_x = x; 94 | } 95 | 96 | /** 97 | * @brief set the y-coordinate of the top left point 98 | * @param y containing the value of the y-coordinate of the top left point 99 | */ 100 | void 101 | setY(const float& y) 102 | { 103 | m_y = y; 104 | } 105 | 106 | /** 107 | * @brief set the hsv histrogram of the detection 108 | * @param hist containing the hsv histrogram of the detection 109 | */ 110 | void 111 | setHist(const cv::Mat& hist) 112 | { 113 | m_hist = hist; 114 | } 115 | 116 | /** 117 | * @brief set the width and the height of the detection 118 | * @param w containing the width of the detection 119 | * @param h containing the height of the detection 120 | */ 121 | void 122 | setSize(int w, int h) 123 | { 124 | m_w = w; 125 | m_h = h; 126 | } 127 | 128 | /** 129 | * @brief operator() 130 | * @return a cv::Point2f containing the topleft point of the detection 131 | */ 132 | inline const cv::Point2f 133 | operator()() const 134 | { 135 | return point; 136 | } 137 | 138 | /** 139 | * @brief copy operator 140 | * @param d_copy object to copy 141 | * @return the pointer to the current instance 142 | */ 143 | Detection& operator=(const Detection& d_copy) 144 | { 145 | this->m_x = d_copy.x(); 146 | this->m_y = d_copy.y(); 147 | this->m_h = d_copy.h(); 148 | this->m_w = d_copy.w(); 149 | this->m_hist = d_copy.hist().clone(); 150 | return *this; 151 | } 152 | 153 | private: 154 | float m_x, m_y; 155 | float m_w, m_h; 156 | cv::Point2f point; 157 | cv::Mat m_hist; 158 | }; 159 | } 160 | } 161 | 162 | #endif 163 | -------------------------------------------------------------------------------- /src/tracker/include/entity.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | 6 | #ifndef _ENTITY_H_ 7 | #define _ENTITY_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "kalman.h" 14 | #include "drawing.h" 15 | #include "utils.h" 16 | #include "camera.h" 17 | 18 | using namespace mctracker::utils; 19 | 20 | namespace mctracker 21 | { 22 | namespace tracker 23 | { 24 | class Entity 25 | { 26 | friend class Tracker; 27 | 28 | public: 29 | /** 30 | * @brief Constructor class Entity 31 | */ 32 | Entity() { ; } 33 | /** 34 | * @brief get the 2d point representing the last predicted position of the entity 35 | * @return the 2d point in a cv::Point2f format 36 | */ 37 | virtual const cv::Point2f getPoint() = 0; 38 | /** 39 | * @brief get the id of the entity 40 | * @return the integer representing the id assigned to the entity 41 | */ 42 | virtual const int label() const = 0; 43 | public: 44 | /** 45 | * @brief get the sizes of the the entity from all the views 46 | * @return a vector of cv::Size containing all the sizes from all the view 47 | */ 48 | const std::vector getSizes() const 49 | { 50 | return sizes; 51 | } 52 | 53 | /** 54 | * @brief draw the tracked entities in all the camera views 55 | * @param images a vector containing the current frames grabbed from all the cameras 56 | * @param cameras a vector containing all the camera infos 57 | */ 58 | inline const void 59 | drawTrack(std::vector& images, const std::vector& cameras) 60 | { 61 | if(isgood) 62 | { 63 | auto i = 0; 64 | for(auto& img : images) 65 | { 66 | auto p = getPoint(); 67 | auto imPoint = cameras.at(i).world2camera(p); 68 | const std::vector& sz = sizes; 69 | const cv::Size& curr_size = sz.at(i); 70 | if(curr_size.width != 0) 71 | { 72 | imPoint.y -= curr_size.height; 73 | imPoint.x -= (curr_size.width>>1); 74 | 75 | if(imPoint.x >= 0 && imPoint.y >= 0 && (imPoint.x + curr_size.width) < (img.cols - 1) && (imPoint.y + curr_size.height) < (img.rows - 1)) 76 | { 77 | tools::Drawing::rectangle(cv::Rect(imPoint.x, imPoint.y, curr_size.width, curr_size.height), color, img); 78 | std::stringstream ss; 79 | ss << "#" << label2string(); 80 | 81 | cv::putText(img, ss.str().c_str(), imPoint, cv::FONT_HERSHEY_SIMPLEX, 82 | 0.55, cv::Scalar(0, 255, 0), 2, CV_AA); 83 | } 84 | } 85 | ++i; 86 | } 87 | } 88 | } 89 | 90 | /** 91 | * @brief draw all the entities on the planview 92 | * @param img cv::Mat containing the plan view 93 | * @param history boolean: if true all the history realted to the entity is drawed 94 | */ 95 | inline const void 96 | drawTrackPlanView(cv::Mat& img, bool history = true) 97 | { 98 | if(isgood) 99 | { 100 | const auto& p = getPoint(); 101 | cv::circle(img, p, 3, color, -1); 102 | cv::putText(img, label2string().c_str(), p, cv::FONT_HERSHEY_SIMPLEX, 103 | 0.55, cv::Scalar(0, 255, 0), 2, CV_AA); 104 | 105 | if(history) 106 | tools::Drawing::history(m_history, color, img); 107 | } 108 | } 109 | 110 | /** 111 | * @brief get the history of the entity 112 | * @return a vector of cv::Points containing the history of the entity 113 | */ 114 | inline const Points 115 | history() const 116 | { 117 | return m_history; 118 | } 119 | 120 | protected: 121 | typedef std::shared_ptr Kalman_ptr; 122 | protected: 123 | virtual const std::string label2string() = 0; 124 | protected: 125 | /** 126 | * @brief set the color of the entity 127 | * @param _color a cv::Scalar containing the color 128 | */ 129 | void 130 | setColor(const cv::Scalar& _color) 131 | { 132 | color = _color; 133 | } 134 | 135 | /** 136 | * @brief set the update frequencies of the entity 137 | * @param dt a float containing the value of dt 138 | */ 139 | inline const void 140 | setDt(const float& dt) 141 | { 142 | kf->setDt(dt); 143 | } 144 | 145 | /** 146 | * @brief get the the control-input model of the kalman filter associated to the entity 147 | * @return the control-input model 148 | */ 149 | inline const 150 | cv::Mat B() const 151 | { 152 | return kf->B(); 153 | } 154 | 155 | /** 156 | * @brief get posteriori error covariance matrix of the kalman filter associated to the entity 157 | * @return the posteriori error covariance matrix 158 | */ 159 | inline const cv::Mat 160 | P() const 161 | { 162 | return kf->P(); 163 | } 164 | 165 | /** 166 | * @brief get the error measurement covariance matrix of the kalman filter associated to the entity 167 | * @return the error measurement covariance matrix 168 | */ 169 | inline const 170 | cv::Mat S() const 171 | { 172 | return kf->S(); 173 | } 174 | 175 | /** 176 | * @brief check the history of the entity in order to keep only the last 10 observations 177 | */ 178 | const 179 | void checkHistory() 180 | { 181 | if(m_history.size() > history_size) 182 | { 183 | m_history.erase(m_history.begin()); 184 | } 185 | } 186 | 187 | protected: 188 | Points m_history; 189 | cv::Scalar color; 190 | Kalman_ptr kf; 191 | uint w, h; 192 | bool isgood; 193 | std::vector sizes; 194 | protected: 195 | constexpr static uint history_size = 10; 196 | }; 197 | } 198 | } 199 | 200 | #endif 201 | -------------------------------------------------------------------------------- /src/tracker/include/hungarianAlg.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * 3 | * hungarianAlg.h 4 | author Andrey Smorodov 5 | modified by Andrea Pennisi 6 | 7 | header file for Hungarian algorithm 8 | * 9 | **************************************************************************/ 10 | 11 | #ifndef _HUNGARIAN_ALG_H_ 12 | #define _HUNGARIAN_ALG_H_ 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "utils.h" 21 | 22 | typedef float track_t; 23 | typedef std::vector assignments_t; 24 | typedef std::vector distMatrix_t; 25 | 26 | namespace mctracker 27 | { 28 | namespace tracker 29 | { 30 | namespace costs 31 | { 32 | class AssignmentProblemSolver 33 | { 34 | public: 35 | // enum containing the Method for searching the optimal solutions 36 | enum TMethod 37 | { 38 | optimal, 39 | many_forbidden_assignments, 40 | without_forbidden_assignments 41 | }; 42 | public: 43 | /** 44 | * @brief Constructor class AssignmentProblemSolver 45 | */ 46 | AssignmentProblemSolver() { ; } 47 | /** 48 | * @brief compute the hungarian algorithm 49 | * @param distMatrix matrix containing the costs 50 | * @param nOfRows representing the number of tracks already existing 51 | * @param nOfColumns number of detections 52 | * @param assignment a vector in which all the assignments are stored 53 | * @param Method the enum variable representing which method that will be used for the hungarian algorithm 54 | */ 55 | track_t Solve(const distMatrix_t& distMatrixIn, const size_t& nOfRows, const size_t& nOfColumns, assignments_t& assignment, const TMethod& Method = optimal); 56 | private: 57 | typedef std::vector BoolVec; 58 | private: 59 | // -------------------------------------------------------------------------- 60 | // Computes the optimal assignment (minimum overall costs) using Munkres algorithm. 61 | // -------------------------------------------------------------------------- 62 | void assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows, const size_t& nOfColumns); 63 | void buildassignmentvector(assignments_t& assignment, BoolVec& starMatrix, const size_t& nOfRows, const size_t& nOfColumns); 64 | void computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows); 65 | void step2a(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim); 66 | void step2b(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim); 67 | void step3(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim); 68 | void step4(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim, const size_t& row, const size_t& col); 69 | void step5(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim); 70 | // -------------------------------------------------------------------------- 71 | // Computes a suboptimal solution. Good for cases with many forbidden assignments. 72 | // -------------------------------------------------------------------------- 73 | void assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows, const size_t& nOfColumns); 74 | // -------------------------------------------------------------------------- 75 | // Computes a suboptimal solution. Good for cases with many forbidden assignments. 76 | // -------------------------------------------------------------------------- 77 | void assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows, const size_t& nOfColumns); 78 | }; 79 | } 80 | } 81 | } 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /src/tracker/include/hypothesis.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _HYPHOTHESIS_H_ 6 | #define _HYPHOTHESIS_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include "lap.h" 14 | #include "hungarianAlg.h" 15 | #include "detection.h" 16 | #include "kalman_param.h" 17 | #include "track.h" 18 | #include "utils.h" 19 | 20 | using namespace mctracker::tracker::costs; 21 | 22 | namespace mctracker 23 | { 24 | namespace tracker 25 | { 26 | class Hyphothesis 27 | { 28 | private: 29 | typedef std::shared_ptr Track_ptr; 30 | typedef std::vector Tracks; 31 | public: 32 | static std::shared_ptr instance(); 33 | /** 34 | * @brief compare the previous unassigned points to the new detection in order to create new tracks 35 | * @param dummmy_assignments a matrix containing the assignment coming from the hungarian algorithm 36 | * @param tracks the se of the current tracks 37 | * @param w width of the tracking space 38 | * @param h height of the tracking space 39 | * @param new_hyp_dummy_costs a dummy cost for making a new hyphothesis 40 | * @param prev_unassigned vector containing the previous unassigned detections 41 | * @param param kalman parameters 42 | * @param cameraNum camera number 43 | */ 44 | void new_hyphothesis(const cv::Mat& dummmy_assignments, Tracks& tracks, const Detections& detections, const uint& w, const uint& h, 45 | const uint& new_hyp_dummy_costs, Detections& prev_unassigned, const KalmanParam& param, const int cameraNum); 46 | private: 47 | static std::shared_ptr m_instance; 48 | private: 49 | // 15 sig. digits for 0<=real(z)<=171 50 | // coeffs should sum to about g*g/2+23/24 51 | constexpr static float g = 4.7422; 52 | constexpr static float sq2pi= 2.5066282746310005024157652848110; 53 | private: 54 | /** 55 | * @brief Constructor class Hyphothesis 56 | */ 57 | Hyphothesis() { ; } 58 | /** 59 | * @brief compute th beta likelihood given a points 60 | * @param prev_unassigned a point previously unassigned 61 | * @param alpha horizontal range of the detection 62 | * @param beta vertical range of the detection 63 | * @param w width of the tracking space 64 | * @param h height of the tracking space 65 | * @return the beta likelihood 66 | */ 67 | float beta_likelihood(const cv::Point2f& prev_unassigned, const float& alpha, const float& beta, const uint& w, const uint& h); 68 | /** 69 | * @brief compute the gamma function 70 | * @param z may be complex and of any size. 71 | * @return the gamma function value 72 | */ 73 | float gamma(const float& z); 74 | }; 75 | } 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /src/tracker/include/kalman.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef KALMAN_H 6 | #define KALMAN_H 7 | 8 | #include 9 | 10 | namespace mctracker 11 | { 12 | namespace tracker 13 | { 14 | class KalmanFilter 15 | { 16 | public: 17 | /** 18 | * @brief Constructor class KalmanFilter 19 | */ 20 | KalmanFilter() {;} 21 | /** 22 | * @brief Constructor class KalmanFilter 23 | * @param _x x-coordinate of the current detection 24 | * @param _y y-coordinate of the current detection 25 | * @param _dt inital update frequency 26 | */ 27 | KalmanFilter(const float &_x, const float &_y, const float &dt); 28 | 29 | /** 30 | * @brief get the kalman filter 31 | * @return the kalman filter 32 | */ 33 | inline cv::KalmanFilter kf() 34 | { 35 | return KF; 36 | } 37 | 38 | /** 39 | * @brief preditction step of the kalman filter 40 | * @return a cv::Mat containing the prediction 41 | */ 42 | cv::Mat predict(); 43 | /** 44 | * @brief correction step of the kalman filter 45 | * @param _x x-coordinate of the current detection 46 | * @param _y y-coordinate of the current detection 47 | * @return cv::Mat containing the corrected prediction 48 | */ 49 | cv::Mat correct(const int &_x, const int &_y); 50 | /** 51 | * @brief the last prediction 52 | * @return a cv;:Mat containing the last prediction 53 | */ 54 | inline cv::Mat 55 | getPrediction() 56 | { 57 | return prediction; 58 | } 59 | 60 | /** 61 | * @brief get the the control-input model of the kalman filter associated to the entity 62 | * @return the control-input model 63 | */ 64 | inline const cv::Mat 65 | B() const 66 | { 67 | return KF.controlMatrix; 68 | } 69 | 70 | /** 71 | * @brief get posteriori error covariance matrix of the kalman filter associated to the entity 72 | * @return the posteriori error covariance matrix 73 | */ 74 | inline const cv::Mat 75 | P() const 76 | { 77 | return KF.errorCovPre; 78 | } 79 | 80 | /** 81 | * @brief get the error measurement covariance matrix of the kalman filter associated to the entity 82 | * @return the error measurement covariance matrix 83 | */ 84 | inline const cv::Mat 85 | S() const 86 | { 87 | return KF.errorCovPre(cv::Rect(0, 0, 2, 2)) + KF.measurementNoiseCov; 88 | } 89 | 90 | /** 91 | * @brief set the update frequencies of the entity 92 | * @param dt a float containing the value of dt 93 | */ 94 | const void 95 | setDt(const float& dt) 96 | { 97 | KF.transitionMatrix.at(2) = dt; 98 | KF.transitionMatrix.at(7) = dt; 99 | } 100 | 101 | private: 102 | //the kalman filter 103 | cv::KalmanFilter KF; 104 | cv::Mat_ measurement; 105 | cv::Mat processNoise; 106 | cv::Mat_ state; 107 | cv::Mat prediction; 108 | uint w, h; 109 | }; 110 | } 111 | } 112 | 113 | #endif //KALMAN_H 114 | -------------------------------------------------------------------------------- /src/tracker/include/lap.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * 3 | * lap.h 4 | version 1.0 - 21 june 1996 5 | author Roy Jonker, MagicLogic Optimization Inc. 6 | modified by Andrea Pennisi 7 | 8 | header file for LAP 9 | * 10 | **************************************************************************/ 11 | 12 | #ifndef _LAP_H_ 13 | #define _LAP_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace mctracker 22 | { 23 | namespace tracker 24 | { 25 | namespace costs 26 | { 27 | class LapCost 28 | { 29 | public: 30 | static std::shared_ptr instance(); 31 | /** 32 | * @brief compute the linear association costs 33 | * @param dim problem size 34 | * @param assigncost cost matrix 35 | * @param rowsol column assigned to row in solution 36 | * @param colsol row assigned to column in solution 37 | * @param u dual variables, row reduction numbers 38 | * @param v dual variables, column reduction numbers 39 | * @return an integer representing the lap cost 40 | */ 41 | int lap(int dim, const std::vector< std::vector >& assigncost, std::vector& rowsol, std::vector& colsol, std::vector& u, std::vector& v); 42 | private: 43 | typedef int row; 44 | typedef int col; 45 | typedef int cost; 46 | private: 47 | static std::shared_ptr m_instance; 48 | /** 49 | * @brief Constructor class LapCost 50 | */ 51 | LapCost() { ; } 52 | private: 53 | constexpr static int BIG = std::numeric_limits::max(); 54 | }; 55 | } 56 | } 57 | } 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /src/tracker/include/track.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | 6 | #ifndef _TRACK_H_ 7 | #define _TRACK_H_ 8 | 9 | #include 10 | #include 11 | 12 | #include "utils.h" 13 | #include "entity.h" 14 | #include "kalman.h" 15 | #include "kalman_param.h" 16 | 17 | using namespace mctracker::config; 18 | 19 | namespace mctracker 20 | { 21 | namespace tracker 22 | { 23 | class Track : public Entity 24 | { 25 | friend class Tracker; 26 | 27 | public: 28 | /** 29 | * @brief Constructor class Track 30 | */ 31 | Track() { ; } 32 | /** 33 | * @brief Constructor class Track 34 | * @param _x x-coordinate of the current detection 35 | * @param _y y-coordinate of the current detection 36 | * @param _param kalaman filter params 37 | * @param _h hsv histogram of the current detection 38 | * @param cameraNum number of cameras of the system 39 | */ 40 | Track(const float& _x, const float& _y, const KalmanParam& _param, const cv::Mat& _h, const int cameraNum); 41 | /** 42 | * @brief get the last prediction 43 | * @return the last predition of the kalman filter 44 | */ 45 | const cv::Point2f getPoint(); 46 | public: 47 | /** 48 | * @brief get the id associated to the track 49 | * @return the id 50 | */ 51 | inline const int 52 | label() const 53 | { 54 | return m_label; 55 | } 56 | protected: 57 | /** 58 | * @brief preditction step of the kalman filter 59 | * @return a cv::Mat containing the prediction 60 | */ 61 | const cv::Mat predict(); 62 | /** 63 | * @brief correction step of the kalman filter 64 | * @param _x x-coordinate of the current detection 65 | * @param _y y-coordinate of the current detection 66 | * @return cv::Mat containing the corrected prediction 67 | */ 68 | const cv::Mat correct(const float& _x, const float& _y); 69 | /** 70 | * @brief update the kalman filter considering all the observation of the same detection 71 | * @return a cv::Mat containing the corrected prediction 72 | */ 73 | const cv::Mat update(); 74 | protected: 75 | /** 76 | * @brief compare the current instance with an external object of the same type 77 | * @return true if the objects are the same, false otherwise 78 | */ 79 | bool 80 | operator==(const Track& _compare) 81 | { 82 | return m_label == _compare.label(); 83 | } 84 | 85 | /** 86 | * @brief get number of times that the track has been ntimes_propagated 87 | * @return the number of times that the track has been ntimes_propagated 88 | */ 89 | inline const uint 90 | nTimePropagation() const 91 | { 92 | return ntimes_propagated; 93 | } 94 | 95 | /** 96 | * @brief get the prediction of the kalman filter 97 | * @return a cv::Mat containing the prediction 98 | */ 99 | inline const cv::Mat 100 | getPrediction() const 101 | { 102 | return kf->getPrediction(); 103 | } 104 | 105 | /** 106 | * @brief the point representing the last prediction 107 | * @eturn a cv::Point2f containing the last prediction 108 | */ 109 | inline const cv::Point 110 | getPointPrediction() 111 | { 112 | const cv::Mat& prediction = kf->getPrediction(); 113 | return cv::Point2f(prediction.at(0), prediction.at(1)); 114 | } 115 | 116 | /** 117 | * @brief get the alive time of the track 118 | * @return the time 119 | */ 120 | inline const double 121 | getTime() const 122 | { 123 | return time_in_sec; 124 | } 125 | 126 | /** 127 | * @brief return the histogram associated to the track 128 | * @return the hsv histogram 129 | */ 130 | inline const cv::Mat 131 | histogram() const 132 | { 133 | return hist; 134 | } 135 | 136 | /** 137 | * @brief set the label of the track 138 | * @param _label unsigned int representing the label 139 | */ 140 | void 141 | setLabel(const uint& _label) 142 | { 143 | m_label = _label; 144 | } 145 | 146 | /** 147 | * @brief set the points representing the detecions associated to the track 148 | * @param p a cv::Point2f representing a single detection 149 | */ 150 | void 151 | push_point(const cv::Point2f& p) 152 | { 153 | points.push_back(p); 154 | } 155 | 156 | /** 157 | * @brief set the hsv histogram of the track 158 | * @param h a cv::Mat containing the hsv histogram 159 | */ 160 | void 161 | set_hist(const cv::Mat& h) 162 | { 163 | hist = h; 164 | } 165 | private: 166 | /** 167 | * @brief convert the id of the track to a string 168 | * @return a string containin the id of the track 169 | */ 170 | const std::string label2string(); 171 | private: 172 | int m_label; 173 | uint ntimes_propagated; 174 | uint freezed; 175 | double time; 176 | std::vector points; 177 | double time_in_sec; 178 | uint ntime_missed; 179 | cv::Mat hist; 180 | }; 181 | } 182 | } 183 | 184 | #endif 185 | -------------------------------------------------------------------------------- /src/tracker/include/tracker.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | 6 | #ifndef _TRACKER_H_ 7 | #define _TRACKER_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include "entity.h" 16 | #include "kalman_param.h" 17 | #include "track.h" 18 | #include "hypothesis.h" 19 | #include "detection.h" 20 | #include "hungarianAlg.h" 21 | #include "utils.h" 22 | #include "camera.h" 23 | 24 | using namespace mctracker::utils; 25 | using namespace mctracker::config; 26 | 27 | namespace mctracker 28 | { 29 | namespace tracker 30 | { 31 | typedef std::shared_ptr Entity_ptr; 32 | typedef std::vector Entities; 33 | 34 | class Tracker 35 | { 36 | friend class Entity; 37 | friend class Track; 38 | 39 | private: 40 | typedef std::shared_ptr Track_ptr; 41 | typedef std::vector Tracks; 42 | public: 43 | /** 44 | * @brief Constructor class Tracker 45 | * @param _param kalaman parameters 46 | * @param camerastack the camera stack stack 47 | */ 48 | Tracker(const KalmanParam& _param, const std::vector& camerastack); 49 | /** 50 | * @brief track the detections 51 | * @param _detections the current detections coming from all the cameras 52 | * @param w width of the tracking space 53 | * @param h height of the tracking space 54 | */ 55 | void track(std::vector& _detections, const int& w, const int& h); 56 | public: 57 | /** 58 | * @brief set the tracking space size 59 | * @param _w width of the tracking space 60 | * @param _h height of the tracking space 61 | */ 62 | inline const void 63 | setSize(const uint& _w, const uint& _h) 64 | { 65 | width = _w; 66 | height = _h; 67 | } 68 | 69 | /** 70 | * @brief evolve the tracks in order to compute the predictions 71 | */ 72 | inline const void 73 | evolveTracks() 74 | { 75 | for(const auto& track : single_tracks) 76 | { 77 | track->predict(); 78 | } 79 | } 80 | 81 | /** 82 | * @brief get the current tracks 83 | * @return a vector containing the tracks 84 | */ 85 | const Entities getTracks(); 86 | private: 87 | 88 | /** 89 | * @brief compute the association between the detections and the tracks 90 | * @param _detections a vector containing all the detections 91 | * @retun a cv::Mat containing the association track-detection 92 | */ 93 | cv::Mat associate_tracks(const Detections& _detections); 94 | 95 | /** 96 | * @brief update the tracks 97 | * @param assigments a vector containing the assigments between the detections of each camera and the old tracks 98 | * @param _detections a vector containing all the detections 99 | */ 100 | void update_tracks(const std::vector& assigments, const std::vector& _detections); 101 | 102 | /** 103 | * @brief delete/freeze the tracks for which no detections are available 104 | */ 105 | void delete_tracks(); 106 | 107 | /** 108 | * @brief check if the freezed tracks can be associated to the new detections 109 | * @param _detections a vector containing all the detections 110 | */ 111 | void check_old_tracks(std::vector< Detections >& _detections); 112 | 113 | /** 114 | * @brief refine the input detections 115 | * @param _detections a vector containing all the detections 116 | */ 117 | void refine_detections(std::vector& _detections); 118 | 119 | /** 120 | * @brief compute the first association between cameras 121 | * @param _detections a vector containing all the detections 122 | * @return a vector containing the associated points 123 | */ 124 | Detections first_assosiation(std::vector& _detections); 125 | private: 126 | KalmanParam param; 127 | Detections last_detection; 128 | Detections prev_unassigned; 129 | Tracks single_tracks; 130 | Tracks old_tracks; 131 | Entities tracks; 132 | uint width, height; 133 | cv::RNG rng; 134 | uint trackIds; 135 | std::vector streams; 136 | //storing the probabilities of the camera system 137 | //each value represensts: 138 | // -prob that the obs is in the FOV of all the cameras given the proximity of the camera 139 | std::vector cameraProbabilities; 140 | int numCams; 141 | private: 142 | static constexpr float freezed_thresh = 0.4; 143 | static constexpr uint association_thresh = 40; 144 | }; 145 | } 146 | } 147 | 148 | #endif 149 | -------------------------------------------------------------------------------- /src/tracker/src/hungarianAlg.cpp: -------------------------------------------------------------------------------- 1 | #include "hungarianAlg.h" 2 | 3 | using namespace mctracker::tracker::costs; 4 | 5 | track_t 6 | AssignmentProblemSolver::Solve(const distMatrix_t& distMatrixIn, const size_t& nOfRows, 7 | const size_t& nOfColumns, std::vector& assignment, const TMethod& Method) 8 | { 9 | assignment.resize(nOfRows, -1); 10 | 11 | track_t cost = 0; 12 | 13 | switch (Method) 14 | { 15 | case optimal: 16 | assignmentoptimal(assignment, cost, distMatrixIn, nOfRows, nOfColumns); 17 | break; 18 | 19 | case many_forbidden_assignments: 20 | assignmentsuboptimal1(assignment, cost, distMatrixIn, nOfRows, nOfColumns); 21 | break; 22 | 23 | case without_forbidden_assignments: 24 | assignmentsuboptimal2(assignment, cost, distMatrixIn, nOfRows, nOfColumns); 25 | break; 26 | } 27 | 28 | return cost; 29 | } 30 | 31 | // -------------------------------------------------------------------------- 32 | // Computes the optimal assignment (minimum overall costs) using Munkres algorithm. 33 | // -------------------------------------------------------------------------- 34 | void 35 | AssignmentProblemSolver::assignmentoptimal(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows, const size_t& nOfColumns) 36 | { 37 | // Generate distance cv::Matrix 38 | // and check cv::Matrix elements positiveness :) 39 | 40 | // Total elements number 41 | const size_t& nOfElements = nOfRows * nOfColumns; 42 | // Memory allocation 43 | 44 | distMatrix_t distMatrix(nOfElements); 45 | // Pointer to last element 46 | track_t* distMatrixEnd = distMatrix.data() + nOfElements; 47 | 48 | track_t value; 49 | for (size_t row = 0; row < nOfElements; ++row) 50 | { 51 | value = distMatrixIn[row]; 52 | assert(value >= 0); 53 | distMatrix[row] = value; 54 | } 55 | 56 | // Memory allocation 57 | BoolVec coveredColumns(nOfColumns, 0); 58 | BoolVec coveredRows(nOfRows, 0); 59 | BoolVec starMatrix(nOfElements, 0); 60 | BoolVec primeMatrix(nOfElements, 0); 61 | BoolVec newStarMatrix(nOfElements, 0); /* used in step4 */ 62 | 63 | /* preliminary steps */ 64 | if (nOfRows <= nOfColumns) 65 | { 66 | track_t minValue; 67 | track_t* distMatrixTemp; 68 | track_t* distMatrix_ptr = distMatrix.data(); 69 | for (size_t row = 0; row < nOfRows; row++) 70 | { 71 | /* find the smallest element in the row */ 72 | distMatrixTemp = distMatrix_ptr + row; 73 | minValue = *distMatrixTemp; 74 | distMatrixTemp += nOfRows; 75 | while (distMatrixTemp < distMatrixEnd) 76 | { 77 | track_t value = *distMatrixTemp; 78 | if (value < minValue) 79 | { 80 | minValue = value; 81 | } 82 | distMatrixTemp += nOfRows; 83 | } 84 | /* subtract the smallest element from each element of the row */ 85 | distMatrixTemp = distMatrix_ptr + row; 86 | while (distMatrixTemp < distMatrixEnd) 87 | { 88 | *distMatrixTemp -= minValue; 89 | distMatrixTemp += nOfRows; 90 | } 91 | } 92 | /* Steps 1 and 2a */ 93 | for (size_t row = 0; row < nOfRows; row++) 94 | { 95 | for (size_t col = 0; col < nOfColumns; col++) 96 | { 97 | if (distMatrix[row + nOfRows*col] == 0) 98 | { 99 | if (!coveredColumns[col]) 100 | { 101 | starMatrix[row + nOfRows * col] = true; 102 | coveredColumns[col] = true; 103 | break; 104 | } 105 | } 106 | } 107 | } 108 | } 109 | else /* if(nOfRows > nOfColumns) */ 110 | { 111 | track_t* distMatrixTemp; 112 | track_t* columnEnd; 113 | track_t minValue; 114 | track_t* distMatrix_ptr = distMatrix.data(); 115 | for (size_t col = 0; col < nOfColumns; col++) 116 | { 117 | /* find the smallest element in the column */ 118 | distMatrixTemp = distMatrix_ptr + nOfRows*col; 119 | columnEnd = distMatrixTemp + nOfRows; 120 | minValue = *distMatrixTemp++; 121 | 122 | while (distMatrixTemp < columnEnd) 123 | { 124 | track_t value = *distMatrixTemp++; 125 | if (value < minValue) 126 | { 127 | minValue = value; 128 | } 129 | } 130 | 131 | /* subtract the smallest element from each element of the column */ 132 | distMatrixTemp = distMatrix_ptr + nOfRows*col; 133 | while (distMatrixTemp < columnEnd) 134 | { 135 | *distMatrixTemp++ -= minValue; 136 | } 137 | } 138 | /* Steps 1 and 2a */ 139 | for (size_t col = 0; col < nOfColumns; col++) 140 | { 141 | for (size_t row = 0; row < nOfRows; row++) 142 | { 143 | if (distMatrix[row + nOfRows*col] == 0) 144 | { 145 | if (!coveredRows[row]) 146 | { 147 | starMatrix[row + nOfRows*col] = true; 148 | coveredColumns[col] = true; 149 | coveredRows[row] = true; 150 | break; 151 | } 152 | } 153 | } 154 | } 155 | 156 | for (size_t row = 0; row < nOfRows; row++) 157 | { 158 | coveredRows[row] = false; 159 | } 160 | } 161 | /* move to step 2b */ 162 | step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, (nOfRows <= nOfColumns) ? nOfRows : nOfColumns); 163 | /* compute cost and remove invalid assignments */ 164 | computeassignmentcost(assignment, cost, distMatrixIn, nOfRows); 165 | 166 | return; 167 | } 168 | 169 | // -------------------------------------------------------------------------- 170 | // 171 | // -------------------------------------------------------------------------- 172 | void 173 | AssignmentProblemSolver::buildassignmentvector(assignments_t& assignment, BoolVec& starMatrix, const size_t& nOfRows, const size_t& nOfColumns) 174 | { 175 | for (size_t row = 0; row < nOfRows; row++) 176 | { 177 | for (size_t col = 0; col < nOfColumns; col++) 178 | { 179 | if (starMatrix[row + nOfRows * col]) 180 | { 181 | assignment[row] = static_cast(col); 182 | break; 183 | } 184 | } 185 | } 186 | } 187 | // -------------------------------------------------------------------------- 188 | // 189 | // -------------------------------------------------------------------------- 190 | void 191 | AssignmentProblemSolver::computeassignmentcost(const assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows) 192 | { 193 | for (size_t row = 0; row < nOfRows; row++) 194 | { 195 | const int col = assignment[row]; 196 | if (col >= 0) 197 | { 198 | cost += distMatrixIn[row + nOfRows * col]; 199 | } 200 | } 201 | } 202 | 203 | // -------------------------------------------------------------------------- 204 | // 205 | // -------------------------------------------------------------------------- 206 | void 207 | AssignmentProblemSolver::step2a(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim) 208 | { 209 | uint *starMatrixTemp, *columnEnd; 210 | /* cover every column containing a starred zero */ 211 | uint* starMatrix_ptr = starMatrix.data(); 212 | for (size_t col = 0; col < nOfColumns; col++) 213 | { 214 | starMatrixTemp = starMatrix_ptr + nOfRows * col; 215 | columnEnd = starMatrixTemp + nOfRows; 216 | while (starMatrixTemp < columnEnd) 217 | { 218 | if (*starMatrixTemp++) 219 | { 220 | coveredColumns[col] = true; 221 | break; 222 | } 223 | } 224 | } 225 | /* move to step 3 */ 226 | step2b(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 227 | } 228 | 229 | // -------------------------------------------------------------------------- 230 | // 231 | // -------------------------------------------------------------------------- 232 | void 233 | AssignmentProblemSolver::step2b(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim) 234 | { 235 | /* count covered columns */ 236 | size_t nOfCoveredColumns = 0; 237 | for (size_t col = 0; col < nOfColumns; col++) 238 | { 239 | if (coveredColumns[col]) 240 | { 241 | nOfCoveredColumns++; 242 | } 243 | } 244 | 245 | if (nOfCoveredColumns == minDim) 246 | { 247 | /* algorithm finished */ 248 | buildassignmentvector(assignment, starMatrix, nOfRows, nOfColumns); 249 | } 250 | else 251 | { 252 | /* move to step 3 */ 253 | step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 254 | } 255 | } 256 | 257 | // -------------------------------------------------------------------------- 258 | // 259 | // -------------------------------------------------------------------------- 260 | void 261 | AssignmentProblemSolver::step3(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim) 262 | { 263 | bool zerosFound = true; 264 | while (zerosFound) 265 | { 266 | zerosFound = false; 267 | for (size_t col = 0; col < nOfColumns; col++) 268 | { 269 | if (!coveredColumns[col]) 270 | { 271 | for (size_t row = 0; row < nOfRows; row++) 272 | { 273 | if ((!coveredRows[row]) && (distMatrix[row + nOfRows*col] == 0)) 274 | { 275 | /* prime zero */ 276 | primeMatrix[row + nOfRows*col] = true; 277 | /* find starred zero in current row */ 278 | size_t starCol = 0; 279 | for (; starCol < nOfColumns; starCol++) 280 | { 281 | if (starMatrix[row + nOfRows * starCol]) 282 | { 283 | break; 284 | } 285 | } 286 | if (starCol == nOfColumns) /* no starred zero found */ 287 | { 288 | /* move to step 4 */ 289 | step4(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim, row, col); 290 | return; 291 | } 292 | else 293 | { 294 | coveredRows[row] = true; 295 | coveredColumns[starCol] = false; 296 | zerosFound = true; 297 | break; 298 | } 299 | } 300 | } 301 | } 302 | } 303 | } 304 | /* move to step 5 */ 305 | step5(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 306 | } 307 | 308 | // -------------------------------------------------------------------------- 309 | // 310 | // -------------------------------------------------------------------------- 311 | void 312 | AssignmentProblemSolver::step4(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim, const size_t& row, const size_t& col) 313 | { 314 | const size_t& nOfElements = nOfRows * nOfColumns; 315 | /* generate temporary copy of starMatrix */ 316 | for (size_t n = 0; n < nOfElements; n++) 317 | { 318 | newStarMatrix[n] = starMatrix[n]; 319 | } 320 | /* star current zero */ 321 | newStarMatrix[row + nOfRows*col] = true; 322 | /* find starred zero in current column */ 323 | size_t starCol = col; 324 | size_t starRow = 0; 325 | for (; starRow < nOfRows; starRow++) 326 | { 327 | if (starMatrix[starRow + nOfRows * starCol]) 328 | { 329 | break; 330 | } 331 | } 332 | 333 | while (starRow < nOfRows) 334 | { 335 | /* unstar the starred zero */ 336 | newStarMatrix[starRow + nOfRows*starCol] = false; 337 | /* find primed zero in current row */ 338 | size_t primeRow = starRow; 339 | size_t primeCol = 0; 340 | for (; primeCol < nOfColumns; primeCol++) 341 | { 342 | if (primeMatrix[primeRow + nOfRows * primeCol]) 343 | { 344 | break; 345 | } 346 | } 347 | 348 | /* star the primed zero */ 349 | newStarMatrix[primeRow + nOfRows*primeCol] = true; 350 | /* find starred zero in current column */ 351 | starCol = primeCol; 352 | for (starRow = 0; starRow < nOfRows; starRow++) 353 | { 354 | if (starMatrix[starRow + nOfRows * starCol]) 355 | { 356 | break; 357 | } 358 | } 359 | } 360 | /* use temporary copy as new starMatrix */ 361 | /* delete all primes, uncover all rows */ 362 | for (size_t n = 0; n < nOfElements; n++) 363 | { 364 | primeMatrix[n] = false; 365 | starMatrix[n] = newStarMatrix[n]; 366 | } 367 | 368 | for (size_t n = 0; n < nOfRows; n++) 369 | { 370 | coveredRows[n] = false; 371 | } 372 | /* move to step 2a */ 373 | step2a(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 374 | } 375 | 376 | // -------------------------------------------------------------------------- 377 | // 378 | // -------------------------------------------------------------------------- 379 | void 380 | AssignmentProblemSolver::step5(assignments_t& assignment, distMatrix_t& distMatrix, BoolVec& starMatrix, BoolVec& newStarMatrix, BoolVec& primeMatrix, BoolVec& coveredColumns, BoolVec& coveredRows, const size_t& nOfRows, const size_t& nOfColumns, const size_t& minDim) 381 | { 382 | /* find smallest uncovered element h */ 383 | float h = std::numeric_limits::max(); 384 | for (size_t row = 0; row < nOfRows; row++) 385 | { 386 | if (!coveredRows[row]) 387 | { 388 | for (size_t col = 0; col < nOfColumns; col++) 389 | { 390 | if (!coveredColumns[col]) 391 | { 392 | const float& value = distMatrix[row + nOfRows*col]; 393 | if (value < h) 394 | { 395 | h = value; 396 | } 397 | } 398 | } 399 | } 400 | } 401 | 402 | /* add h to each covered row */ 403 | for (size_t row = 0; row < nOfRows; row++) 404 | { 405 | if (coveredRows[row]) 406 | { 407 | for (size_t col = 0; col < nOfColumns; col++) 408 | { 409 | distMatrix[row + nOfRows*col] += h; 410 | } 411 | } 412 | } 413 | 414 | /* subtract h from each uncovered column */ 415 | for (size_t col = 0; col < nOfColumns; col++) 416 | { 417 | if (!coveredColumns[col]) 418 | { 419 | for (size_t row = 0; row < nOfRows; row++) 420 | { 421 | distMatrix[row + nOfRows*col] -= h; 422 | } 423 | } 424 | } 425 | 426 | /* move to step 3 */ 427 | step3(assignment, distMatrix, starMatrix, newStarMatrix, primeMatrix, coveredColumns, coveredRows, nOfRows, nOfColumns, minDim); 428 | } 429 | 430 | 431 | // -------------------------------------------------------------------------- 432 | // Computes a suboptimal solution. Good for cases without forbidden assignments. 433 | // -------------------------------------------------------------------------- 434 | void 435 | AssignmentProblemSolver::assignmentsuboptimal2(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows, const size_t& nOfColumns) 436 | { 437 | /* make working copy of distance Matrix */ 438 | const size_t& nOfElements = nOfRows * nOfColumns; 439 | 440 | distMatrix_t distMatrix(nOfElements); 441 | 442 | for (size_t n = 0; n < nOfElements; n++) 443 | { 444 | distMatrix[n] = distMatrixIn[n]; 445 | } 446 | 447 | float minValue; 448 | size_t tmpRow; 449 | size_t tmpCol; 450 | 451 | /* recursively search for the minimum element and do the assignment */ 452 | for (;;) 453 | { 454 | /* find minimum distance observation-to-track pair */ 455 | minValue = std::numeric_limits::max(); 456 | tmpRow = 0; 457 | tmpCol = 0; 458 | for (size_t row = 0; row < nOfRows; row++) 459 | { 460 | for (size_t col = 0; col < nOfColumns; col++) 461 | { 462 | const float& value = distMatrix[row + nOfRows*col]; 463 | if (value != std::numeric_limits::max() && (value < minValue)) 464 | { 465 | minValue = value; 466 | tmpRow = row; 467 | tmpCol = col; 468 | } 469 | } 470 | } 471 | 472 | if (minValue != std::numeric_limits::max()) 473 | { 474 | assignment[tmpRow] = static_cast(tmpCol); 475 | cost += minValue; 476 | for (size_t n = 0; n < nOfRows; n++) 477 | { 478 | distMatrix[n + nOfRows*tmpCol] = std::numeric_limits::max(); 479 | } 480 | for (size_t n = 0; n < nOfColumns; n++) 481 | { 482 | distMatrix[tmpRow + nOfRows*n] = std::numeric_limits::max(); 483 | } 484 | } 485 | else 486 | { 487 | break; 488 | } 489 | } 490 | } 491 | // -------------------------------------------------------------------------- 492 | // Computes a suboptimal solution. Good for cases with many forbidden assignments. 493 | // -------------------------------------------------------------------------- 494 | void 495 | AssignmentProblemSolver::assignmentsuboptimal1(assignments_t& assignment, track_t& cost, const distMatrix_t& distMatrixIn, const size_t& nOfRows, const size_t& nOfColumns) 496 | { 497 | /* make working copy of distance Matrix */ 498 | // const size_t& nOfElements = nOfRows * nOfColumns; 499 | 500 | distMatrix_t distMatrix = distMatrixIn; 501 | 502 | /* allocate memory */ 503 | assignments_t nOfValidObservations(nOfRows, 0); 504 | assignments_t nOfValidTracks(nOfColumns, 0); 505 | 506 | /* compute number of validations */ 507 | bool infiniteValueFound = false; 508 | bool finiteValueFound = false; 509 | 510 | for (size_t row = 0; row < nOfRows; row++) 511 | { 512 | for (size_t col = 0; col < nOfColumns; col++) 513 | { 514 | if (distMatrix[row + nOfRows*col] != std::numeric_limits::max()) 515 | { 516 | nOfValidTracks[col] += 1; 517 | nOfValidObservations[row] += 1; 518 | finiteValueFound = true; 519 | } 520 | else 521 | { 522 | infiniteValueFound = true; 523 | } 524 | } 525 | } 526 | 527 | if (infiniteValueFound) 528 | { 529 | if (!finiteValueFound) 530 | { 531 | return; 532 | } 533 | bool repeatSteps = true; 534 | 535 | while (repeatSteps) 536 | { 537 | repeatSteps = false; 538 | bool singleValidationFound; 539 | /* step 1: reject assignments of multiply validated tracks to singly validated observations */ 540 | for (size_t col = 0; col < nOfColumns; col++) 541 | { 542 | singleValidationFound = false; 543 | for (size_t row = 0; row < nOfRows; row++) 544 | { 545 | if (distMatrix[row + nOfRows * col] != std::numeric_limits::max() && (nOfValidObservations[row] == 1)) 546 | { 547 | singleValidationFound = true; 548 | break; 549 | } 550 | 551 | if (singleValidationFound) 552 | { 553 | for (size_t row = 0; row < nOfRows; row++) 554 | { 555 | if ((nOfValidObservations[row] > 1) && distMatrix[row + nOfRows*col] != std::numeric_limits::max()) 556 | { 557 | distMatrix[row + nOfRows*col] = std::numeric_limits::max(); 558 | nOfValidObservations[row] -= 1; 559 | nOfValidTracks[col] -= 1; 560 | repeatSteps = true; 561 | } 562 | } 563 | } 564 | } 565 | } 566 | 567 | /* step 2: reject assignments of multiply validated observations to singly validated tracks */ 568 | if (nOfColumns > 1) 569 | { 570 | bool singleValidationFound; 571 | for (size_t row = 0; row < nOfRows; row++) 572 | { 573 | // singleValidationFound = false; 574 | for (size_t col = 0; col < nOfColumns; col++) 575 | { 576 | if (distMatrix[row + nOfRows*col] != std::numeric_limits::max() && (nOfValidTracks[col] == 1)) 577 | { 578 | singleValidationFound = true; 579 | break; 580 | } 581 | } 582 | 583 | if (singleValidationFound) 584 | { 585 | for (size_t col = 0; col < nOfColumns; col++) 586 | { 587 | if ((nOfValidTracks[col] > 1) && distMatrix[row + nOfRows*col] != std::numeric_limits::max()) 588 | { 589 | distMatrix[row + nOfRows*col] = std::numeric_limits::max(); 590 | nOfValidObservations[row] -= 1; 591 | nOfValidTracks[col] -= 1; 592 | repeatSteps = true; 593 | } 594 | } 595 | } 596 | } 597 | } 598 | } /* while(repeatSteps) */ 599 | 600 | /* for each multiply validated track that validates only with singly validated */ 601 | /* observations, choose the observation with minimum distance */ 602 | for (size_t row = 0; row < nOfRows; row++) 603 | { 604 | if (nOfValidObservations[row] > 1) 605 | { 606 | bool allSinglyValidated = true; 607 | float minValue = std::numeric_limits::max(); 608 | size_t tmpCol = 0; 609 | for (size_t col = 0; col < nOfColumns; col++) 610 | { 611 | const float& value = distMatrix[row + nOfRows*col]; 612 | if (value != std::numeric_limits::max()) 613 | { 614 | if (nOfValidTracks[col] > 1) 615 | { 616 | allSinglyValidated = false; 617 | break; 618 | } 619 | else if ((nOfValidTracks[col] == 1) && (value < minValue)) 620 | { 621 | tmpCol = col; 622 | minValue = value; 623 | } 624 | } 625 | } 626 | 627 | if (allSinglyValidated) 628 | { 629 | assignment[row] = static_cast(tmpCol); 630 | cost += minValue; 631 | for (size_t n = 0; n < nOfRows; n++) 632 | { 633 | distMatrix[n + nOfRows*tmpCol] = std::numeric_limits::max(); 634 | } 635 | for (size_t n = 0; n < nOfColumns; n++) 636 | { 637 | distMatrix[row + nOfRows*n] = std::numeric_limits::max(); 638 | } 639 | } 640 | } 641 | } 642 | 643 | // for each multiply validated observation that validates only with singly validated track, choose the track with minimum distance 644 | for (size_t col = 0; col < nOfColumns; col++) 645 | { 646 | if (nOfValidTracks[col] > 1) 647 | { 648 | bool allSinglyValidated = true; 649 | float minValue = std::numeric_limits::max(); 650 | size_t tmpRow = 0; 651 | for (size_t row = 0; row < nOfRows; row++) 652 | { 653 | const float& value = distMatrix[row + nOfRows*col]; 654 | if (value != std::numeric_limits::max()) 655 | { 656 | if (nOfValidObservations[row] > 1) 657 | { 658 | allSinglyValidated = false; 659 | break; 660 | } 661 | else if ((nOfValidObservations[row] == 1) && (value < minValue)) 662 | { 663 | tmpRow = row; 664 | minValue = value; 665 | } 666 | } 667 | } 668 | 669 | if (allSinglyValidated) 670 | { 671 | assignment[tmpRow] = static_cast(col); 672 | cost += minValue; 673 | for (size_t n = 0; n < nOfRows; n++) 674 | { 675 | distMatrix[n + nOfRows*col] = std::numeric_limits::max(); 676 | } 677 | 678 | for (size_t n = 0; n < nOfColumns; n++) 679 | { 680 | distMatrix[tmpRow + nOfRows*n] = std::numeric_limits::max(); 681 | } 682 | } 683 | } 684 | } 685 | } /* if(infiniteValueFound) */ 686 | 687 | 688 | /* now, recursively search for the minimum element and do the assignment */ 689 | for (;;) 690 | { 691 | /* find minimum distance observation-to-track pair */ 692 | float minValue = std::numeric_limits::max(); 693 | size_t tmpRow = 0; 694 | size_t tmpCol = 0; 695 | for (size_t row = 0; row < nOfRows; row++) 696 | { 697 | for (size_t col = 0; col < nOfColumns; col++) 698 | { 699 | const float& value = distMatrix[row + nOfRows*col]; 700 | if (value != std::numeric_limits::max() && (value < minValue)) 701 | { 702 | minValue = value; 703 | tmpRow = row; 704 | tmpCol = col; 705 | } 706 | } 707 | } 708 | 709 | if (minValue != std::numeric_limits::max()) 710 | { 711 | assignment[tmpRow] = static_cast(tmpCol); 712 | cost += minValue; 713 | for (size_t n = 0; n < nOfRows; n++) 714 | { 715 | distMatrix[n + nOfRows*tmpCol] = std::numeric_limits::max(); 716 | } 717 | for (size_t n = 0; n < nOfColumns; n++) 718 | { 719 | distMatrix[tmpRow + nOfRows*n] = std::numeric_limits::max(); 720 | } 721 | } 722 | else 723 | { 724 | break; 725 | } 726 | } 727 | } 728 | -------------------------------------------------------------------------------- /src/tracker/src/hypothesis.cpp: -------------------------------------------------------------------------------- 1 | #include "hypothesis.h" 2 | 3 | using namespace mctracker::tracker; 4 | 5 | std::shared_ptr Hyphothesis::m_instance = nullptr; 6 | 7 | std::shared_ptr< Hyphothesis > 8 | Hyphothesis::instance() 9 | { 10 | if(!m_instance) 11 | { 12 | m_instance = std::shared_ptr(new Hyphothesis); 13 | } 14 | 15 | return m_instance; 16 | } 17 | 18 | void 19 | Hyphothesis::new_hyphothesis(const cv::Mat& dummmy_assignments, Tracks& tracks, const Detections& detections, const uint& w, const uint& h, 20 | const uint& new_hyp_dummy_costs, Detections& prev_unassigned, const KalmanParam& param, const int cameraNum) 21 | { 22 | cv::Mat assignments = dummmy_assignments.clone(); 23 | 24 | assignments.setTo(1, assignments == 255); 25 | 26 | //Unassigned observations 27 | cv::Mat ass_sum; 28 | cv::reduce(assignments, ass_sum, 0, CV_REDUCE_SUM, CV_32S); 29 | 30 | cv::Mat tmp_unassigned = ass_sum == 0; 31 | cv::Mat unassigned; 32 | cv::findNonZero(tmp_unassigned, unassigned); 33 | 34 | uint nunassigned = unassigned.total(); 35 | 36 | cv::Mat new_hyp = cv::Mat(cv::Size(prev_unassigned.size(), nunassigned), CV_32FC1, cv::Scalar(0)); 37 | distMatrix_t cost(prev_unassigned.size() * nunassigned); 38 | assignments_t new_assignments; 39 | 40 | if(cost.size() != 0) 41 | { 42 | float amp; 43 | cv::Point2f elem; 44 | const uint& nHrows = (uint) new_hyp.rows; 45 | const uint& nHcols = (uint) new_hyp.cols; 46 | //Compute the cost between the current detections and the previous 47 | for(uint i = 0; i < nHrows; ++i) 48 | { 49 | const uint idx = unassigned.at(i).x; 50 | elem = (cv::Point2f)detections.at(idx)(); 51 | for(uint j = 0; j < nHcols; ++j) 52 | { 53 | // How "good" are tracks that started at one of the previously 54 | // unassigned observations? The cost amplification is high in 55 | // the middle, but low on the sides. 56 | amp = beta_likelihood(prev_unassigned.at(j)(), 1.5, 1.5, w, h); 57 | 58 | elem = prev_unassigned.at(j)() - elem; 59 | 60 | new_hyp.at(i, j) = amp * sqrt(elem.x * elem.x + elem.y * elem.y); 61 | cost.at(i + j * new_hyp.rows ) = amp * new_hyp.at(i, j); 62 | } 63 | } 64 | 65 | 66 | const uint& rows = new_hyp.rows; 67 | const uint& cols = new_hyp.cols; 68 | 69 | uint n_w, n_h; 70 | if(rows < cols) 71 | { 72 | n_w = 2*cols; 73 | n_h = 2*(rows + (cols - rows)); 74 | } 75 | else if (rows > cols) 76 | { 77 | n_w = 2*(cols + (rows - cols)); 78 | n_h = 2 * rows; 79 | } 80 | else 81 | { 82 | n_w = cols; 83 | n_h = rows; 84 | } 85 | 86 | 87 | cv::Mat lap_costs = cv::Mat(cv::Size(n_w, n_h), CV_32SC1, cv::Scalar(new_hyp_dummy_costs)); 88 | new_hyp.copyTo(lap_costs(cv::Rect(0, 0, cols, rows))); 89 | 90 | 91 | std::vector colsol(n_w); 92 | std::vector rowsol(n_w); 93 | 94 | std::vector u(n_w), v(n_w); 95 | 96 | std::vector< std::vector > lap_costs_matrix(n_h, std::vector(n_w)); 97 | for(uint i = 0; i < n_h; ++i) 98 | { 99 | for(uint j = 0; j < n_w; ++j) 100 | { 101 | lap_costs_matrix[i][j] = lap_costs.at(i, j); 102 | } 103 | } 104 | 105 | LapCost::instance()->lap(n_w, lap_costs_matrix, rowsol, colsol, u, v); 106 | 107 | new_hyp = cv::Mat(cv::Size(n_w, n_h), CV_32SC1, cv::Scalar(0)); 108 | 109 | 110 | for(uint i = 0; i < n_w; ++i) 111 | { 112 | new_hyp.at((i*n_w)+rowsol.at(i)) = 1; 113 | } 114 | 115 | 116 | cv::Mat new_assigments = new_hyp(cv::Rect(0, 0, cols, rows)); 117 | new_assigments.convertTo(new_assigments, CV_32S); 118 | 119 | if(new_assigments.total() > 1) 120 | { 121 | ass_sum = cv::Mat(cv::Size(1, rows), new_assigments.type(), cv::Scalar(0)); 122 | const uint& nAcols = new_assigments.cols; 123 | for(uint i = 0; i < nAcols; ++i) 124 | { 125 | ass_sum += new_assigments(cv::Rect(i, 0, 1, rows)); 126 | } 127 | } 128 | else 129 | { 130 | ass_sum = new_assigments.clone(); 131 | } 132 | 133 | 134 | if(ass_sum.cols > 1) 135 | { 136 | std::cout << "ass_sum.cols > 1 " << std::endl; 137 | exit(-1); 138 | } 139 | 140 | tmp_unassigned = ass_sum == 1; 141 | cv::Mat new_unassigned; 142 | cv::findNonZero(tmp_unassigned, new_unassigned); 143 | 144 | //create new tracks if needed 145 | if(new_unassigned.total() != 0) 146 | { 147 | const uint& nUtotal = new_unassigned.total(); 148 | for(uint i = 0; i < nUtotal; ++i) 149 | { 150 | const int idx = unassigned.at(new_unassigned.at(i).y).x; 151 | Track_ptr tr(new Track(detections.at(idx).x(), detections.at(idx).y(), param, detections.at(idx).hist(), cameraNum)); 152 | tracks.push_back(tr); 153 | } 154 | } 155 | } 156 | 157 | prev_unassigned.clear(); 158 | const uint& dSize = detections.size(); 159 | for(uint i = 0; i < dSize; ++i) 160 | { 161 | if(ass_sum.at(i) == 0 || 162 | (new_assignments.size() > 0 && std::find(new_assignments.begin(), new_assignments.end(), i) == new_assignments.end())) 163 | { 164 | prev_unassigned.push_back(detections.at(i)); 165 | } 166 | } 167 | } 168 | 169 | float 170 | Hyphothesis::beta_likelihood(const cv::Point2f& prev_unassigned, const float& alpha, const float& beta, const uint& w, const uint& h) 171 | { 172 | const float& x = prev_unassigned.x / float(w); 173 | const float& y = prev_unassigned.y / float(h); 174 | 175 | const float& norm = gamma(alpha + beta) / gamma(alpha) * gamma(beta); 176 | 177 | const float& likelihood_x = norm * std::pow(x, alpha - 1) * std::pow((1 - x),(beta - 1)); 178 | const float& likelihood_y = norm * std::pow(y, alpha - 1) * std::pow((1 - y),(beta - 1)); 179 | 180 | return likelihood_x * likelihood_y; 181 | } 182 | 183 | static std::vector c = {0.99999999999999709182, 57.156235665862923517, -59.597960355475491248, 184 | 14.136097974741747174, -0.49191381609762019978, .000033994649984811888699, 185 | .000046523628927048575665, -.000098374475304879564677, .00015808870322491248884, 186 | -.00021026444172410488319, .00021743961811521264320, -.00016431810653676389022, 187 | .000084418223983852743293, -.000026190838401581408670, .0000036899182659531622704}; 188 | 189 | // Gamma function valid in the entire complex plane. 190 | // Accuracy is 15 significant digits along the real axis 191 | // and 13 significant digits elsewhere. 192 | // This routine uses a superb Lanczos series 193 | // approximation for the complex Gamma function. 194 | 195 | float 196 | Hyphothesis::gamma(const float& z) 197 | { 198 | const float& m_z = z - 1; 199 | const float& m_zh = m_z + .5; 200 | const float& m_zgh = m_zh + g; 201 | 202 | //trick for avoiding FP overflow above z=141 203 | const float& zp = std::pow(m_zgh, m_zh*.5); 204 | 205 | float ss = .0; 206 | for(int i = 13; i >= 0; --i) 207 | { 208 | ss += (c.at(i+1) / (z + (i+1))); 209 | } 210 | 211 | return (sq2pi * (c.at(0) + ss) * (zp * exp(-m_zgh) * zp)); 212 | } 213 | 214 | 215 | 216 | -------------------------------------------------------------------------------- /src/tracker/src/kalman.cpp: -------------------------------------------------------------------------------- 1 | #include "kalman.h" 2 | 3 | using namespace mctracker::tracker; 4 | 5 | 6 | KalmanFilter 7 | ::KalmanFilter(const float &_x, const float &_y, const float &dt) 8 | { 9 | 10 | measurement = cv::Mat_(2, 1, CV_32F); 11 | measurement.setTo(cv::Scalar(0)); 12 | 13 | KF = cv::KalmanFilter(4, 2, 0); 14 | state = cv::Mat_(4, 1); 15 | processNoise = cv::Mat(4, 1, CV_32F); 16 | measurement = cv::Mat_(2, 1); 17 | measurement.setTo(cv::Scalar(0)); 18 | 19 | KF.statePre.at(0, 0) = _x; 20 | KF.statePre.at(1, 0) = _y; 21 | KF.statePre.at(2) = 0; 22 | KF.statePre.at(3) = 0; 23 | 24 | KF.statePost.at(0) = _x; 25 | KF.statePost.at(1) = _y; 26 | KF.statePost.at(2) = 0; 27 | KF.statePost.at(3) = 0; 28 | 29 | KF.transitionMatrix = (cv::Mat_(4, 4) << 1,0,dt,0, 30 | 0,1,0,dt, 31 | 0,0,1,0, 32 | 0,0,0,1); 33 | 34 | cv::setIdentity(KF.measurementMatrix); 35 | 36 | KF.processNoiseCov=(cv::Mat_(4, 4) << 37 | pow(dt,4.0)/4.0, 0, pow(dt,3.0)/2.0, 0, 38 | 0, pow(dt,4.0)/4.0, 0, pow(dt,3.0)/2.0, 39 | pow(dt,3.0)/2.0, 0, pow(dt,2.0), 0, 40 | 0, pow(dt,3.0)/2.0, 0, pow(dt,2.0)); 41 | 42 | KF.processNoiseCov*=.3; 43 | cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(1e-0)); 44 | cv::setIdentity(KF.errorCovPost, cv::Scalar::all(0.5)); 45 | 46 | prediction = cv::Mat(cv::Size(4, 1), CV_32FC1); 47 | prediction.at(0) = _x; 48 | prediction.at(1) = _y; 49 | prediction.at(2) = 0; 50 | prediction.at(3) = 0; 51 | } 52 | 53 | cv::Mat 54 | KalmanFilter::predict() 55 | { 56 | prediction = KF.predict(); 57 | KF.statePre.copyTo(KF.statePost); 58 | KF.errorCovPre.copyTo(KF.errorCovPost); 59 | 60 | return prediction; 61 | } 62 | 63 | cv::Mat 64 | KalmanFilter::correct(const int &_x, const int &_y) 65 | { 66 | measurement(0) = _x; 67 | measurement(1) = _y; 68 | const auto& estimated = KF.correct(measurement); 69 | return estimated; 70 | } 71 | -------------------------------------------------------------------------------- /src/tracker/src/lap.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * 3 | * lap.cpp 4 | version 1.0 - 4 September 1996 5 | author: Roy Jonker @ MagicLogic Optimization Inc. 6 | e-mail: roy_jonker@magiclogic.com 7 | 8 | Code for Linear Assignment Problem, according to 9 | 10 | "A Shortest Augmenting Path Algorithm for Dense and Sparse Linear 11 | Assignment Problems", Computing 38, 325-340, 1987 12 | 13 | by 14 | 15 | R. Jonker and A. Volgenant, University of Amsterdam. 16 | 17 | Modified by Andrea Pennisi 18 | * 19 | *************************************************************************/ 20 | #include "lap.h" 21 | 22 | using namespace mctracker::tracker::costs; 23 | 24 | std::shared_ptr LapCost::m_instance = nullptr; 25 | 26 | std::shared_ptr< LapCost > 27 | LapCost::instance() 28 | { 29 | if(!m_instance) 30 | { 31 | m_instance = std::shared_ptr(new LapCost); 32 | } 33 | return m_instance; 34 | } 35 | 36 | 37 | 38 | int 39 | LapCost::lap(int dim, const std::vector< std::vector >& assigncost, 40 | std::vector& rowsol, std::vector& colsol, std::vector& u, std::vector& v) 41 | 42 | // input: 43 | // dim - problem size 44 | // assigncost - cost matrix 45 | 46 | // output: 47 | // rowsol - column assigned to row in solution 48 | // colsol - row assigned to column in solution 49 | // u - dual variables, row reduction numbers 50 | // v - dual variables, column reduction numbers 51 | 52 | { 53 | bool unassignedfound; 54 | row i, imin, numfree = 0, prvnumfree, f, i0, k, freerow, *pred, *free; 55 | col j, j1, j2, endofpath, last, low, up, *collist, *matches; 56 | cost min, h, umin, usubmin, v2, *d; 57 | 58 | free = new row[dim]; // list of unassigned rows. 59 | collist = new col[dim]; // list of columns to be scanned in various ways. 60 | matches = new col[dim]; // counts how many times a row could be assigned. 61 | d = new cost[dim]; // 'cost-distance' in augmenting path calculation. 62 | pred = new row[dim]; // row-predecessor of column in augmenting/alternating path. 63 | 64 | // init how many times a row will be assigned in the column reduction. 65 | for (i = 0; i < dim; i++) 66 | matches[i] = 0; 67 | 68 | // COLUMN REDUCTION 69 | for (j = dim-1; j >= 0; j--) // reverse order gives better results. 70 | { 71 | // find minimum cost over rows. 72 | min = assigncost[0][j]; 73 | imin = 0; 74 | for (i = 1; i < dim; i++) 75 | if (assigncost[i][j] < min) 76 | { 77 | min = assigncost[i][j]; 78 | imin = i; 79 | } 80 | v[j] = min; 81 | 82 | if (++matches[imin] == 1) 83 | { 84 | // init assignment if minimum row assigned for first time. 85 | rowsol[imin] = j; 86 | colsol[j] = imin; 87 | } 88 | else 89 | colsol[j] = -1; // row already assigned, column not assigned. 90 | } 91 | 92 | // REDUCTION TRANSFER 93 | for (i = 0; i < dim; i++) 94 | if (matches[i] == 0) // fill list of unassigned 'free' rows. 95 | free[numfree++] = i; 96 | else 97 | if (matches[i] == 1) // transfer reduction from rows that are assigned once. 98 | { 99 | j1 = rowsol[i]; 100 | min = BIG; 101 | for (j = 0; j < dim; j++) 102 | if (j != j1) 103 | if (assigncost[i][j] - v[j] < min) 104 | min = assigncost[i][j] - v[j]; 105 | v[j1] = v[j1] - min; 106 | } 107 | 108 | // AUGMENTING ROW REDUCTION 109 | int loopcnt = 0; // do-loop to be done twice. 110 | do 111 | { 112 | loopcnt++; 113 | 114 | // scan all free rows. 115 | // in some cases, a free row may be replaced with another one to be scanned next. 116 | k = 0; 117 | prvnumfree = numfree; 118 | numfree = 0; // start list of rows still free after augmenting row reduction. 119 | while (k < prvnumfree) 120 | { 121 | i = free[k]; 122 | k++; 123 | 124 | // find minimum and second minimum reduced cost over columns. 125 | umin = assigncost[i][0] - v[0]; 126 | j1 = 0; 127 | usubmin = BIG; 128 | for (j = 1; j < dim; j++) 129 | { 130 | h = assigncost[i][j] - v[j]; 131 | if (h < usubmin) 132 | { 133 | if (h >= umin) 134 | { 135 | usubmin = h; 136 | j2 = j; 137 | } 138 | else 139 | { 140 | usubmin = umin; 141 | umin = h; 142 | j2 = j1; 143 | j1 = j; 144 | } 145 | } 146 | } 147 | 148 | i0 = colsol[j1]; 149 | if (umin < usubmin) 150 | // change the reduction of the minimum column to increase the minimum 151 | // reduced cost in the row to the subminimum. 152 | v[j1] = v[j1] - (usubmin - umin); 153 | else // minimum and subminimum equal. 154 | if (i0 >= 0) // minimum column j1 is assigned. 155 | { 156 | // swap columns j1 and j2, as j2 may be unassigned. 157 | j1 = j2; 158 | i0 = colsol[j2]; 159 | } 160 | 161 | // (re-)assign i to j1, possibly de-assigning an i0. 162 | rowsol[i] = j1; 163 | colsol[j1] = i; 164 | 165 | if (i0 >= 0) // minimum column j1 assigned earlier. 166 | { 167 | if (umin < usubmin) 168 | // put in current k, and go back to that k. 169 | // continue augmenting path i - j1 with i0. 170 | free[--k] = i0; 171 | else 172 | // no further augmenting reduction possible. 173 | // store i0 in list of free rows for next phase. 174 | free[numfree++] = i0; 175 | } 176 | } 177 | } 178 | while (loopcnt < 2); // repeat once. 179 | 180 | // AUGMENT SOLUTION for each free row. 181 | for (f = 0; f < numfree; f++) 182 | { 183 | freerow = free[f]; // start row of augmenting path. 184 | 185 | // Dijkstra shortest path algorithm. 186 | // runs until unassigned column added to shortest path tree. 187 | for (j = 0; j < dim; j++) 188 | { 189 | d[j] = assigncost[freerow][j] - v[j]; 190 | pred[j] = freerow; 191 | collist[j] = j; // init column list. 192 | } 193 | 194 | low = 0; // columns in 0..low-1 are ready, now none. 195 | up = 0; // columns in low..up-1 are to be scanned for current minimum, now none. 196 | // columns in up..dim-1 are to be considered later to find new minimum, 197 | // at this stage the list simply contains all columns 198 | unassignedfound = false; 199 | do 200 | { 201 | if (up == low) // no more columns to be scanned for current minimum. 202 | { 203 | last = low - 1; 204 | 205 | // scan columns for up..dim-1 to find all indices for which new minimum occurs. 206 | // store these indices between low..up-1 (increasing up). 207 | min = d[collist[up++]]; 208 | for (k = up; k < dim; k++) 209 | { 210 | j = collist[k]; 211 | h = d[j]; 212 | if (h <= min) 213 | { 214 | if (h < min) // new minimum. 215 | { 216 | up = low; // restart list at index low. 217 | min = h; 218 | } 219 | // new index with same minimum, put on undex up, and extend list. 220 | collist[k] = collist[up]; 221 | collist[up++] = j; 222 | } 223 | } 224 | 225 | // check if any of the minimum columns happens to be unassigned. 226 | // if so, we have an augmenting path right away. 227 | for (k = low; k < up; k++) 228 | if (colsol[collist[k]] < 0) 229 | { 230 | endofpath = collist[k]; 231 | unassignedfound = true; 232 | break; 233 | } 234 | } 235 | 236 | if (!unassignedfound) 237 | { 238 | // update 'distances' between freerow and all unscanned columns, via next scanned column. 239 | j1 = collist[low]; 240 | low++; 241 | i = colsol[j1]; 242 | h = assigncost[i][j1] - v[j1] - min; 243 | 244 | for (k = up; k < dim; k++) 245 | { 246 | j = collist[k]; 247 | v2 = assigncost[i][j] - v[j] - h; 248 | if (v2 < d[j]) 249 | { 250 | pred[j] = i; 251 | if (v2 == min) // new column found at same minimum value 252 | { 253 | if (colsol[j] < 0) 254 | { 255 | // if unassigned, shortest augmenting path is complete. 256 | endofpath = j; 257 | unassignedfound = true; 258 | break; 259 | } 260 | // else add to list to be scanned right away. 261 | else 262 | { 263 | collist[k] = collist[up]; 264 | collist[up++] = j; 265 | } 266 | } 267 | d[j] = v2; 268 | } 269 | } 270 | } 271 | } 272 | while (!unassignedfound); 273 | 274 | // update column prices. 275 | for (k = 0; k <= last; k++) 276 | { 277 | j1 = collist[k]; 278 | v[j1] = v[j1] + d[j1] - min; 279 | } 280 | 281 | // reset row and column assignments along the alternating path. 282 | do 283 | { 284 | i = pred[endofpath]; 285 | colsol[endofpath] = i; 286 | j1 = endofpath; 287 | endofpath = rowsol[i]; 288 | rowsol[i] = j1; 289 | } 290 | while (i != freerow); 291 | } 292 | 293 | // calculate optimal cost. 294 | cost lapcost = 0; 295 | for (i = 0; i < dim; i++) 296 | { 297 | j = rowsol[i]; 298 | u[i] = assigncost[i][j] - v[j]; 299 | lapcost = lapcost + assigncost[i][j]; 300 | } 301 | 302 | // free reserved memory. 303 | delete[] pred; 304 | delete[] free; 305 | delete[] collist; 306 | delete[] matches; 307 | delete[] d; 308 | 309 | return lapcost; 310 | } 311 | -------------------------------------------------------------------------------- /src/tracker/src/track.cpp: -------------------------------------------------------------------------------- 1 | #include "track.h" 2 | 3 | using namespace mctracker::tracker; 4 | 5 | Track 6 | ::Track(const float& _x, const float& _y, const KalmanParam& _param, const cv::Mat& h, const int cameraNum) 7 | : Entity(), hist(h) 8 | { 9 | kf = std::shared_ptr(new KalmanFilter(_x, _y, _param.getDt())); 10 | ntimes_propagated = 0; 11 | freezed = 0; 12 | ntime_missed = 0; 13 | isgood = false; 14 | m_label = -1; 15 | time = (double)cv::getTickCount(); 16 | sizes.resize(cameraNum); 17 | } 18 | 19 | const cv::Mat 20 | Track::update() 21 | { 22 | if(points.size() > 0) 23 | { 24 | cv::Point2f result(0,0); 25 | for(const auto& p : points) 26 | { 27 | result += p; 28 | } 29 | 30 | const auto& correction = correct(result.x, result.y); 31 | 32 | points.clear(); 33 | 34 | return correction; 35 | } 36 | 37 | ntime_missed++; 38 | return cv::Mat(); 39 | } 40 | 41 | const cv::Mat 42 | Track::predict() 43 | { 44 | const auto& prediction = kf->predict(); 45 | m_history.push_back(cv::Point2f(prediction.at(0), prediction.at(1))); 46 | checkHistory(); 47 | return prediction; 48 | } 49 | 50 | const cv::Mat 51 | Track::correct(const float& _x, const float& _y) 52 | { 53 | ntimes_propagated++; 54 | time_in_sec = ((double) cv::getTickCount() - time) / cv::getTickFrequency(); 55 | return kf->correct(_x, _y); 56 | } 57 | 58 | const cv::Point2f 59 | Track::getPoint() 60 | { 61 | const auto& prediction = kf->getPrediction(); 62 | return cv::Point2f(prediction.at(0), prediction.at(1)); 63 | } 64 | 65 | const std::string 66 | Track::label2string() 67 | { 68 | std::stringstream ss; 69 | ss << m_label; 70 | return ss.str(); 71 | } 72 | -------------------------------------------------------------------------------- /src/tracker/src/tracker.cpp: -------------------------------------------------------------------------------- 1 | #include "tracker.h" 2 | 3 | using namespace mctracker::tracker; 4 | 5 | Tracker 6 | ::Tracker(const KalmanParam& _param, const std::vector& camerastack) 7 | : streams(camerastack) 8 | { 9 | param = _param; 10 | rng = cv::RNG(12345); 11 | trackIds = 1; 12 | numCams = streams.size(); 13 | if(numCams > 10 || numCams == 0) 14 | { 15 | throw std::invalid_argument("The number of cameras has to be: 0 < num of cam <= 10"); 16 | } 17 | 18 | auto numClose = 0; 19 | for(const auto& stream : streams) 20 | { 21 | if(stream.getProximity()) 22 | numClose++; 23 | } 24 | auto numNotClose = numCams - numClose; 25 | 26 | auto p1 = 1. / numCams; 27 | auto p2 = .2*p1*numNotClose; 28 | auto not_close_prob1 = p1 - (p2 / numNotClose); 29 | auto close_prob = p1 + (p2 / numClose); 30 | 31 | for(const auto& stream : streams) 32 | { 33 | if(stream.getProximity()) 34 | { 35 | cameraProbabilities.push_back(close_prob); 36 | } 37 | else 38 | { 39 | cameraProbabilities.push_back(not_close_prob1); 40 | } 41 | } 42 | } 43 | 44 | void 45 | Tracker::refine_detections(std::vector& _detections) 46 | { 47 | auto m = 0; 48 | for(auto& det : _detections) 49 | { 50 | const auto& fov = streams.at(m).getFOV(); 51 | for(int i = int(det.size()) - 1; i >= 0 ; --i) 52 | { 53 | if(det.at(i).y() < 0 || det.at(i).x() < 0 || det.at(i).y() > fov.rows || det.at(i).x() > fov.cols || 54 | int(fov.at(cv::Point2i(det.at(i).x(), det.at(i).y()))) != 255) 55 | { 56 | det.erase(det.begin() + i); 57 | } 58 | } 59 | ++m; 60 | } 61 | } 62 | 63 | 64 | Detections 65 | Tracker::first_assosiation(std::vector& observations) 66 | { 67 | Detections detections; 68 | std::vector< std::vector > checked(observations.size()); 69 | 70 | for(auto i = 0; i < int(observations.size()); ++i) 71 | { 72 | checked.at(i).resize(observations.at(i).size(), false); 73 | } 74 | 75 | std::vector< std::vector< std::pair > > associations; //camera, idx_detection 76 | //scroll the observations from the camera i 77 | for(auto i = 0; i < int(observations.size() - 1); ++i) 78 | { 79 | //assign the obeservations 80 | const auto& points_i = observations.at(i); 81 | auto m = 0; 82 | //compare each point of the camera i with all the points of the other cameras 83 | for(const auto& pi : points_i) 84 | { 85 | std::vector< std::pair > association; 86 | //if the point is not already checked 87 | if(!checked[i][m]) 88 | { 89 | //insert the point in the local association 90 | association.push_back(std::make_pair(i, m)); 91 | 92 | //take the obeservations for the other camera j (i+1) 93 | for(auto j = i + 1; j < int(observations.size()); j++) 94 | { 95 | const auto& points_j = observations.at(j); 96 | auto min_dist = std::numeric_limits::max(); 97 | auto idx = -1; 98 | auto k = 0; 99 | //scroll all the obeservations of the camera j 100 | for(const auto& pj : points_j) 101 | { 102 | //if the point is not checked 103 | if(!checked[j][k]) 104 | { 105 | //I compute the distance 106 | auto p = cv::Point2f(pi.x() - pj.x(), pi.y() - pj.y()); 107 | float dist = sqrt( p.x * p.x + p.y * p.y); 108 | 109 | if(dist < min_dist) 110 | { 111 | min_dist = dist; 112 | idx = k; 113 | } 114 | } 115 | ++k; 116 | } 117 | 118 | //if the distance is less than 30 pixel 119 | // the point included in the observesion j 120 | // is added to the point related to the 121 | // observation i 122 | if(min_dist < 30 && idx != -1) 123 | { 124 | //set the point as checked 125 | checked[j][idx] = true; 126 | association.push_back(std::make_pair(j, idx)); 127 | } 128 | } 129 | } 130 | if(association.size() > 0) 131 | associations.push_back(association); 132 | 133 | checked[i][m] = true; 134 | ++m; 135 | } 136 | } 137 | 138 | 139 | for(const auto& association : associations) 140 | { 141 | if(association.size() == 1) 142 | { 143 | const auto& idx = association.at(0); 144 | const auto& obs = observations[idx.first][idx.second]; 145 | Detection d(obs.x(), obs.y(), obs.w(), obs.h(), obs.hist()); 146 | detections.push_back(d); 147 | } 148 | else 149 | { 150 | Detection d; 151 | float prob; 152 | d.setX(0.); 153 | d.setY(0.); 154 | 155 | for(const auto& ass : association) 156 | { 157 | prob = cameraProbabilities.at(ass.first); 158 | const auto& obs = observations[ass.first][ass.second]; 159 | d.setX(d.x() + prob*obs.x()); 160 | d.setY(d.y() + prob*obs.y()); 161 | if(streams.at(ass.first).getProximity()) 162 | { 163 | d.setHist(obs.hist()); 164 | } 165 | d.setSize(obs.w(), obs.h()); 166 | } 167 | 168 | detections.push_back(d); 169 | } 170 | } 171 | 172 | return detections; 173 | } 174 | 175 | void 176 | Tracker::track(std::vector< Detections >& _detections, const int& w, const int& h) 177 | { 178 | //prediction 179 | evolveTracks(); 180 | 181 | auto d = _detections; 182 | 183 | //check if the detections are in the FOV of the cameras 184 | refine_detections(_detections); 185 | 186 | //check if the freezed tracks can be restored 187 | if(old_tracks.size() > 0) 188 | check_old_tracks(_detections); 189 | 190 | 191 | //assign the new observations to the tracklets 192 | std::vector assignments(_detections.size()); 193 | 194 | auto i = 0; 195 | for(const auto& det : _detections) 196 | { 197 | assignments.at(i) = associate_tracks(det).clone(); 198 | 199 | if(assignments.at(i).total() != 0) 200 | Hyphothesis::instance()->new_hyphothesis(assignments.at(i), single_tracks, det, w, h, 201 | param.getNewhypdummycost(), prev_unassigned, param, (streams.size())); 202 | assignments.at(i).setTo(0, (assignments.at(i) == 255)); 203 | 204 | i++; 205 | } 206 | 207 | 208 | if(single_tracks.size() == 0) 209 | { 210 | //first association between camera points 211 | const auto& detections = first_assosiation(_detections); 212 | 213 | //start new tracks 214 | for(const auto& t : detections) 215 | { 216 | single_tracks.push_back(Track_ptr(new Track(t.x(), t.y(), param, t.hist(), int(streams.size())))); 217 | } 218 | } 219 | else 220 | { 221 | //update the tracks given the assigments 222 | update_tracks(assignments, _detections); 223 | //delete or freeze the tracks which have no detections 224 | delete_tracks(); 225 | 226 | } 227 | } 228 | 229 | void 230 | Tracker::delete_tracks() 231 | { 232 | for(int i = single_tracks.size() - 1; i >= 0; --i) 233 | { 234 | const auto& p = single_tracks.at(i)->getPointPrediction(); 235 | const auto& ntime_missed = single_tracks.at(i)->ntime_missed; 236 | 237 | if(p.x < 0 || p.x >= int(width) || p.y < 0 || p.y >= int(height) || ntime_missed >= param.getMaxmissed()) 238 | { 239 | bool isInside = false; 240 | for(const auto& stream : streams) 241 | { 242 | const auto& fov = stream.getFOV(); 243 | if(int(fov.at(p)) != 0) 244 | { 245 | isInside = true; 246 | break; 247 | } 248 | } 249 | 250 | if(isInside) 251 | { 252 | old_tracks.push_back(single_tracks.at(i)); 253 | } 254 | 255 | single_tracks.erase(single_tracks.begin() + i); 256 | } 257 | } 258 | } 259 | 260 | 261 | cv::Mat 262 | Tracker::associate_tracks(const Detections& _detections) 263 | { 264 | if(_detections.size() == 0) return cv::Mat(); 265 | cv::Mat assigmentsBin(cv::Size(_detections.size(), single_tracks.size()), CV_8UC1, cv::Scalar(0)); 266 | assignments_t assignments; 267 | distMatrix_t cost(_detections.size() * single_tracks.size()); 268 | cv::Mat costs(cv::Size(_detections.size(), single_tracks.size()), CV_32FC1); 269 | cv::Mat mu; 270 | cv::Mat sigma; 271 | cv::Point2f t; 272 | 273 | 274 | //COMPUTE COSTS 275 | const uint& tSize = single_tracks.size(); 276 | const uint& dSize = _detections.size(); 277 | 278 | for(uint i = 0; i < tSize; ++i) 279 | { 280 | mu = single_tracks.at(i)->getPrediction(); 281 | sigma = single_tracks.at(i)->S(); 282 | t = cv::Point2f(mu.at(0), mu.at(1)); 283 | 284 | for(uint j = 0; j < dSize; ++j) 285 | { 286 | cv::Mat detection(cv::Size(1, 2), CV_32FC1); 287 | detection.at(0) = _detections.at(j).x(); 288 | detection.at(1) = _detections.at(j).y(); 289 | //compute mahalanobis distance costs 290 | costs.at(i, j) = cv::Mahalanobis(detection, cv::Mat(t), sigma.inv()); 291 | cost.at(i + j * single_tracks.size() ) = costs.at(i, j); 292 | } 293 | } 294 | 295 | //compute the munkres algorithm 296 | AssignmentProblemSolver APS; 297 | APS.Solve(cost, single_tracks.size(), _detections.size(), assignments, AssignmentProblemSolver::optimal); 298 | 299 | for(auto i = 0; i < int(assignments.size()); ++i) 300 | { 301 | //check if there is an assigment and if the cost associated to the assignment is less then 302 | //a threshold 303 | if(costs.at(i, assignments[i]) < association_thresh && assignments[i] != -1) 304 | assigmentsBin.at(i, assignments[i]) = 1; 305 | } 306 | 307 | 308 | 309 | return assigmentsBin; 310 | } 311 | 312 | 313 | 314 | void 315 | Tracker::update_tracks(const std::vector& assigments, const std::vector& _detections) 316 | { 317 | std::map > > idx_to_update; 318 | std::map > idx_hists; 319 | auto m = 0; 320 | 321 | for(const auto& assignment : assigments) 322 | { 323 | const uint& aRows = assignment.rows; 324 | const uint& aCols = assignment.cols; 325 | const auto& detection = _detections.at(m); 326 | if(detection.size() > 0) 327 | { 328 | for(uint i = 0; i < aRows; ++i) 329 | { 330 | for(uint j = 0; j < aCols; ++j) 331 | { 332 | if(assignment.at(i, j) == uchar(1)) 333 | { 334 | if(idx_to_update.find(i) != idx_to_update.end()) 335 | { 336 | idx_to_update[i].push_back(std::make_pair (m, cv::Rect(detection.at(j).x(), detection.at(j).y(), 337 | detection.at(j).w(), detection.at(j).h()))); 338 | idx_hists[i].push_back(detection.at(j).hist()); 339 | } 340 | else 341 | { 342 | idx_to_update.insert(std::make_pair(i, std::vector >())); 343 | idx_to_update[i].push_back(std::make_pair(m, cv::Rect(detection.at(j).x(), detection.at(j).y(), 344 | detection.at(j).w(), detection.at(j).h()))); 345 | idx_hists.insert(std::make_pair(i, std::vector())); 346 | idx_hists[i].push_back(detection.at(j).hist()); 347 | } 348 | } 349 | } 350 | } 351 | } 352 | ++m; 353 | } 354 | 355 | for(const auto& idx : idx_to_update) 356 | { 357 | const auto& points = idx.second; 358 | int id_hist = -1; 359 | if(points.size() == 1) 360 | { 361 | single_tracks.at(idx.first)->push_point(points.at(0).second.tl()); 362 | single_tracks.at(idx.first)->sizes.at(points.at(0).first) = cv::Size(points.at(0).second.width, points.at(0).second.height); 363 | single_tracks.at(idx.first)->set_hist(idx_hists[idx.first].at(0)); 364 | } 365 | else 366 | { 367 | cv::Point2f p(0, 0); 368 | for(const auto& point : points) 369 | { 370 | p.x += (cameraProbabilities.at(point.first) * point.second.x); 371 | p.y += (cameraProbabilities.at(point.first) * point.second.y); 372 | single_tracks.at(idx.first)->sizes.at(point.first) = cv::Size(point.second.width, point.second.height); 373 | } 374 | single_tracks.at(idx.first)->push_point(p); 375 | } 376 | 377 | single_tracks.at(idx.first)->update(); 378 | single_tracks.at(idx.first)->ntime_missed = 0; 379 | 380 | if(id_hist != -1) 381 | single_tracks.at(idx.first)->set_hist(idx_hists[idx.first].at(id_hist)); 382 | 383 | 384 | if(single_tracks.at(idx.first)->nTimePropagation() >= param.getMinpropagate() && !single_tracks.at(idx.first)->isgood) 385 | { 386 | single_tracks.at(idx.first)->setLabel(trackIds++); 387 | 388 | single_tracks.at(idx.first)->setColor(cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), 389 | rng.uniform(0, 255))); 390 | single_tracks.at(idx.first)->isgood = true; 391 | } 392 | } 393 | 394 | for(const auto& assignment : assigments) 395 | { 396 | cv::Mat ass_sum; 397 | cv::reduce(assignment, ass_sum, 1, CV_REDUCE_SUM, CV_32S); 398 | 399 | for(uint i = 0; i < ass_sum.total(); ++i) 400 | { 401 | if(ass_sum.at(i) == 0) 402 | { 403 | single_tracks.at(i)->ntime_missed++; 404 | } 405 | } 406 | } 407 | 408 | } 409 | 410 | void 411 | Tracker::check_old_tracks(std::vector& _detections) 412 | { 413 | std::vector assignents; 414 | for(auto &det : _detections) 415 | { 416 | const int& tSize = int(old_tracks.size()); 417 | const int& dSize = int(det.size()); 418 | 419 | if(dSize > 0) 420 | { 421 | 422 | //COMPUTE COSTS 423 | assignments_t assignments; 424 | distMatrix_t cost; 425 | 426 | cv::Mat costs(cv::Size(det.size(), old_tracks.size()), CV_32FC1); 427 | cv::Mat hist_costs(cv::Size(det.size(), old_tracks.size()), CV_32FC1); 428 | cv::Mat assigmentsBin(cv::Size(det.size(), old_tracks.size()), CV_8UC1, cv::Scalar(0)); 429 | 430 | cv::Mat mu; 431 | cv::Mat sigma; 432 | cv::Point2f t; 433 | 434 | for(auto i = 0; i < tSize; ++i) 435 | { 436 | mu = old_tracks.at(i)->getPrediction(); 437 | sigma = old_tracks.at(i)->S(); 438 | t = cv::Point2f(mu.at(0), mu.at(1)); 439 | 440 | for(auto j = 0; j < dSize; ++j) 441 | { 442 | cv::Mat detection(cv::Size(1, 2), CV_32FC1); 443 | detection.at(0) = det.at(j).x(); 444 | detection.at(1) = det.at(j).y(); 445 | // compute the mahalanobis distance 446 | costs.at(i, j) = cv::Mahalanobis(detection, cv::Mat(t), sigma.inv()); 447 | // compute the correlation distance 448 | hist_costs.at(i, j) = 1 - cv::compareHist(det.at(j).hist(), old_tracks.at(i)->histogram(), cv::HISTCMP_CORREL); 449 | } 450 | } 451 | //normalize the costs between 0 and 1 452 | cv::normalize(costs, costs, 0., 1., cv::NORM_MINMAX, -1, cv::Mat()); 453 | //linear combination between Mahalanobis distance costs and histogram costs 454 | costs = .6 * costs + .4 * hist_costs; 455 | 456 | //copy the matrix of costs into a vector 457 | if (costs.isContinuous()) 458 | { 459 | cost.assign((float*)costs.data, (float*)costs.data + costs.total()); 460 | } 461 | else 462 | { 463 | for (int i = 0; i < costs.rows; ++i) 464 | { 465 | cost.insert(cost.end(), costs.ptr(i), costs.ptr(i)+costs.cols); 466 | } 467 | } 468 | 469 | //computing munkres algorithm 470 | AssignmentProblemSolver APS; 471 | APS.Solve(cost, old_tracks.size(), det.size(), assignments, AssignmentProblemSolver::optimal); 472 | 473 | 474 | for(auto i = 0; i < int(assignments.size()); ++i) 475 | { 476 | //check if there is an assigment and if the cost associated to the assignment is less then 477 | //a threshold 478 | if(costs.at(i, assignments[i]) < freezed_thresh && assignments[i] != -1) 479 | assigmentsBin.at(i, assignments[i]) = 1; 480 | } 481 | assignents.push_back(assigmentsBin); 482 | } 483 | } 484 | 485 | std::vector notAssigned; 486 | std::set notUp; 487 | std::set > to_restore; 488 | //check the ids of the tracks to restore 489 | for(const auto& assignment : assignents) 490 | { 491 | for(auto i = 0; i < assignment.rows; ++i) 492 | { 493 | for(auto j = 0; j < assignment.cols; ++j) 494 | { 495 | if(assignment.at(i, j) == uchar(1)) 496 | { 497 | to_restore.insert(i); 498 | } 499 | } 500 | } 501 | } 502 | 503 | //restore the freezed tracks 504 | for(const auto& id : to_restore) 505 | { 506 | single_tracks.push_back(old_tracks.at(id)); 507 | old_tracks.erase(old_tracks.begin() + id); 508 | } 509 | 510 | 511 | for(int m = old_tracks.size() - 1; m >= 0; --m) 512 | { 513 | //delete freezed tracks 514 | if(old_tracks.at(m)->freezed >= param.getMaxmissed()) 515 | { 516 | old_tracks.erase(old_tracks.begin() + m); 517 | } 518 | //or increment the number of "freezing" 519 | else 520 | { 521 | old_tracks.at(m)->freezed++; 522 | } 523 | } 524 | } 525 | 526 | const Entities 527 | Tracker::getTracks() 528 | { 529 | tracks.clear(); 530 | tracks.insert(tracks.end(), single_tracks.begin(), single_tracks.end()); 531 | return tracks; 532 | } 533 | -------------------------------------------------------------------------------- /src/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(utils) 2 | include_directories(${PROJECT_BINARY_DIR}/../src/homography/include) 3 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../src/homography/include) 4 | 5 | include_directories(${PROJECT_BINARY_DIR}/../src/utils/include) 6 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../src/utils/include) 7 | 8 | file(GLOB_RECURSE UTILS_SRC "src/utils/src/*.cpp" "src/homography/src/*.cpp") 9 | 10 | add_library(utils SHARED ${UTILS_SRC}) 11 | target_link_libraries(utils ${OpenCV_LIBS}) 12 | 13 | endfunction() 14 | -------------------------------------------------------------------------------- /src/utils/include/camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _CAMERA_H_ 6 | #define _CAMERA_H_ 7 | 8 | #include 9 | #include 10 | #include "homography.h" 11 | #include "yamlmanager.h" 12 | 13 | using namespace mctracker::config; 14 | 15 | namespace mctracker 16 | { 17 | namespace utils 18 | { 19 | class Camera 20 | { 21 | public: 22 | /** 23 | * @brief Constructor class Camera 24 | * @param stream a string containing the source path of the streaming 25 | * @param view a string containing the path to the binary image representing the field of view of the cameras 26 | * @param homography_file a string containing the path to the homography file 27 | * @param prox a bool which if is true means that the camera is close to scene to monitor 28 | */ 29 | Camera(const std::string& stream, const std::string& view, const std::string& homography_file, bool prox); 30 | /** 31 | * @brief get a frame from the stream 32 | * @param frame cv::Mat where the frame is stored 33 | * @return true if the frame is read successfully 34 | */ 35 | bool getFrame(cv::Mat& frame); 36 | 37 | /** 38 | * @brief convert a point to world coordinates 39 | * @param p cv::Point2f to convert 40 | * @return the converted point 41 | */ 42 | cv::Point2f camera2world(const cv::Point2f& p) const; 43 | 44 | /** 45 | * @brief convert a point to image coordinates 46 | * @param p cv::Point2f to convert 47 | * @return the converted point 48 | */ 49 | cv::Point2f world2camera(const cv::Point2f& p) const; 50 | public: 51 | /** 52 | * @brief get if the camera is close to the scene 53 | * @return a bool which if is true means that the camera is close to scene to monitor 54 | */ 55 | inline const bool 56 | getProximity() const 57 | { 58 | return proximity; 59 | } 60 | 61 | /** 62 | * @brief get the binary image representing the fov of the camera 63 | * @return a cv::Mat representing the fov of the camera 64 | */ 65 | inline const cv::Mat 66 | getFOV() const 67 | { 68 | return image_view; 69 | } 70 | private: 71 | cv::VideoCapture cap; 72 | cv::Mat image_view; 73 | cv::Mat H, Hinv; 74 | bool proximity; 75 | }; 76 | } 77 | } 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/utils/include/camerastack.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _CAMERA_STACK_H_ 6 | #define _CAMERA_STACK_H_ 7 | 8 | #include 9 | #include 10 | #include "camera.h" 11 | #include "camera_param.h" 12 | #include "yamlmanager.h" 13 | 14 | using namespace mctracker::config; 15 | 16 | namespace mctracker 17 | { 18 | namespace utils 19 | { 20 | class CameraStack 21 | { 22 | public: 23 | /** 24 | * @brief Constructor class Camera 25 | */ 26 | CameraStack() { ; } 27 | /** 28 | * @brief Constructor class Camera 29 | * @param params camera parameters 30 | */ 31 | CameraStack(const std::vector& params); 32 | /** 33 | * @brief Get the frames from all the camera of the system 34 | * @param frames a vector of cv::Mat where the frames are stored 35 | * @return a bool which if true means that the frames have been correctly readed from all the cameras, false otherwise 36 | */ 37 | bool getFrame(std::vector& frames); 38 | public: 39 | /** 40 | * @brief get the vector containing the cameras 41 | * @retun the camera stack 42 | */ 43 | inline const std::vector 44 | getCameraStack() 45 | { 46 | return streams; 47 | } 48 | private: 49 | /** 50 | * @brief open all the streams of the system 51 | * @param params a vector containing all the parameters of each stream of the system 52 | */ 53 | void open_streams(const std::vector& params); 54 | private: 55 | std::vector streams; 56 | }; 57 | } 58 | } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /src/utils/include/drawing.h: -------------------------------------------------------------------------------- 1 | #ifndef _DRAWING_H_ 2 | #define _DRAWING_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "utils.h" 9 | 10 | namespace mctracker 11 | { 12 | namespace utils 13 | { 14 | namespace tools 15 | { 16 | class Drawing 17 | { 18 | public: 19 | Drawing() { ; } 20 | static void history(const Points& _history, const cv::Scalar& color, cv::Mat& img); 21 | static void arrow(const cv::Point& start_point, const cv::Point& end_point, const cv::Scalar& color, cv::Mat& img); 22 | static void rectangle(const cv::Rect& _rect, const cv::Scalar& color, cv::Mat& img); 23 | }; 24 | } 25 | } 26 | } 27 | 28 | #endif 29 | 30 | -------------------------------------------------------------------------------- /src/utils/include/utility.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _UTILITY_H_ 6 | #define _UTILITY_H_ 7 | 8 | #include 9 | #include "object_detector.h" 10 | #include "detection.h" 11 | #include "camera.h" 12 | 13 | using namespace mctracker::tracker; 14 | using namespace mctracker::objectdetection; 15 | 16 | namespace mctracker 17 | { 18 | namespace utils 19 | { 20 | class Utility 21 | { 22 | public: 23 | /** 24 | * @brief Constructor class Utility 25 | */ 26 | Utility() { ; } 27 | /** 28 | * @brief convert a set of detections of type bbox_t coming from the detector into type Detection 29 | * @param detections a vector of vector of type bbox_t coming from the detector 30 | * @param frames all the frames grabbed from all the streams in the system 31 | * @param streams camera stack 32 | * @return a vector of vector of type Detection compatible with the tracker format 33 | */ 34 | static std::vector > dets2Obs(const std::vector< std::vector >& detections, 35 | const std::vector& frames, const std::vector& masks, const std::vector& streams); 36 | 37 | /** 38 | * @brief make a mosaic between a set of images 39 | * @param images a set of images 40 | * @return an image containing a mosaic of all the input images 41 | */ 42 | static cv::Mat makeMosaic(const std::vector& images); 43 | 44 | /** 45 | * @brief compute a hsv histogram 46 | * @param img input image 47 | * @param mask mask (if any) used for computing the histogram 48 | * @param d bounding box representing the detection for which the histogram is computed 49 | */ 50 | static cv::Mat computeHist(const cv::Mat& img, const cv::Mat& mask, const bbox_t& d); 51 | }; 52 | } 53 | } 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /src/utils/include/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Written by Andrea Pennisi 3 | */ 4 | 5 | #ifndef _UTILS_H_ 6 | #define _UTILS_H_ 7 | 8 | #include 9 | #include 10 | 11 | #include "detection.h" 12 | 13 | using namespace mctracker; 14 | using namespace mctracker::tracker; 15 | 16 | typedef std::vector Detections; 17 | typedef std::vector UIntVec; 18 | typedef std::vector UIntMat; 19 | typedef std::vector BoolVec; 20 | typedef std::vector Points; 21 | typedef std::vector MatVec; 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /src/utils/src/camera.cpp: -------------------------------------------------------------------------------- 1 | #include "camera.h" 2 | 3 | using namespace mctracker; 4 | using namespace mctracker::utils; 5 | 6 | Camera 7 | ::Camera(const std::string& stream, const std::string& view, const std::string& homography_file, bool prox) 8 | : proximity(prox) 9 | { 10 | cap.open(stream); 11 | if(!cap.isOpened()) 12 | { 13 | throw std::invalid_argument("Invalid Stream File: " + stream); 14 | } 15 | 16 | try 17 | { 18 | image_view = cv::imread(view, cv::IMREAD_GRAYSCALE); 19 | } 20 | catch(cv::Exception& ex) 21 | { 22 | std::cout << "Invalid image name: " << view << std::endl; 23 | std::cout << ex.what() << std::endl; 24 | exit(-1); 25 | } 26 | 27 | YamlManager homographyReader; 28 | homographyReader.read(homography_file); 29 | 30 | if(!homographyReader.getElem("Matrix", H, CV_64FC1)) 31 | { 32 | throw std::invalid_argument("Homography is not specified!"); 33 | } 34 | 35 | Hinv = H.inv(); 36 | } 37 | 38 | bool 39 | Camera::getFrame(cv::Mat& frame) 40 | { 41 | cap >> frame; 42 | if(frame.empty()) 43 | { 44 | return false; 45 | } 46 | return true; 47 | } 48 | 49 | cv::Point2f 50 | Camera::camera2world(const cv::Point2f& p) const 51 | { 52 | double x, y; 53 | mctracker::geometry::Homography::calcProjection(p, H, x, y); 54 | return cv::Point2f(x, y); 55 | } 56 | 57 | cv::Point2f 58 | Camera::world2camera(const cv::Point2f& p) const 59 | { 60 | double x, y; 61 | mctracker::geometry::Homography::calcProjection(p, Hinv, x, y); 62 | return cv::Point2f(x, y); 63 | } 64 | -------------------------------------------------------------------------------- /src/utils/src/camerastack.cpp: -------------------------------------------------------------------------------- 1 | #include "camerastack.h" 2 | 3 | using namespace mctracker; 4 | using namespace mctracker::utils; 5 | 6 | CameraStack 7 | ::CameraStack(const std::vector& params) 8 | { 9 | open_streams(params); 10 | } 11 | 12 | void 13 | CameraStack::open_streams(const std::vector& params) 14 | { 15 | for(const auto& param : params) 16 | { 17 | Camera cam(param.getStream(), param.getFOV(), param.getHomography(), param.getProximity()); 18 | streams.push_back(cam); 19 | } 20 | } 21 | 22 | bool 23 | CameraStack::getFrame(std::vector& frames) 24 | { 25 | if(frames.size() !=0) 26 | frames.clear(); 27 | 28 | cv::Mat frame; 29 | 30 | for(auto& stream : streams) 31 | { 32 | if(!stream.getFrame(frame)) 33 | return false; 34 | frames.push_back(frame.clone()); 35 | } 36 | return true; 37 | } 38 | -------------------------------------------------------------------------------- /src/utils/src/drawing.cpp: -------------------------------------------------------------------------------- 1 | #include "drawing.h" 2 | 3 | using namespace mctracker; 4 | using namespace mctracker::utils::tools; 5 | 6 | 7 | void 8 | Drawing::arrow(const cv::Point& start_point, const cv::Point& end_point, 9 | const cv::Scalar& color, cv::Mat& img) 10 | { 11 | cv::line(img, start_point, end_point, color, 2); 12 | } 13 | 14 | void 15 | Drawing::history(const Points& _history, const cv::Scalar& color, cv::Mat& img) 16 | { 17 | for(const auto& point : _history) 18 | { 19 | cv::circle(img, point, 1, color, -1); 20 | } 21 | } 22 | 23 | void 24 | Drawing::rectangle(const cv::Rect& _rect, const cv::Scalar& color, cv::Mat& img) 25 | { 26 | cv::rectangle(img, _rect, color, 2); 27 | } 28 | 29 | 30 | -------------------------------------------------------------------------------- /src/utils/src/utility.cpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | using namespace mctracker::utils; 4 | 5 | std::vector > 6 | Utility::dets2Obs(const std::vector< std::vector >& detections, 7 | const std::vector& frames, const std::vector& masks, const std::vector& streams) 8 | { 9 | std::vector > obs; 10 | auto i = 0; 11 | for(const auto& detection : detections) 12 | { 13 | const auto& stream = streams.at(i); 14 | std::vector camera_det; 15 | for(const auto& det : detection) 16 | { 17 | cv::Point2f point(det.x + (det.w >> 1), det.y + det.h); 18 | const auto& worldPoint = stream.camera2world(point); 19 | Detection d(worldPoint.x, worldPoint.y, det.w, det.h, computeHist(frames.at(i), masks.at(i), det)); 20 | camera_det.push_back(d); 21 | } 22 | obs.push_back(camera_det); 23 | ++i; 24 | } 25 | return obs; 26 | } 27 | 28 | cv::Mat 29 | Utility::makeMosaic(const std::vector &images) 30 | { 31 | auto numImgs = images.size(); 32 | if(numImgs > 0) 33 | { 34 | const auto& sz = images.at(0).size(); 35 | cv::Size newSize(sz.width*numImgs, sz.height); 36 | cv::Mat newImg(newSize, CV_8UC3); 37 | auto wIdx = 0; 38 | for(const auto& im : images) 39 | { 40 | im.copyTo(newImg(cv::Rect(wIdx, 0, im.cols, im.rows))); 41 | wIdx += im.cols; 42 | } 43 | return newImg; 44 | } 45 | return cv::Mat(); 46 | } 47 | 48 | 49 | cv::Mat 50 | Utility::computeHist(const cv::Mat& img, const cv::Mat& mask, const bbox_t& d) 51 | { 52 | if(img.empty()) 53 | { 54 | throw std::invalid_argument("Error: the image is empty"); 55 | } 56 | 57 | auto rect = [](const bbox_t& r, const cv::Size& sz) 58 | { 59 | cv::Rect rectangle(r.x, r.y, r.w, r.h); 60 | if(rectangle.x < 0) rectangle.x = 0; 61 | if(rectangle.y < 0) rectangle.y = 0; 62 | if(rectangle.width + rectangle.x > sz.width ) rectangle.width -= (((rectangle.width + rectangle.x) - sz.width ) + 1); 63 | if(rectangle.height + rectangle.y > sz.height ) rectangle.height -= (((rectangle.height + rectangle.y) - sz.height ) + 1); 64 | return rectangle; 65 | }; 66 | 67 | const auto& r = rect(d, img.size()); 68 | const auto& patch = img(r); 69 | cv::Mat maskPatch; 70 | if(!mask.empty()) 71 | { 72 | maskPatch = mask(r); 73 | } 74 | 75 | cv::Mat hsvPatch; 76 | cv::cvtColor(patch, hsvPatch, CV_BGR2HSV); 77 | static const cv::Mat noMask; 78 | static const int hueBins = 50; 79 | static const int satBins = 60; 80 | static const int bins[] = {hueBins, satBins}; 81 | static const float hueRanges[] = {0, 256}; 82 | static const float satRanges[] = {0, 180}; 83 | static const float *ranges[] = {hueRanges, satRanges}; 84 | static const int imageCount = 1; 85 | static const int channels[] = {0, 1}; 86 | static const int dimensionCount = 2; 87 | static const bool uniform = true; 88 | static const bool accumulate = false; 89 | cv::Mat hist; 90 | cv::calcHist(&hsvPatch, imageCount, channels, maskPatch, hist, 91 | dimensionCount, bins, ranges, uniform, accumulate); 92 | static const double alpha = 0.0; 93 | static const double beta = 1.0; 94 | static const int normKind = cv::NORM_MINMAX; 95 | static const int dtype = -1; 96 | cv::normalize(hist, hist, alpha, beta, normKind, dtype, noMask); 97 | return hist; 98 | } 99 | -------------------------------------------------------------------------------- /videos/View_001.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apennisi/mctracker/29a89046f796f84eac922b2dc9cf4cd6c7aebb6b/videos/View_001.mp4 -------------------------------------------------------------------------------- /videos/View_006.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/apennisi/mctracker/29a89046f796f84eac922b2dc9cf4cd6c7aebb6b/videos/View_006.mp4 --------------------------------------------------------------------------------