├── .gitignore ├── .travis.yml ├── .vscode ├── .cmaketools.json ├── c_cpp_properties.json ├── launch.json └── settings.json ├── CMakeLists.txt ├── README.md ├── demo ├── CMakeLists.txt └── src │ └── demo.cpp └── gmphd ├── eigen_tools.h ├── gaussian_mixture.h └── gmphd_filter.h /.gitignore: -------------------------------------------------------------------------------- 1 | *.user 2 | build/* 3 | .directory 4 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | sudo: false 3 | compiler: 4 | - g++ 5 | - clang 6 | 7 | script: 8 | - mkdir build 9 | - cd build 10 | - cmake .. 11 | - make all 12 | 13 | addons: 14 | apt: 15 | sources: 16 | - ubuntu-toolchain-r-test 17 | packages: 18 | - g++ 19 | - libeigen3-dev 20 | 21 | notifications: 22 | email: none 23 | -------------------------------------------------------------------------------- /.vscode/.cmaketools.json: -------------------------------------------------------------------------------- 1 | { 2 | "variant": null, 3 | "activeEnvironments": [], 4 | "codeModel": null 5 | } -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/**", 7 | "/usr/include/eigen3", 8 | "/usr/include/opencv4" 9 | ], 10 | "defines": [], 11 | "compilerPath": "/usr/bin/clang", 12 | "cStandard": "c17", 13 | "cppStandard": "c++14", 14 | "intelliSenseMode": "linux-clang-x64" 15 | } 16 | ], 17 | "version": 4 18 | } -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "(gdb) Launch", 9 | "type": "cppdbg", 10 | "request": "launch", 11 | "program": "${workspaceFolder}/build/demo/demo", 12 | "args": [], 13 | "stopAtEntry": false, 14 | "cwd": "${workspaceFolder}", 15 | "environment": [], 16 | "externalConsole": false, 17 | "MIMode": "gdb", 18 | "setupCommands": [ 19 | { 20 | "description": "Enable pretty-printing for gdb", 21 | "text": "-enable-pretty-printing", 22 | "ignoreFailures": true 23 | } 24 | ] 25 | } 26 | ] 27 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "deque": "cpp", 4 | "string": "cpp", 5 | "vector": "cpp" 6 | } 7 | } -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | project(gmphd) 4 | 5 | SET(DEMO FALSE CACHE BOOL "Build the demo app") 6 | 7 | add_library (gmphd INTERFACE) 8 | target_include_directories(gmphd INTERFACE "gmphd/.") 9 | 10 | if(DEMO) 11 | add_subdirectory (demo) 12 | endif() 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | gmphd [![Build Status](https://travis-ci.org/blefaudeux/gmphd.svg?branch=master)](https://travis-ci.org/blefaudeux/gmphd) 2 | ===== 3 | 4 | What is it ? 5 | ---------- 6 | A Gaussian-Mixtures Probability Hypothesis Density (GM-PHD) filter for multitarget tracking in a bayesian framework. It allows you to track targets over time, given noisy measurements and missed observations. It's implemented as an header-only templated library. 7 | 8 | What does it depend on ? 9 | ------------------------- 10 | Eigen, and OpenCV for the quick and dirty demo 11 | 12 | How to build ? 13 | -------------- 14 | After cloning, it's as simple as (unix, for windows you could probably do something with the CMake GUI): 15 | 16 | 1. `mkdir build` 17 | 18 | 2. `cd build` 19 | 20 | 3. `cmake ..` (or `cmake .. -DDEMO=1` if you want to build the demo executable) 21 | 22 | 4. `make` 23 | 24 | General observations 25 | -------------------- 26 | Code quality is not top notch, leaves a lot to be desired. Feel free to contribute, just check that 27 | Travis CI is still happy from time to time.. 28 | 29 | License 30 | ------- 31 | 32 | The MIT License (MIT) 33 | Copyright (c) 2013 Benjamin Lefaudeux 34 | 35 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 36 | 37 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 38 | 39 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 40 | -------------------------------------------------------------------------------- /demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Define the project's name 2 | project(Demo) 3 | 4 | cmake_minimum_required(VERSION 2.6) 5 | 6 | include_directories(${PROJECT_SOURCE_DIR}/src) 7 | set(CMAKE_INCLUDE_CURRENT_DIR ON) 8 | 9 | # Try to find the needed packages 10 | find_package( OpenCV REQUIRED ) 11 | find_package( PkgConfig ) 12 | pkg_check_modules( EIGEN3 REQUIRED eigen3 ) 13 | include_directories( ${EIGEN3_INCLUDE_DIRS} ) 14 | 15 | # Set debug/release flags 16 | if( CMAKE_BUILD_TYPE MATCHES "[Dd][Ee][Bb][Uu][Gg]" ) 17 | message( "INFO: DEBUG BUILD" ) 18 | # for debug type builds, turn on verbose makefiles 19 | SET(CMAKE_VERBOSE_MAKEFILE ON) 20 | 21 | # Tell other CMake files that we're doing a debug build 22 | SET( DEBUG_BUILD 1 ) 23 | 24 | # Tell C/C++ that we're doing a debug build 25 | ADD_DEFINITIONS( -DDEBUG ) 26 | endif() 27 | 28 | # we use C++14 features 29 | if(CMAKE_COMPILER_IS_GNUCXX) 30 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 31 | endif() 32 | 33 | if( CMAKE_BUILD_TYPE MATCHES "[Rr][Ee][Ll][Ee][Aa][Ss][Ee]" ) 34 | message( "INFO: RELEASE BUILD" ) 35 | endif() 36 | 37 | # Add all the files we're interested in 38 | file(GLOB SRC ${PROJECT_SOURCE_DIR}/src/*.cpp) 39 | file(GLOB HEADERS ${PROJECT_SOURCE_DIR}/headers/*.h) 40 | 41 | add_executable(demo ${SRC} ${HEADERS}) 42 | target_link_libraries (demo gmphd ${OpenCV_LIBS}) 43 | 44 | # Sanitizers 45 | set (CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-omit-frame-pointer -fsanitize=address") 46 | set (CMAKE_LINKER_FLAGS_DEBUG "${CMAKE_LINKER_FLAGS_DEBUG} -fno-omit-frame-pointer -fsanitize=address") -------------------------------------------------------------------------------- /demo/src/demo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Benjamin Lefaudeux (blefaudeux@github) 3 | * Very basic example of a gmphd usecase, tracking moving targets on a 2D plane 4 | * whose measurement is polluted by random noise (both in terms of position and 5 | * detection probability). Rendering is pretty much as basic as it gets, 6 | * but you get the point 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace gmphd; 16 | 17 | bool isTargetVisible(float probaDetection) 18 | { 19 | int const maxRand = probaDetection * RAND_MAX; 20 | return rand() < maxRand; 21 | } 22 | 23 | // Init the Gaussian Mixture Probability Hypothesis Density filter 24 | void initTargetTracking(GMPHD<2> &tracker) 25 | { 26 | // Birth model (spawn) 27 | GaussianModel<4> Birth; 28 | Birth.m_weight = 0.2f; 29 | Birth.m_mean(0, 0) = 400.f; 30 | Birth.m_mean(1, 0) = 400.f; 31 | Birth.m_mean(2, 0) = 0.f; 32 | Birth.m_mean(3, 0) = 0.f; 33 | Birth.m_cov = 400.f * MatrixXf::Identity(4, 4); 34 | 35 | GaussianMixture<4> birthModel({Birth}); 36 | tracker.setBirthModel(birthModel); 37 | 38 | // Dynamics (motion model) 39 | tracker.setDynamicsModel(1.f, 10.f); 40 | 41 | // Detection model 42 | float const probDetection = 0.5f; 43 | float const measNoisePose = 2.f; 44 | float const measNoiseSpeed = 20.f; 45 | float const measBackground = 0.5f; 46 | tracker.setObservationModel(probDetection, measNoisePose, measNoiseSpeed, 47 | measBackground); 48 | 49 | // Pruning parameters 50 | tracker.setPruningParameters(0.1f, 3.f, 10); 51 | 52 | // Spawn (target apparition) 53 | SpawningModel<4> spawnModel; 54 | std::vector> spawns = {spawnModel}; 55 | tracker.setSpawnModel(spawns); 56 | 57 | // Survival over time 58 | tracker.setSurvivalProbability(0.95f); 59 | } 60 | 61 | bool display(std::vector> const &measures, std::vector> const &filtered, cv::Mat &pict) 62 | { 63 | // Display measurement hits 64 | for (const auto &meas : measures) 65 | { 66 | cv::circle(pict, cv::Point(meas.position[0], meas.position[1]), 2, cv::Scalar(0, 0, 255), 2); 67 | } 68 | 69 | // Display filter output 70 | float const scale = 5.f; 71 | for (const auto &filter : filtered) 72 | { 73 | cv::circle(pict, cv::Point(filter.position[0], filter.position[1]), filter.weight * scale, 74 | cv::Scalar(200, 0, 200), 2); 75 | } 76 | 77 | cv::imshow("Filtering results", pict); 78 | 79 | int const k = cv::waitKey(100); 80 | return (k != 27) && (k != 1048603); 81 | } 82 | 83 | int main() 84 | { 85 | // Deal with the OpenCV window.. 86 | unsigned int width = 800; 87 | unsigned int height = 800; 88 | namedWindow("Filtering results", cv::WINDOW_AUTOSIZE); 89 | 90 | cv::Mat image = cv::Mat(cv::Size(width, height), 8, CV_8UC3); 91 | 92 | // Declare the target tracker and initialize the motion model 93 | int const n_targets = 5; 94 | GMPHD<2> targetTracker(n_targets); 95 | initTargetTracking(targetTracker); 96 | 97 | // Track the circling targets 98 | std::vector> targetMeas; 99 | std::vector> previousPoses(n_targets); 100 | 101 | Matrix measurements; 102 | float const detectionProbability = 0.5f; 103 | 104 | for (float angle = CV_PI / 2 - 0.03f;; angle += 0.01) 105 | { 106 | image = cv::Mat::zeros(image.size(), CV_8UC3); 107 | 108 | targetMeas.clear(); 109 | 110 | for (unsigned int i = 0; i < n_targets; ++i) 111 | { 112 | // For each target, randomly visible or not 113 | // Make up noisy measurements 114 | if (isTargetVisible(detectionProbability)) 115 | { 116 | measurements[0] = (width >> 1) + 300 * cos(angle) + 117 | (rand() % 2 == 1 ? -1 : 1) * (rand() % 50); 118 | measurements[1] = (height >> 1) + 300 * sin(angle) + 119 | (rand() % 2 == 1 ? -1 : 1) * (rand() % 50); 120 | 121 | targetMeas.push_back({.position = measurements, .speed = {0., 0.}, .weight = 1.}); 122 | 123 | previousPoses[i].first = measurements[0]; 124 | previousPoses[i].second = measurements[1]; 125 | } 126 | } 127 | 128 | // Update the tracker 129 | targetTracker.setNewMeasurements(targetMeas); 130 | 131 | // Get all the predicted targets 132 | targetTracker.propagate(); 133 | const auto targetEstim = targetTracker.getTrackedTargets(0.2f); 134 | 135 | // Show our drawing 136 | if (!display(targetMeas, targetEstim, image)) 137 | { 138 | break; 139 | } 140 | } 141 | 142 | return 1; 143 | } 144 | -------------------------------------------------------------------------------- /gmphd/eigen_tools.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Author : Benjamin Lefaudeux (blefaudeux@github) 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace gmphd { 13 | using namespace std; 14 | using namespace Eigen; 15 | 16 | template 17 | float pseudo_inv(Matrix const &mat_in, 18 | Matrix &mat_out) 19 | { 20 | Matrix U; 21 | Matrix eig_val; 22 | Matrix eig_val_inv; 23 | Matrix V; 24 | float det; 25 | 26 | eig_val_inv = MatrixXf::Identity(size, size); 27 | 28 | // Compute the SVD decomposition 29 | JacobiSVD svd(*mat_in, 30 | ComputeThinU | ComputeThinV); 31 | 32 | eig_val = svd.singularValues(); 33 | U = svd.matrixU(); 34 | V = svd.matrixV(); 35 | 36 | // Compute pseudo-inverse 37 | // - quick'n'dirty inversion of eigen Matrix 38 | for (int i = 0; i < size; ++i) 39 | { 40 | if (eig_val(i, 0) != 0.f) 41 | { 42 | eig_val_inv(i, i) = 1.f / eig_val(i, 0); 43 | } 44 | else 45 | { 46 | eig_val_inv(i, i) = 0.f; 47 | } 48 | } 49 | 50 | *mat_out = V.transpose() * eig_val_inv * U.transpose(); 51 | 52 | // Compute determinant from eigenvalues.. 53 | det = 1.f; 54 | for (int i = 0; i < size; ++i) 55 | { 56 | det *= eig_val(i, 0); 57 | } 58 | 59 | return det; 60 | } 61 | 62 | template 63 | static float mahalanobis(const Matrix &point, 64 | const Matrix &mean, 65 | const Matrix &cov) 66 | { 67 | int ps = point.rows(); 68 | Matrix x_cen = point - mean; 69 | Matrix b = Matrix::Identity(); 70 | 71 | // TODO: Ben - cov needs to be normalized ! 72 | cov.ldlt().solveInPlace(b); 73 | x_cen = b * x_cen; 74 | return (x_cen.transpose() * x_cen).sum(); // FIXME / looks a bit fishy 75 | } 76 | 77 | template 78 | static float gaussDensity(const Matrix &point, 79 | const Matrix &mean, 80 | const Matrix &cov) 81 | { 82 | float det, res; 83 | 84 | Matrix cov_inverse; 85 | Matrix mismatch; 86 | 87 | det = cov.determinant(); 88 | cov_inverse = cov.inverse(); 89 | 90 | mismatch = point - mean; 91 | 92 | Matrix distance = mismatch.transpose() * cov_inverse * mismatch; 93 | distance /= -2.f; 94 | 95 | // Deal with faulty determinant case 96 | if (det == 0.f) 97 | { 98 | return 0.f; 99 | } 100 | 101 | res = 1.f / sqrt(pow(2 * M_PI, T) * fabs(det)) * exp(distance.coeff(0, 0)); 102 | 103 | if (isinf(det)) 104 | { 105 | printf("Problem in multivariate gaussian\n distance : %f - det %f\n", distance.coeff(0, 0), det); 106 | cout << "Cov \n" 107 | << cov << endl 108 | << "Cov inverse \n" 109 | << cov_inverse << endl; 110 | return 0.f; 111 | } 112 | 113 | return res; 114 | } 115 | 116 | } 117 | -------------------------------------------------------------------------------- /gmphd/gaussian_mixture.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // Author : Benjamin Lefaudeux (blefaudeux@github) 3 | 4 | #include "eigen_tools.h" 5 | #include 6 | #include 7 | #include 8 | 9 | namespace gmphd 10 | { 11 | using namespace std; 12 | using namespace Eigen; 13 | 14 | template 15 | struct GaussianModel 16 | { 17 | GaussianModel() 18 | { 19 | clear(); 20 | m_isFalseTarget = false; 21 | } 22 | 23 | GaussianModel &operator=(const GaussianModel &rhs) 24 | { 25 | if (this != &rhs) 26 | { 27 | m_mean = rhs.m_mean; 28 | m_cov = rhs.m_cov; 29 | m_weight = rhs.m_weight; 30 | m_isFalseTarget = rhs.m_isFalseTarget; 31 | } 32 | 33 | return *this; 34 | } 35 | 36 | void clear() 37 | { 38 | m_mean.setZero(); 39 | m_cov.setIdentity(); 40 | m_weight = 0.f; 41 | } 42 | 43 | float m_weight; 44 | Matrix m_mean; 45 | Matrix m_cov; 46 | bool m_isFalseTarget; 47 | }; 48 | 49 | /*! 50 | * \brief The gaussian_mixture is a sum of gaussian models, 51 | * with according weights. Everything is public, no need to get/set... 52 | */ 53 | template 54 | class GaussianMixture 55 | { 56 | public: 57 | GaussianMixture() 58 | { 59 | m_gaussians.clear(); 60 | } 61 | 62 | GaussianMixture(GaussianMixture const &source) 63 | { 64 | m_gaussians = source.m_gaussians; 65 | } 66 | 67 | GaussianMixture(vector> const &source) 68 | { 69 | m_gaussians = source; 70 | } 71 | 72 | GaussianMixture operator=(const GaussianMixture &source) 73 | { 74 | // Skip assignment if same object 75 | if (this == &source) 76 | return *this; 77 | 78 | // Else, use vectors & Eigen "=" operator 79 | m_gaussians = source.m_gaussians; 80 | return *this; 81 | } 82 | 83 | GaussianModel mergeGaussians(vector &i_gaussians_to_merge, bool b_remove_from_mixture) 84 | { 85 | // TODO: Ben - rewrite this crap, could be half way long 86 | 87 | GaussianModel merged_model; 88 | Matrix diff; 89 | 90 | if (i_gaussians_to_merge.size() > 1) 91 | { 92 | // Reset the destination 93 | merged_model.clear(); 94 | 95 | // Build merged gaussian : 96 | // - weight is the sum of all weights 97 | for (auto const &i_g : i_gaussians_to_merge) 98 | { 99 | merged_model.m_weight += m_gaussians[i_g].m_weight; 100 | } 101 | 102 | // - gaussian center is the weighted m_mean of all centers 103 | for (auto const &i_g : i_gaussians_to_merge) 104 | { 105 | merged_model.m_mean += m_gaussians[i_g].m_mean * m_gaussians[i_g].m_weight; 106 | } 107 | 108 | if (merged_model.m_weight != 0.f) 109 | { 110 | merged_model.m_mean /= merged_model.m_weight; 111 | } 112 | 113 | // - covariance is related to initial gaussian model cov and the discrepancy 114 | // from merged m_mean position and every merged gaussian pose 115 | merged_model.m_cov.setZero(); 116 | for (auto const &i_g : i_gaussians_to_merge) 117 | { 118 | diff = merged_model.m_mean - m_gaussians[i_g].m_mean; 119 | 120 | merged_model.m_cov += m_gaussians[i_g].m_weight * (m_gaussians[i_g].m_cov + diff * diff.transpose()); 121 | } 122 | 123 | if (merged_model.m_weight != 0.f) 124 | { 125 | merged_model.m_cov /= merged_model.m_weight; 126 | } 127 | } 128 | else 129 | { 130 | // Just return the initial single gaussian model : 131 | merged_model = m_gaussians[i_gaussians_to_merge[0]]; 132 | } 133 | 134 | if (b_remove_from_mixture) 135 | { 136 | // Remove input gaussians from the mixture 137 | // - sort the index vector 138 | std::sort(i_gaussians_to_merge.begin(), 139 | i_gaussians_to_merge.end()); 140 | 141 | // - pop out the corresponding gaussians, in reverse 142 | auto it = m_gaussians.begin(); 143 | 144 | for (int i = i_gaussians_to_merge.size() - 1; i > -1; ++i) 145 | { 146 | m_gaussians.erase(it + i); 147 | } 148 | } 149 | 150 | return merged_model; 151 | } 152 | 153 | void normalize(float linear_offset) 154 | { 155 | const float sum = std::accumulate(m_gaussians.begin(), m_gaussians.end(), 0.f, [](const GaussianModel &g1, const GaussianModel &g2) { return g1.m_weight + g2.m_weight; }); 156 | 157 | if ((linear_offset + sum) != 0.f) 158 | { 159 | for (auto &gaussian : m_gaussians) 160 | { 161 | gaussian.m_weight /= (linear_offset + sum); 162 | } 163 | } 164 | } 165 | 166 | void normalize(float linear_offset, int start_pos, int stop_pos, int step) 167 | { 168 | float sum = 0.f; 169 | for (int i = start_pos; i < stop_pos; ++i) 170 | { 171 | sum += m_gaussians[i * step].m_weight; 172 | } 173 | 174 | if ((linear_offset + sum) != 0.f) 175 | { 176 | for (int i = start_pos; i < stop_pos; ++i) 177 | { 178 | m_gaussians[i * step].m_weight /= (linear_offset + sum); 179 | } 180 | } 181 | } 182 | 183 | void prune(float trunc_threshold, float merge_threshold, uint max_gaussians) 184 | { 185 | // Sort the gaussians mixture, ascending order 186 | sort(); 187 | 188 | int index, i_best; 189 | 190 | vector i_close_to_best; 191 | GaussianMixture pruned_targets; 192 | GaussianModel merged_gaussian; 193 | 194 | merged_gaussian.clear(); 195 | pruned_targets.m_gaussians.clear(); 196 | 197 | while (!m_gaussians.empty() && pruned_targets.m_gaussians.size() < max_gaussians) 198 | { 199 | // - Pick the biggest gaussian (based on weight) 200 | i_best = selectBestGaussian(); 201 | 202 | if (i_best == -1 || m_gaussians[i_best].m_weight < trunc_threshold) 203 | { 204 | break; 205 | } 206 | else 207 | { 208 | // - Select all the gaussians close enough, to merge if needed 209 | i_close_to_best.clear(); 210 | selectCloseGaussians(i_best, merge_threshold, i_close_to_best); 211 | 212 | // - Build a new merged gaussian 213 | i_close_to_best.push_back(i_best); // Add the initial gaussian 214 | 215 | if (i_close_to_best.size() > 1) 216 | { 217 | merged_gaussian = mergeGaussians(i_close_to_best, false); 218 | } 219 | else 220 | { 221 | merged_gaussian = m_gaussians[i_close_to_best[0]]; 222 | } 223 | 224 | // - Append merged gaussian to the pruned_targets gaussian mixture 225 | pruned_targets.m_gaussians.push_back(merged_gaussian); 226 | 227 | // - Remove all the merged gaussians from current_targets : 228 | // -- Sort the indexes 229 | std::sort(i_close_to_best.begin(), i_close_to_best.end()); 230 | 231 | // -- Remove from the last one (to keep previous indexes unchanged) 232 | while (!i_close_to_best.empty()) 233 | { 234 | index = i_close_to_best.back(); 235 | i_close_to_best.pop_back(); 236 | 237 | m_gaussians.erase(m_gaussians.begin() + index); 238 | } 239 | } 240 | } 241 | 242 | m_gaussians = pruned_targets.m_gaussians; 243 | } 244 | 245 | void sort() 246 | { 247 | std::sort(m_gaussians.begin(), m_gaussians.end(), [](const auto &lhs, const auto &rhs) { 248 | return lhs.m_weight > rhs.m_weight; 249 | }); 250 | } 251 | 252 | void selectCloseGaussians(int i_ref, float threshold, vector &close_gaussians) 253 | { 254 | close_gaussians.clear(); 255 | 256 | float gauss_distance; 257 | 258 | Matrix diff_vec; 259 | Matrix cov_inverse; 260 | 261 | // We only take positions into account there 262 | int i = 0; 263 | for (auto const &gaussian : m_gaussians) 264 | { 265 | if (i != i_ref) 266 | { 267 | // Compute distance 268 | diff_vec = m_gaussians[i_ref].m_mean.head(D) - 269 | gaussian.m_mean.head(D); 270 | 271 | cov_inverse = (m_gaussians[i_ref].m_cov.topLeftCorner(D, D)).inverse(); 272 | 273 | gauss_distance = diff_vec.transpose() * 274 | cov_inverse.topLeftCorner(D, D) * 275 | diff_vec; 276 | 277 | // Add to the set of close gaussians, if below threshold 278 | if ((gauss_distance < threshold) && (gaussian.m_weight != 0.f)) 279 | { 280 | close_gaussians.push_back(i); 281 | } 282 | } 283 | ++i; 284 | } 285 | } 286 | 287 | int selectBestGaussian() 288 | { 289 | float best_weight = 0.f; 290 | int best_index = -1; 291 | int i = 0; 292 | 293 | std::for_each(m_gaussians.begin(), m_gaussians.end(), [&](GaussianModel const &gaussian) { 294 | if (gaussian.m_weight > best_weight) 295 | { 296 | best_weight = gaussian.m_weight; 297 | best_index = i; 298 | } 299 | ++i; 300 | }); 301 | 302 | return best_index; 303 | } 304 | 305 | // void changeReferential(const Matrix4f &transform) 306 | // { 307 | // Matrix temp_vec, temp_vec_new; 308 | // temp_vec(3, 0) = 1.f; 309 | 310 | // // Gaussian model : 311 | // // - [x, y, z, dx/dt, dy/dt, dz/dt] m_mean values 312 | // // - 6x6 covariance 313 | 314 | // // For every gaussian model, change referential 315 | // for (auto &gaussian : m_gaussians) 316 | // { 317 | // // Change positions 318 | // temp_vec.block(0, 0, 3, 1) = gaussian.m_mean.block(0, 0, 3, 1); 319 | 320 | // temp_vec_new = transform * temp_vec; 321 | 322 | // gaussian.m_mean.block(0, 0, 3, 1) = temp_vec_new.block(0, 0, 3, 1); 323 | 324 | // // Change speeds referential 325 | // temp_vec.block(0, 0, 3, 1) = gaussian.m_mean.block(3, 0, 3, 1); 326 | 327 | // temp_vec_new = transform * temp_vec; 328 | 329 | // gaussian.m_mean.block(3, 0, 3, 1) = temp_vec_new.block(0, 0, 3, 1); 330 | 331 | // // Change covariance referential 332 | // // (only take the rotation into account) 333 | // // TODO 334 | // } 335 | // } 336 | 337 | public: 338 | vector> m_gaussians; 339 | }; 340 | } // namespace gmphd -------------------------------------------------------------------------------- /gmphd/gmphd_filter.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Author : Benjamin Lefaudeux (blefaudeux@github) 4 | 5 | #include "gaussian_mixture.h" 6 | #include 7 | #include 8 | 9 | namespace gmphd 10 | { 11 | using namespace Eigen; 12 | 13 | template 14 | struct SpawningModel 15 | { 16 | SpawningModel() 17 | { 18 | m_trans.setIdentity(); 19 | m_cov.setIdentity(); 20 | m_offset.setZero(); 21 | m_weight = 0.1f; 22 | } 23 | 24 | float m_weight; 25 | 26 | Matrix m_trans; 27 | Matrix m_cov; 28 | Matrix m_offset; 29 | }; 30 | 31 | template 32 | struct Target 33 | { 34 | Matrix position; 35 | Matrix speed; 36 | float weight; 37 | }; 38 | 39 | template 40 | class GMPHD 41 | { 42 | static const size_t S = D * 2; 43 | 44 | public: 45 | GMPHD(int max_gaussians) : m_maxGaussians(max_gaussians) 46 | 47 | { 48 | m_pruneTruncThld = 0.f; 49 | m_pDetection = 0.f; 50 | m_pSurvival = 0.f; 51 | 52 | // Initialize all gaussian mixtures, we know the dimension now 53 | m_measTargets.reset(new GaussianMixture()); 54 | m_birthTargets.reset(new GaussianMixture()); 55 | m_currTargets.reset(new GaussianMixture()); 56 | m_expTargets.reset(new GaussianMixture()); 57 | m_extractedTargets.reset(new GaussianMixture()); 58 | m_spawnTargets.reset(new GaussianMixture()); 59 | } 60 | 61 | // Input: raw measurements and possible ref change 62 | void setNewReferential(Matrix4f const &transform) 63 | { 64 | // Change referential for every gaussian in the gaussian mixture 65 | m_currTargets->changeReferential(transform); 66 | } 67 | 68 | void setNewMeasurements(std::vector> const &measurements) 69 | { 70 | // Clear the gaussian mixture 71 | m_measTargets->m_gaussians.clear(); 72 | 73 | for (const auto &meas : measurements) 74 | { 75 | // Create new gaussian model according to measurement 76 | GaussianModel new_obs; 77 | new_obs.m_mean.template head() = meas.position; 78 | new_obs.m_mean.template tail() = meas.speed; 79 | new_obs.m_cov = m_obsCov; 80 | new_obs.m_weight = meas.weight; 81 | 82 | m_measTargets->m_gaussians.push_back(std::move(new_obs)); 83 | } 84 | } 85 | 86 | // Output 87 | std::vector> getTrackedTargets(float const &extract_thld) 88 | { 89 | // Get through every target, keep the ones whose weight is above threshold 90 | float const thld = std::max(extract_thld, 0.f); 91 | m_extractedTargets->m_gaussians.clear(); 92 | std::copy_if(begin(m_currTargets->m_gaussians), end(m_currTargets->m_gaussians), std::back_inserter(m_extractedTargets->m_gaussians), [&thld](const GaussianModel &gaussian) { return gaussian.m_weight >= thld; }); 93 | 94 | // Fill in "extracted_targets" from the "current_targets" 95 | std::vector> targets; 96 | for (auto const &gaussian : m_extractedTargets->m_gaussians) 97 | { 98 | targets.push_back({.position = gaussian.m_mean.template head(), .speed = gaussian.m_mean.template tail(), .weight = gaussian.m_weight}); 99 | } 100 | return targets; 101 | } 102 | 103 | // Parameters to set before use 104 | void setDynamicsModel(float sampling, float processNoise) 105 | { 106 | m_samplingPeriod = sampling; 107 | m_processNoise = processNoise; 108 | 109 | // Fill in propagation matrix : 110 | m_tgtDynTrans.setIdentity(); 111 | 112 | for (uint i = 0; i < D; ++i) 113 | { 114 | m_tgtDynTrans(i, D + i) = m_samplingPeriod; 115 | } 116 | 117 | // Fill in covariance matrix 118 | // Extra covariance added by the dynamics. Could be 0. 119 | m_tgtDynCov = processNoise * processNoise * Matrix::Identity(); 120 | } 121 | 122 | void setDynamicsModel(MatrixXf const &tgtDynTransitions, MatrixXf const &tgtDynCovariance) 123 | { 124 | m_tgtDynTrans = tgtDynTransitions; 125 | m_tgtDynCov = tgtDynCovariance; 126 | } 127 | 128 | void setSurvivalProbability(float prob_survival) 129 | { 130 | m_pSurvival = prob_survival; 131 | } 132 | 133 | void setObservationModel(float probDetectionOverall, float measNoisePose, 134 | float measNoiseSpeed, float measNoiseBackground) 135 | { 136 | m_pDetection = probDetectionOverall; 137 | m_measNoisePose = measNoisePose; 138 | m_measNoiseSpeed = measNoiseSpeed; 139 | m_measNoiseBackground = measNoiseBackground; // False detection probability 140 | 141 | // Set model matrices 142 | m_obsMat.setIdentity(); 143 | m_obsMatT = m_obsMat.transpose(); 144 | m_obsCov.setIdentity(); 145 | 146 | m_obsCov.template topLeftCorner() *= m_measNoisePose * m_measNoisePose; 147 | m_obsCov.template bottomRightCorner() *= m_measNoiseSpeed * m_measNoiseSpeed; 148 | } 149 | 150 | void setPruningParameters(float prune_trunc_thld, float prune_merge_thld, 151 | int prune_max_nb) 152 | { 153 | m_pruneTruncThld = prune_trunc_thld; 154 | m_pruneMergeThld = prune_merge_thld; 155 | m_nMaxPrune = prune_max_nb; 156 | } 157 | 158 | void setBirthModel(const GaussianMixture &birthModel) 159 | { 160 | m_birthModel.reset(new GaussianMixture(birthModel)); 161 | 162 | // Mark the targets as "false", in that they do not match any measure really 163 | for (auto &gaussian : m_birthModel->m_gaussians) 164 | { 165 | gaussian.m_isFalseTarget = true; 166 | } 167 | } 168 | 169 | void setSpawnModel(std::vector> &spawnModels) 170 | { 171 | m_spawnModels = spawnModels; 172 | } 173 | 174 | void propagate() 175 | { 176 | m_nPredTargets = 0; 177 | 178 | // Predict new targets (spawns): 179 | predictBirth(); 180 | 181 | // Predict propagation of expected targets 182 | predictTargets(); 183 | 184 | // Build the update components 185 | buildUpdate(); 186 | 187 | // Update the probabilities 188 | update(); 189 | 190 | // Prune gaussians (remove weakest, merge close enough gaussians) 191 | pruneGaussians(); 192 | 193 | // Clean std::vectors : 194 | m_expMeasure.clear(); 195 | m_expDisp.clear(); 196 | m_uncertainty.clear(); 197 | m_covariance.clear(); 198 | } 199 | 200 | void reset() 201 | { 202 | m_currTargets->m_gaussians.clear(); 203 | m_extractedTargets->m_gaussians.clear(); 204 | } 205 | 206 | private: 207 | /*! 208 | * \brief The spawning models (how gaussians spawn from existing targets) 209 | * Example : how airplanes take off from a carrier.. 210 | */ 211 | std::vector> m_spawnModels; 212 | 213 | void buildUpdate() 214 | { 215 | 216 | // Concatenate all the wannabe targets : 217 | // - birth targets 218 | if (m_birthTargets->m_gaussians.size() > 0) 219 | { 220 | m_iBirthTargets.resize(m_birthTargets->m_gaussians.size()); 221 | std::iota(m_iBirthTargets.begin(), m_iBirthTargets.end(), m_birthTargets->m_gaussians.size()); 222 | m_expTargets->m_gaussians.insert(m_expTargets->m_gaussians.end(), m_birthTargets->m_gaussians.begin(), 223 | m_birthTargets->m_gaussians.begin() + m_birthTargets->m_gaussians.size()); 224 | } 225 | 226 | // - spawned targets 227 | if (m_spawnTargets->m_gaussians.size() > 0) 228 | { 229 | m_expTargets->m_gaussians.insert(m_expTargets->m_gaussians.end(), m_spawnTargets->m_gaussians.begin(), 230 | m_spawnTargets->m_gaussians.begin() + m_spawnTargets->m_gaussians.size()); 231 | } 232 | 233 | // Compute PHD update components (for every expected target) 234 | m_nPredTargets = m_expTargets->m_gaussians.size(); 235 | 236 | m_expMeasure.clear(); 237 | m_expMeasure.reserve(m_nPredTargets); 238 | 239 | m_expDisp.clear(); 240 | m_expDisp.reserve(m_nPredTargets); 241 | 242 | m_uncertainty.clear(); 243 | m_uncertainty.reserve(m_nPredTargets); 244 | 245 | m_covariance.clear(); 246 | m_covariance.reserve(m_nPredTargets); 247 | 248 | for (auto const &tgt : m_expTargets->m_gaussians) 249 | { 250 | // Compute the expected measurement 251 | m_expMeasure.push_back(m_obsMat * tgt.m_mean); 252 | m_expDisp.push_back(m_obsCov + m_obsMat * tgt.m_cov * m_obsMatT); 253 | 254 | m_uncertainty.push_back(tgt.m_cov * m_obsMatT * m_expDisp.back().inverse()); 255 | m_covariance.push_back((Matrix::Identity() - m_uncertainty.back() * m_obsMat) * tgt.m_cov); 256 | } 257 | } 258 | 259 | void predictBirth() 260 | { 261 | m_spawnTargets->m_gaussians.clear(); 262 | m_birthTargets->m_gaussians.clear(); 263 | 264 | // ----------------------------------------- 265 | // Compute spontaneous births 266 | m_birthTargets->m_gaussians = m_birthModel->m_gaussians; 267 | 268 | m_nPredTargets += m_birthTargets->m_gaussians.size(); 269 | 270 | // ----------------------------------------- 271 | // Compute spawned targets 272 | for (auto const &curr : m_currTargets->m_gaussians) 273 | { 274 | for (auto const &spawn : m_spawnModels) 275 | { 276 | GaussianModel new_spawn; 277 | 278 | // Define a gaussian model from the existing target 279 | // and spawning properties 280 | new_spawn.m_weight = curr.m_weight * spawn.m_weight; 281 | new_spawn.m_mean = spawn.m_offset + spawn.m_trans * curr.m_mean; 282 | new_spawn.m_cov = spawn.m_cov + spawn.m_trans * curr.m_cov * spawn.m_trans.transpose(); 283 | new_spawn.m_isFalseTarget = true; 284 | 285 | // Add this new gaussian to the list of expected targets 286 | m_spawnTargets->m_gaussians.push_back(std::move(new_spawn)); 287 | 288 | // Update the number of expected targets 289 | ++m_nPredTargets; 290 | } 291 | } 292 | } 293 | 294 | void predictTargets() 295 | { 296 | m_expTargets->m_gaussians.clear(); 297 | m_expTargets->m_gaussians.reserve(m_currTargets->m_gaussians.size()); 298 | 299 | for (auto const &curr : m_currTargets->m_gaussians) 300 | { 301 | // Compute the new shape of the target 302 | GaussianModel new_target; 303 | new_target.m_weight = m_pSurvival * curr.m_weight; 304 | new_target.m_mean = m_tgtDynTrans * curr.m_mean; 305 | new_target.m_cov = m_tgtDynCov + m_tgtDynTrans * curr.m_cov * m_tgtDynTrans.transpose(); 306 | 307 | // Push back to the expected targets 308 | m_expTargets->m_gaussians.push_back(new_target); 309 | ++m_nPredTargets; 310 | } 311 | } 312 | 313 | void pruneGaussians() 314 | { 315 | m_currTargets->prune(m_pruneTruncThld, m_pruneMergeThld, m_nMaxPrune); 316 | } 317 | 318 | void update() 319 | { 320 | m_currTargets->m_gaussians.clear(); 321 | 322 | // We'll consider every possible association : std::vector size is (expected targets)*(measured targets) 323 | m_currTargets->m_gaussians.reserve((m_measTargets->m_gaussians.size() + 1) * 324 | m_expTargets->m_gaussians.size()); 325 | 326 | // First set of gaussians : mere propagation of existing ones 327 | // don't propagate the "birth" targets... we set their weight to 0 328 | for (auto const &target : m_expTargets->m_gaussians) 329 | { 330 | // Copy the target into the final set, adjust the weight if it was spawned 331 | auto newTarget = target; 332 | newTarget.m_weight = target.m_isFalseTarget ? 0.f : (1.f - m_pDetection) * target.m_weight; 333 | m_currTargets->m_gaussians.emplace_back(std::move(newTarget)); 334 | } 335 | 336 | // Second set of gaussians : match observations and previsions 337 | for (auto &measuredTarget : m_measTargets->m_gaussians) 338 | { 339 | uint start_normalize = m_currTargets->m_gaussians.size(); 340 | 341 | for (uint n_targt = 0; n_targt < m_expTargets->m_gaussians.size(); ++n_targt) 342 | { 343 | 344 | // Compute matching factor between predictions and measures. 345 | const auto distance = mahalanobis<2>(measuredTarget.m_mean.template head(), 346 | m_expMeasure[n_targt].template head(), 347 | m_expDisp[n_targt].template topLeftCorner()); 348 | 349 | GaussianModel matchTarget; 350 | 351 | matchTarget.m_weight = m_pDetection * m_expTargets->m_gaussians[n_targt].m_weight / distance; 352 | 353 | matchTarget.m_mean = m_expTargets->m_gaussians[n_targt].m_mean + 354 | m_uncertainty[n_targt] * (measuredTarget.m_mean - m_expMeasure[n_targt]); 355 | 356 | matchTarget.m_cov = m_covariance[n_targt]; 357 | 358 | m_currTargets->m_gaussians.emplace_back(std::move(matchTarget)); 359 | } 360 | 361 | // Normalize weights in the same predicted set, taking clutter into account 362 | m_currTargets->normalize(m_measNoiseBackground, start_normalize, 363 | m_currTargets->m_gaussians.size(), 1); 364 | } 365 | } 366 | 367 | private: 368 | bool m_motionModel; 369 | 370 | uint m_maxGaussians; 371 | uint m_nPredTargets; 372 | uint m_nCurrentTargets; 373 | uint m_nMaxPrune; 374 | 375 | float m_pSurvival; 376 | float m_pDetection; 377 | 378 | float m_samplingPeriod; 379 | float m_processNoise; 380 | 381 | float m_pruneMergeThld; 382 | float m_pruneTruncThld; 383 | 384 | float m_measNoisePose; 385 | float m_measNoiseSpeed; 386 | float m_measNoiseBackground; // Background detection "noise", other models are possible.. 387 | 388 | std::vector m_iBirthTargets; 389 | 390 | Matrix m_tgtDynTrans; 391 | Matrix m_tgtDynCov; 392 | 393 | Matrix m_obsMat; 394 | Matrix m_obsMatT; 395 | Matrix m_obsCov; 396 | 397 | // Temporary matrices, used for the update process 398 | std::vector> m_covariance; 399 | std::vector> m_expMeasure; 400 | std::vector> m_expDisp; 401 | std::vector> m_uncertainty; 402 | 403 | std::unique_ptr> m_birthModel; 404 | 405 | std::unique_ptr> m_birthTargets; 406 | std::unique_ptr> m_currTargets; 407 | std::unique_ptr> m_expTargets; 408 | std::unique_ptr> m_extractedTargets; 409 | std::unique_ptr> m_measTargets; 410 | std::unique_ptr> m_spawnTargets; 411 | }; 412 | } --------------------------------------------------------------------------------