From 24912a8fc36fb0ecc4112159121f6b1e7a424412 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 4 Jan 2025 19:51:50 +0000 Subject: [PATCH 1/7] Update repo config files --- .clang-format | 64 +----- .github/ISSUE_TEMPLATE/1_bug_report.md | 23 ++ .github/ISSUE_TEMPLATE/2_feature_request.md | 17 ++ .github/ISSUE_TEMPLATE/3_need_help.md | 15 ++ .github/pull_request_template.md | 19 ++ .github/workflows/pre-commit.yml | 16 ++ .gitignore | 222 +++++++++++++++++--- .pre-commit-config.yaml | 23 ++ .rdmanifest | 16 -- Doxyfile | 49 ----- Jenkinsfile | 7 - 11 files changed, 310 insertions(+), 161 deletions(-) create mode 100644 .github/ISSUE_TEMPLATE/1_bug_report.md create mode 100644 .github/ISSUE_TEMPLATE/2_feature_request.md create mode 100644 .github/ISSUE_TEMPLATE/3_need_help.md create mode 100644 .github/pull_request_template.md create mode 100644 .github/workflows/pre-commit.yml create mode 100644 .pre-commit-config.yaml delete mode 100644 .rdmanifest delete mode 100644 Doxyfile delete mode 100644 Jenkinsfile diff --git a/.clang-format b/.clang-format index 1e2ef9d..3442bc7 100644 --- a/.clang-format +++ b/.clang-format @@ -1,62 +1,4 @@ ---- -AccessModifierOffset: '-4' -AlignAfterOpenBracket: DontAlign -AlignConsecutiveAssignments: 'false' -AlignConsecutiveDeclarations: 'false' -AlignEscapedNewlines: Left -AlignOperands: 'true' -AlignTrailingComments: 'false' -AllowAllParametersOfDeclarationOnNextLine: 'false' -AllowShortBlocksOnASingleLine: 'false' -AllowShortCaseLabelsOnASingleLine: 'true' -AllowShortFunctionsOnASingleLine: Empty -AllowShortIfStatementsOnASingleLine: 'false' -AllowShortLoopsOnASingleLine: 'false' -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: 'false' -AlwaysBreakTemplateDeclarations: 'true' -BinPackArguments: 'true' -BinPackParameters: 'false' -BreakBeforeBinaryOperators: None -BreakBeforeBraces: Attach -BreakBeforeInheritanceComma: 'false' -BreakBeforeTernaryOperators: 'true' -BreakConstructorInitializers: AfterColon -BreakStringLiterals: 'false' -ColumnLimit: '80' -CompactNamespaces: 'false' -ConstructorInitializerAllOnOneLineOrOnePerLine: 'true' -ConstructorInitializerIndentWidth: '8' -ContinuationIndentWidth: '8' -Cpp11BracedListStyle: 'true' -DerivePointerAlignment: 'false' -DisableFormat: 'false' -ExperimentalAutoDetectBinPacking: 'false' -IndentCaseLabels: 'true' -IndentWidth: '4' -IndentWrappedFunctionNames: 'false' -KeepEmptyLinesAtTheStartOfBlocks: 'false' +--- Language: Cpp -MaxEmptyLinesToKeep: '2' -NamespaceIndentation: None -PenaltyReturnTypeOnItsOwnLine: '0' -PointerAlignment: Left -SortIncludes: 'false' -SortUsingDeclarations: 'true' -SpaceAfterCStyleCast: 'true' -SpaceAfterTemplateKeyword: 'true' -SpaceBeforeAssignmentOperators: 'true' -SpaceBeforeParens: ControlStatements -SpaceInEmptyParentheses: 'false' -SpacesBeforeTrailingComments: '2' -SpacesInAngles: 'false' -SpacesInCStyleCastParentheses: 'false' -SpacesInContainerLiterals: 'false' -SpacesInParentheses: 'false' -SpacesInSquareBrackets: 'false' -Standard: Cpp11 -TabWidth: '4' -UseTab: Never - -... +BasedOnStyle: Google +ColumnLimit: 120 diff --git a/.github/ISSUE_TEMPLATE/1_bug_report.md b/.github/ISSUE_TEMPLATE/1_bug_report.md new file mode 100644 index 0000000..cc12b25 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/1_bug_report.md @@ -0,0 +1,23 @@ +--- +name: "\U0001F41B Bug report" +about: Report a misbehavior or other bug +labels: 'bug' +--- +# Summary + + +## Expected behavior + + +## Current behavior + + +## Steps to Reproduce + + +1. +2. +3. + +## Possible Solution + diff --git a/.github/ISSUE_TEMPLATE/2_feature_request.md b/.github/ISSUE_TEMPLATE/2_feature_request.md new file mode 100644 index 0000000..875cf49 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/2_feature_request.md @@ -0,0 +1,17 @@ +--- +name: "\U0001F680 Feature request" +about: Suggest an idea for this project +labels: 'enhancement' +--- +# Summary + + +## Is your feature request related to a problem? + + +## Describe the solution you'd like + + +## Describe alternatives you've considered + + diff --git a/.github/ISSUE_TEMPLATE/3_need_help.md b/.github/ISSUE_TEMPLATE/3_need_help.md new file mode 100644 index 0000000..48cba38 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/3_need_help.md @@ -0,0 +1,15 @@ +--- +name: "⁉️ Help me" +about: "Get help with using or improving our software" +labels: 'question' +--- + + +# What I'm trying to do + + +## What I've tried + + +## Additional context + diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..41cf5a8 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,19 @@ +# Summary + + +## Proposed changes + + +## Related issues + + + + +## Checklist + +- [ ] Run `colcon build` +- [ ] Write documentation +- [ ] Create issues for future work +- [ ] Test on your machine +- [ ] Test on the robot +- [ ] This PR is on our `Software` project board diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 0000000..bb68480 --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,16 @@ +name: Code style checks + +on: + pull_request: + push: + branches: [main] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v4 + - name: Install cppcheck + run: sudo apt install cppcheck -y + - uses: pre-commit/action@v3.0.0 diff --git a/.gitignore b/.gitignore index 259148f..a01e461 100644 --- a/.gitignore +++ b/.gitignore @@ -1,32 +1,198 @@ -# Prerequisites -*.d +# Created by .ignore support plugin (hsz.mobi) +### JetBrains template +# Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm +# Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 -# Compiled Object files -*.slo -*.lo -*.o -*.obj +# auto-generated documentation +**/docs/_build +**/docs/_out +**/docs/cppapi +**/docs/pyapi -# Precompiled Headers -*.gch -*.pch +# auto-generated documentation +**/docs/_build +**/docs/_out +**/docs/cppapi +**/docs/pyapi -# Compiled Dynamic libraries +# User-specific stuff: +.idea/workspace.xml +.idea/tasks.xml +.idea/* + +# Sensitive or high-churn files: +.idea/dataSources/ +.idea/dataSources.ids +.idea/dataSources.xml +.idea/dataSources.local.xml +.idea/sqlDataSources.xml +.idea/dynamic.xml +.idea/uiDesigner.xml + +# Gradle: +.idea/gradle.xml +.idea/libraries + +# Mongo Explorer plugin: +.idea/mongoSettings.xml + +## File-based project format: +*.iws + +## Plugin-specific files: + +# IntelliJ +/out/ + +# mpeltonen/sbt-idea plugin +.idea_modules/ + +# JIRA plugin +atlassian-ide-plugin.xml + +# Crashlytics plugin (for Android Studio and IntelliJ) +com_crashlytics_export_strings.xml +crashlytics.properties +crashlytics-build.properties +fabric.properties +### ROS template +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +*/cfg/cpp/ +*/src/*/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +### Python template +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions *.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app + +# Distribution / packaging +.Python +env/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ +lib64/ +parts/ +sdist/ +var/ +*.egg-info/ +.installed.cfg +*.egg + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*,cover +.hypothesis/ + +# Translations +*.mo +*.pot + +# Django stuff: +*.log +local_settings.py + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy + +# Sphinx documentation +docs/_build/ + +# PyBuilder +target/ + +# Jupyter Notebook +.ipynb_checkpoints + +# pyenv +.python-version + +# celery beat schedule file +celerybeat-schedule + +# dotenv +.env + +# virtualenv +.venv/ +venv/ +ENV/ + +# Spyder project settings +.spyderproject + +# Rope project settings +.ropeproject + +# Ruff cache +.ruff_cache diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..561c3bd --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,23 @@ +repos: +- repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.1.6 + hooks: + - id: ruff + args: + - "--fix" + - "--exit-non-zero-on-fix" +- repo: https://github.com/psf/black + rev: 23.11.0 + hooks: + - id: black +- repo: https://github.com/pocc/pre-commit-hooks + rev: v1.3.5 + hooks: + - id: clang-format + args: + - "-i" + - id: cppcheck + args: + - "--suppress=missingInclude" + - "--suppress=unmatchedSuppression" + - "--suppress=unusedFunction" diff --git a/.rdmanifest b/.rdmanifest deleted file mode 100644 index 12f7624..0000000 --- a/.rdmanifest +++ /dev/null @@ -1,16 +0,0 @@ ---- -# See http://doku.bit-bots.de/meta/manual/software/ci.html#make-package-resolvable-in-ci -check-presence-script: '#!/bin/bash - - test -d $BITBOTS_CATKIN_WORKSPACE/src/particle_filter' -depends: -- geometry_msgs -- rosconsole -- roscpp -- std_msgs -- visualization_msgs -exec-path: particle_filter-master -install-script: '#!/bin/bash - - cp -r . $BITBOTS_CATKIN_WORKSPACE/src/particle_filter' -uri: https://github.com/bit-bots/particle_filter/archive/refs/heads/master.tar.gz diff --git a/Doxyfile b/Doxyfile deleted file mode 100644 index 3c01855..0000000 --- a/Doxyfile +++ /dev/null @@ -1,49 +0,0 @@ -DOXYFILE_ENCODING = UTF-8 -PROJECT_NAME = "particle_filter" -PROJECT_BRIEF = "Particle Filter Library for the ROS context" -OUTPUT_DIRECTORY = docs -ALLOW_UNICODE_NAMES = YES -OUTPUT_LANGUAGE = English -BRIEF_MEMBER_DESC = YES -REPEAT_BRIEF = YES -FULL_PATH_NAMES = YES -MULTILINE_CPP_IS_BRIEF = NO -INHERIT_DOCS = YES -SEPARATE_MEMBER_PAGES = NO -TAB_SIZE = 4 -MARKDOWN_SUPPORT = YES -AUTOLINK_SUPPORT = YES -BUILTIN_STL_SUPPORT = YES -SUBGROUPING = YES -LOOKUP_CACHE_SIZE = 2 -EXTRACT_ALL = YES -EXTRACT_PRIVATE = YES -EXTRACT_LOCAL_CLASSES = YES -HIDE_UNDOC_MEMBERS = NO -HIDE_UNDOC_CLASSES = NO -HIDE_FRIEND_COMPOUNDS = NO -CASE_SENSE_NAMES = YES -HIDE_SCOPE_NAMES = NO -HIDE_COMPOUND_REFERENCE= NO -SHOW_INCLUDE_FILES = YES -SHOW_GROUPED_MEMB_INC = NO -FORCE_LOCAL_INCLUDES = NO -INLINE_INFO = YES -SORT_MEMBER_DOCS = YES -STRICT_PROTO_MATCHING = NO -MAX_INITIALIZER_LINES = 30 -SHOW_USED_FILES = YES -SHOW_FILES = YES -SHOW_NAMESPACES = YES -QUIET = YES -WARNINGS = YES -WARN_IF_UNDOCUMENTED = YES -WARN_IF_DOC_ERROR = YES -WARN_NO_PARAMDOC = NO -WARN_AS_ERROR = NO -WARN_FORMAT = "$file:$line: $text" -INPUT = include/particle_filter src -INPUT_ENCODING = UTF-8 -RECURSIVE = YES -DISABLE_INDEX = NO -GENERATE_TREEVIEW = NO diff --git a/Jenkinsfile b/Jenkinsfile deleted file mode 100644 index f3f08e8..0000000 --- a/Jenkinsfile +++ /dev/null @@ -1,7 +0,0 @@ -@Library('bitbots_jenkins_library') import de.bitbots.jenkins.*; - -defineProperties() - -def pipeline = new BitbotsPipeline(this, env, currentBuild, scm) -pipeline.configurePipelineForPackage(new PackagePipelineSettings(new PackageDefinition("particle_filter", ".")).withoutDocumentation().withoutPublishing()) -pipeline.execute() \ No newline at end of file From f723d2814904a75a4f59f30bc401f6271bf800ec Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 4 Jan 2025 19:55:59 +0000 Subject: [PATCH 2/7] Apply formatting --- .../particle_filter/CRandomNumberGenerator.h | 70 +- .../particle_filter/CompareParticleWeights.h | 18 +- .../particle_filter/ImportanceResampling.h | 116 ++- include/particle_filter/MovementModel.h | 49 +- include/particle_filter/ObservationModel.h | 46 +- include/particle_filter/Particle.h | 146 ++-- include/particle_filter/ParticleFilter.h | 726 +++++++++--------- include/particle_filter/ParticleFilter.hxx | 626 +++++++-------- .../RandomNumberGenerationStrategy.h | 60 +- include/particle_filter/ResamplingStrategy.h | 55 +- include/particle_filter/StateDistribution.h | 43 +- include/particle_filter/defs.h | 12 +- include/particle_filter/gaussian.h | 101 +-- .../particle_filter/gaussian_mixture_model.h | 349 ++++----- include/particle_filter/gmm_classifier.h | 108 ++- include/particle_filter/gmm_regressor.h | 114 ++- include/particle_filter/k_means.h | 100 ++- include/particle_filter/matrix_io.h | 22 +- include/particle_filter/pca.h | 53 +- src/CRandomNumberGenerator.cpp | 63 +- src/gaussian.cpp | 168 ++-- src/gaussian_mixture_model.cpp | 479 ++++++------ src/gmm_classifier.cpp | 342 ++++----- src/gmm_regressor.cpp | 276 ++++--- src/k_means.cpp | 161 ++-- src/matrix_io.cpp | 111 ++- src/pca.cpp | 49 +- 27 files changed, 2089 insertions(+), 2374 deletions(-) diff --git a/include/particle_filter/CRandomNumberGenerator.h b/include/particle_filter/CRandomNumberGenerator.h index 111ce29..f077297 100644 --- a/include/particle_filter/CRandomNumberGenerator.h +++ b/include/particle_filter/CRandomNumberGenerator.h @@ -17,47 +17,47 @@ namespace particle_filter { * @author Stephan Wirth */ class CRandomNumberGenerator : public RandomNumberGenerationStrategy { -public: - /** - * The constructor calls init(). - */ - CRandomNumberGenerator(); + public: + /** + * The constructor calls init(). + */ + CRandomNumberGenerator(); - /** - * Empty destructor. - */ - ~CRandomNumberGenerator(); + /** + * Empty destructor. + */ + ~CRandomNumberGenerator(); - /** - * This method creates gaussian distributed random numbers (Box-Müller - * method). - * @param standardDeviation Standard deviation d of the random number to - * generate. - * @return N(0, d*d)-distributed random number - */ - double getGaussian(double standardDeviation) const; + /** + * This method creates gaussian distributed random numbers (Box-Müller + * method). + * @param standardDeviation Standard deviation d of the random number to + * generate. + * @return N(0, d*d)-distributed random number + */ + double getGaussian(double standardDeviation) const; - /** - * Generates a uniform distributed random number between min and max. - * @param min the minimum value, default is 0.0 - * @param max the maximum value, default is 1.0 - * @return random number between min and max, uniform distributed. - */ - double getUniform(double min = 0.0, double max = 1.0) const; + /** + * Generates a uniform distributed random number between min and max. + * @param min the minimum value, default is 0.0 + * @param max the maximum value, default is 1.0 + * @return random number between min and max, uniform distributed. + */ + double getUniform(double min = 0.0, double max = 1.0) const; -protected: - /** - * Initializes the seed by calling srand(time(0)) - */ - void init(); + protected: + /** + * Initializes the seed by calling srand(time(0)) + */ + void init(); -private: - /// stores if there is a buffered gaussian variable or not - mutable bool m_GaussianBufferFilled; + private: + /// stores if there is a buffered gaussian variable or not + mutable bool m_GaussianBufferFilled; - /// buffer for a gaussian distributed variable, the Box-Müller method always - /// creates two variables, we return one and store the other here. - mutable double m_GaussianBufferVariable; + /// buffer for a gaussian distributed variable, the Box-Müller method always + /// creates two variables, we return one and store the other here. + mutable double m_GaussianBufferVariable; }; } // namespace particle_filter diff --git a/include/particle_filter/CompareParticleWeights.h b/include/particle_filter/CompareParticleWeights.h index aa6a1a0..3afb77b 100644 --- a/include/particle_filter/CompareParticleWeights.h +++ b/include/particle_filter/CompareParticleWeights.h @@ -21,15 +21,15 @@ namespace particle_filter { */ template class CompareParticleWeights { -public: - /** - * @return true if the weight of the particle p1 is higher than the weight - * of particle p2. - */ - bool operator()(const particle_filter::Particle* p1, - const particle_filter::Particle* p2) const { - return p1->getWeight() > p2->getWeight(); - } + public: + /** + * @return true if the weight of the particle p1 is higher than the weight + * of particle p2. + */ + bool operator()(const particle_filter::Particle* p1, + const particle_filter::Particle* p2) const { + return p1->getWeight() > p2->getWeight(); + } }; } // namespace particle_filter diff --git a/include/particle_filter/ImportanceResampling.h b/include/particle_filter/ImportanceResampling.h index fd13d37..e8bf77b 100644 --- a/include/particle_filter/ImportanceResampling.h +++ b/include/particle_filter/ImportanceResampling.h @@ -2,6 +2,7 @@ #define IMPORTANCERESAMPLING_H #include + #include "particle_filter/CRandomNumberGenerator.h" #include "particle_filter/Particle.h" @@ -22,84 +23,77 @@ namespace particle_filter { template class ImportanceResampling : public ResamplingStrategy { - /** - * A ParticleList is an array of pointers to Particles. - */ - typedef std::vector*> ParticleList; + /** + * A ParticleList is an array of pointers to Particles. + */ + typedef std::vector*> ParticleList; -public: - /** - * The constructor of this base class inits some members. - */ - ImportanceResampling(bool reset_weights = false, double particle_reset_weight = 0); + public: + /** + * The constructor of this base class inits some members. + */ + ImportanceResampling(bool reset_weights = false, double particle_reset_weight = 0); - /** - * The destructor is empty. - */ - virtual ~ImportanceResampling(); + /** + * The destructor is empty. + */ + virtual ~ImportanceResampling(); - /** - * This is the main method of ImportanceResampling. It takes two references - * to particle lists. The first reference refers to the old particle list, - * the second to the new one. - * @param source the source list to draw new particles from. - * @param destination the destination list where to put the copies. - */ - void - resample(const ParticleList& source, const ParticleList& destination) const; + /** + * This is the main method of ImportanceResampling. It takes two references + * to particle lists. The first reference refers to the old particle list, + * the second to the new one. + * @param source the source list to draw new particles from. + * @param destination the destination list where to put the copies. + */ + void resample(const ParticleList& source, const ParticleList& destination) const; - /** - * Sets the Random Number Generator to use in resample() to generate - * uniformly distributed random numbers. - */ + /** + * Sets the Random Number Generator to use in resample() to generate + * uniformly distributed random numbers. + */ -protected: - // The default random number generator - CRandomNumberGenerator m_RNG; + protected: + // The default random number generator + CRandomNumberGenerator m_RNG; -private: - bool reset_weights_ = true; - double particle_reset_weight_ = 0.01; + private: + bool reset_weights_ = true; + double particle_reset_weight_ = 0.01; }; - template -ImportanceResampling::ImportanceResampling(bool reset_weights, double particle_reset_weight) : reset_weights_(reset_weights), particle_reset_weight_(particle_reset_weight) {} +ImportanceResampling::ImportanceResampling(bool reset_weights, double particle_reset_weight) + : reset_weights_(reset_weights), particle_reset_weight_(particle_reset_weight) {} template ImportanceResampling::~ImportanceResampling() {} - // resampling based on the cumulative distribution function (CDF) template void ImportanceResampling::resample(const ParticleList& sourceList, - const ParticleList& destinationList) const { - double inverseNum = 1.0f / sourceList.size(); - double start = m_RNG.getUniform() * inverseNum; // random start in CDF - double cumulativeWeight = 0.0f; - unsigned int sourceIndex = 0; // index to draw from - cumulativeWeight += sourceList[sourceIndex]->getWeight(); - for (unsigned int destIndex = 0; destIndex < destinationList.size(); - destIndex++) { - double probSum = - start + - inverseNum * destIndex; // amount of cumulative weight to reach - while (probSum > cumulativeWeight) { // sum weights until - sourceIndex++; - if (sourceIndex >= sourceList.size()) { - sourceIndex = sourceList.size() - 1; - break; - } - cumulativeWeight += - sourceList[sourceIndex]->getWeight(); // target sum reached - } - *(destinationList[destIndex]) = - *(sourceList[sourceIndex]); // copy particle (via assignment - // operator) - if (reset_weights_) { - destinationList[destIndex]->setWeight(particle_reset_weight_); - } + const ParticleList& destinationList) const { + double inverseNum = 1.0f / sourceList.size(); + double start = m_RNG.getUniform() * inverseNum; // random start in CDF + double cumulativeWeight = 0.0f; + unsigned int sourceIndex = 0; // index to draw from + cumulativeWeight += sourceList[sourceIndex]->getWeight(); + for (unsigned int destIndex = 0; destIndex < destinationList.size(); destIndex++) { + double probSum = start + inverseNum * destIndex; // amount of cumulative weight to reach + while (probSum > cumulativeWeight) { // sum weights until + sourceIndex++; + if (sourceIndex >= sourceList.size()) { + sourceIndex = sourceList.size() - 1; + break; + } + cumulativeWeight += sourceList[sourceIndex]->getWeight(); // target sum reached + } + *(destinationList[destIndex]) = *(sourceList[sourceIndex]); // copy particle (via assignment + // operator) + if (reset_weights_) { + destinationList[destIndex]->setWeight(particle_reset_weight_); } + } } } // namespace particle_filter #endif // IMPORTANCERESAMPLING_H diff --git a/include/particle_filter/MovementModel.h b/include/particle_filter/MovementModel.h index ccdd698..7d48eb3 100644 --- a/include/particle_filter/MovementModel.h +++ b/include/particle_filter/MovementModel.h @@ -30,34 +30,33 @@ namespace particle_filter { template class MovementModel { -public: - /** - * The destructor is empty. - */ - virtual ~MovementModel(); + public: + /** + * The destructor is empty. + */ + virtual ~MovementModel(); - /** - * This is the main method of MovementModel. It takes a state reference as - * argument and is supposed to extract the state's variables and manipulate - * them. dt means delta t and defines the time in seconds that has passed - * since the last filter update. - * Define this function in your sub-class! - * @param state Reference to the state that has to be manipulated. - * @param linear Linear movement of the robot during the last filter step - * @param angular Angular movement of the robot during the last filter step - */ - virtual void drift(StateType& state, - geometry_msgs::msg::Vector3 linear, - geometry_msgs::msg::Vector3 angular) const = 0; + /** + * This is the main method of MovementModel. It takes a state reference as + * argument and is supposed to extract the state's variables and manipulate + * them. dt means delta t and defines the time in seconds that has passed + * since the last filter update. + * Define this function in your sub-class! + * @param state Reference to the state that has to be manipulated. + * @param linear Linear movement of the robot during the last filter step + * @param angular Angular movement of the robot during the last filter step + */ + virtual void drift(StateType& state, geometry_msgs::msg::Vector3 linear, + geometry_msgs::msg::Vector3 angular) const = 0; - /** - * This method will be applied in a ParticleFilter after drift(). It can be - * used to add a small jitter to the state. - * @param state Reference to the state that has to be manipulated. - */ - virtual void diffuse(StateType& state) const = 0; + /** + * This method will be applied in a ParticleFilter after drift(). It can be + * used to add a small jitter to the state. + * @param state Reference to the state that has to be manipulated. + */ + virtual void diffuse(StateType& state) const = 0; -private: + private: }; template diff --git a/include/particle_filter/ObservationModel.h b/include/particle_filter/ObservationModel.h index 970c159..f89045b 100644 --- a/include/particle_filter/ObservationModel.h +++ b/include/particle_filter/ObservationModel.h @@ -28,29 +28,29 @@ namespace particle_filter { template class ObservationModel { -public: - /** - * The destructor is empty. - */ - virtual ~ObservationModel(); - - /** - * This is the main method of ObservationModel. It takes a state reference - * as argument and is supposed to extract the state's variables to compute - * an importance weight for the state. - * Define this method in your sub-class! - * @param state Reference to the state that has to be weightened. - * @return importance weight for the given state (positive, non-zero value). - */ - virtual double measure(const StateType& state) const = 0; - - virtual bool measurements_available() = 0; - // virtual void clear_measurement(); - virtual double get_min_weight() const = 0; - - bool accumulate_weights_ = false; - -private: + public: + /** + * The destructor is empty. + */ + virtual ~ObservationModel(); + + /** + * This is the main method of ObservationModel. It takes a state reference + * as argument and is supposed to extract the state's variables to compute + * an importance weight for the state. + * Define this method in your sub-class! + * @param state Reference to the state that has to be weightened. + * @return importance weight for the given state (positive, non-zero value). + */ + virtual double measure(const StateType& state) const = 0; + + virtual bool measurements_available() = 0; + // virtual void clear_measurement(); + virtual double get_min_weight() const = 0; + + bool accumulate_weights_ = false; + + private: }; template diff --git a/include/particle_filter/Particle.h b/include/particle_filter/Particle.h index b552a78..6f2fae6 100644 --- a/include/particle_filter/Particle.h +++ b/include/particle_filter/Particle.h @@ -20,109 +20,107 @@ namespace particle_filter { */ template class Particle { -public: - /** - * This constructor assigns the given state to the member m_State - * and the given weight to the member m_Weight. - * @param state The initial state of the particle - * @param weight The initial weight of the particle - */ - Particle(const StateType& state, double weight); - - /** - * The destructor is empty. - */ - virtual ~Particle(); - - /** - * @return reference to the state of the particle - */ - inline const StateType& getState() const; - - /** - * Sets a new state (assignment operator is used, be sure it works - * for StateType!). - * @param newState a new state for the particle. - */ - inline void setState(const StateType& newState); - - /** - * @return the normalized weight - */ - inline double getWeight() const; - - /** - * @return the raw weight - */ - inline double getWeightUnnormalized() const; - - /** - * Sets a new weight - * @param newWeight the new weight - */ - inline void setWeight(double newWeight); - - /** - * Sets the normalization factor - * @param newWeight the new weight - */ - inline void setNormalization(double normalization); - - bool is_explorer_; - -private: - // make ParticleFilter a friend that can have non-const access - // to m_State - template - friend class ParticleFilter; - - // Stores the state of the particle. - StateType m_State; - - // Stores the importance factor (=weight) of the particle. - double m_Weight; - - // Stores the normalization - double m_normalization = 1; + public: + /** + * This constructor assigns the given state to the member m_State + * and the given weight to the member m_Weight. + * @param state The initial state of the particle + * @param weight The initial weight of the particle + */ + Particle(const StateType& state, double weight); + + /** + * The destructor is empty. + */ + virtual ~Particle(); + + /** + * @return reference to the state of the particle + */ + inline const StateType& getState() const; + + /** + * Sets a new state (assignment operator is used, be sure it works + * for StateType!). + * @param newState a new state for the particle. + */ + inline void setState(const StateType& newState); + + /** + * @return the normalized weight + */ + inline double getWeight() const; + + /** + * @return the raw weight + */ + inline double getWeightUnnormalized() const; + + /** + * Sets a new weight + * @param newWeight the new weight + */ + inline void setWeight(double newWeight); + + /** + * Sets the normalization factor + * @param newWeight the new weight + */ + inline void setNormalization(double normalization); + + bool is_explorer_; + + private: + // make ParticleFilter a friend that can have non-const access + // to m_State + template + friend class ParticleFilter; + + // Stores the state of the particle. + StateType m_State; + + // Stores the importance factor (=weight) of the particle. + double m_Weight; + + // Stores the normalization + double m_normalization = 1; }; template -Particle::Particle(const StateType& state, double weight) : - is_explorer_(false), - m_State(state), - m_Weight(weight) {} +Particle::Particle(const StateType& state, double weight) + : is_explorer_(false), m_State(state), m_Weight(weight) {} template Particle::~Particle() {} template const StateType& Particle::getState() const { - return m_State; + return m_State; } template void Particle::setState(const StateType& newState) { - m_State = newState; + m_State = newState; } template double Particle::getWeight() const { - return m_Weight * m_normalization; + return m_Weight * m_normalization; } template double Particle::getWeightUnnormalized() const { - return m_Weight; + return m_Weight; } template void Particle::setWeight(double newWeight) { - m_Weight = newWeight; + m_Weight = newWeight; } template void Particle::setNormalization(double normalization) { - m_normalization = normalization; + m_normalization = normalization; } } // namespace particle_filter diff --git a/include/particle_filter/ParticleFilter.h b/include/particle_filter/ParticleFilter.h index 732f8e8..35ae553 100644 --- a/include/particle_filter/ParticleFilter.h +++ b/include/particle_filter/ParticleFilter.h @@ -2,19 +2,6 @@ #define PARTICLEFILTER_H #include - -#include -#include // for time measurement -#include -#include -#include -#include - - -#include - -#include - #include #include #include @@ -22,12 +9,18 @@ #include #include #include - #include - +#include +#include +#include // for time measurement #include +#include +#include +#include +#include #include +#include #include #include @@ -158,369 +151,354 @@ namespace particle_filter { * Resampling modes. */ enum ResamplingMode { - /// never resample, - RESAMPLE_NEVER, - /// always resample - RESAMPLE_ALWAYS, - /// only resample if Neff < numParticles / 2 - RESAMPLE_NEFF + /// never resample, + RESAMPLE_NEVER, + /// always resample + RESAMPLE_ALWAYS, + /// only resample if Neff < numParticles / 2 + RESAMPLE_NEFF }; template class ParticleFilter { -public: - /** - * A ParticleList is an array of pointers to Particles. - */ - typedef std::vector*> ParticleList; - - /** - * Typedef for an iterator over particles - */ - typedef typename ParticleList::iterator ParticleIterator; - - /** - * Typedef for a const iterator over particles - */ - typedef typename ParticleList::const_iterator ConstParticleIterator; - - /** - * The constructor allocates the memory for the particle lists and saves - * pointers to ObservationModel and MovementModel in member - * variables. Be sure that these objects are valid through the lifetime of - * ParticleFilter! The default constructor of StateType will be used to - * create the initial particles. The particle lists will have @a - * numParticles elements of type StateType. - * @param numParticles Number of particles for the filter. Has to be greater - * than zero. - * @param os ObservationModel to use for weighting particles - * @param ms MovementModel to use for propagation of particles - */ - ParticleFilter(unsigned int numParticles, - std::shared_ptr> os, - std::shared_ptr> ms); - - /** - * The destructor releases the particle lists. - */ - virtual ~ParticleFilter(); - - /** - * @return Number of particles used in this filter - */ - unsigned int numParticles() const; - - /** - * @param os new observation model - */ - void setObservationModel(std::shared_ptr> os); - - /** - * @return the observation model the particle filter currently uses - */ - std::shared_ptr> getObservationModel() const; - - /** - * @param ms new movement model - */ - void setMovementModel(std::shared_ptr> ms); - - /** - * @return the movement model the particle filter currently uses - */ - std::shared_ptr> getMovementModel() const; - - /** - * @param rs new resampling strategy - */ - void - setResamplingStrategy(std::shared_ptr> rs); - - /** - * @return the resampling strategy the particle filter currently uses - */ - std::shared_ptr> - getResamplingStrategy() const; - - /** - * Changes the resampling mode - * @param mode new resampling mode. - */ - void setResamplingMode(ResamplingMode mode); - - /** - * @return the currently set resampling mode - */ - ResamplingMode getResamplingMode() const; - - /** - * Computes and returns the number of effective particles. - * @return The estimated number of effective particles according to the - * formula: \f[ N_{eff} = \frac{1}{\sum_{i=1}^{N_s} (w_k^i)^2} \f] - */ - unsigned int getNumEffectiveParticles() const; - - /** - * Sets all particle states to the given state. Useful for integrating a - * known prior state to begin with tracking. - * @param priorState State that will be copied to all particles. - */ - void setPriorState(const StateType& priorState); - - /** - * Draws all particle states from the given distribution. - * @param distribution The state distribution to draw the states from. - */ - void drawAllFromDistribution( - const std::shared_ptr>& distribution); - - /** - * Resets the filter timer. Call this function after pausing the filter - * to avoid a drift step with a high delta t. - */ - void resetTimer(); - - /** - * @return Pointer to the particle that has the highest weight. - */ - const Particle* getBestParticle() const; - - /** - * @return State that is carried by the particle with highest weight. - */ - const StateType& getBestState() const; - - /** - * @param i Particle index - * @return weight of Particle i. - */ - double getWeight(unsigned int i) const; - - - /** - * Returns the weight of the highest rated particle. - * @return weight of the highest rated particle. - */ - double getMaxParticleWeight() const; - - /** - * Performs an entire filter procedure. - * filter() also saves the last filter time to be able to compute the - * appropriate dt for the drift step. To reset the time manually, call - * resetTimer(). - * The functions resample(), - * drift(), diffuse() and measure() are called. - */ - void filter(); - - /** - * Returns a pointer to a particle with a given index. - * @param particleNo Index of requested particle - * @return Pointer to particle with index particleNo - */ - const Particle* getParticle(unsigned int particleNo) const; - - /** - * Returns a const reference to the state of particle with given index. - * @param particleNo Index of particle - * @return Pointer to the state of particle at index particleNo. - */ - const StateType& getState(unsigned int particleNo) const; - - /** - * Returns the "mean" state, i.e. the sum of the weighted states. You can - * use this only if you implemented operator*(double) and - * operator+=(MyState) in your derived State MyState. - * @return "mean" state. Best estimation. - */ - StateType getMmseEstimate() const; - - /** - * Same as getMmseEstimate(), but uses only the best x% of the particles. - * @param x percentage of particles to use. Has to be positive and greater - * than zero. - * @return "mean" state of the best x% particles. If x <= 0, the state - * with the highest weight is returned. - */ - StateType getBestXPercentEstimate(float x) const; - - /** - * This method selects a new set of particles out of an old set according to - * their weight (importance resampling). The particles from the list - * m_CurrentList points to are used as source, m_LastList points to the - * destination list. The pointers m_CurrentList and m_LastList are switched. - * The higher the weight of a particle, the more particles are drawn - * (copied) from this particle. The weight remains untouched, because - * measure() will be called afterwards. This method only works on a sorted - * m_CurrentList, therefore sort() is called first. - */ - void resample(); - - /** - * This method drifts the particles (second step of a filter process) using - * the movement model of the particle filter. dt defines the time interval - * that has to be used in drifting (in seconds). - * - * @param linear Linear movement of the robot during the last filter step - * @param angular Angular movement of the robot during the last filter step - */ - void drift(geometry_msgs::msg::Vector3 linear, geometry_msgs::msg::Vector3 angular); - - /** - * This method "diffuses" the particles using the movement model of the - * particle filter to add a small jitter to the particle states. - */ - void diffuse(); - - /** - * This method assigns weights to the particles using the observation model - * of the particle filter. - */ - virtual void measure(); - - /** - * Returns an iterator to the particle list's beginning. - * - * @returns Iterator to the particle list's beginning - */ - ConstParticleIterator particleListBegin(); - - /** - * Returns an iterator to the end of the particle list. - * - * @returns Iterator to the end of the particle list - */ - ConstParticleIterator particleListEnd(); - - /** - * Passes the arguments on to the renderPointsMarker method of the StateType. - * - * - * @param n_space Namespace of the rendered marker - * @param frame Frame in which the rendered marker is published - * @param lifetime Lifetime of the rendered marker - * @param color Color of the rendered marker - * @return A Marker message containing a point representation of each particle - */ - visualization_msgs::msg::Marker renderPointsMarker(std::string n_space, - std::string frame, - rclcpp::Duration lifetime, - std_msgs::msg::ColorRGBA color); - - - /** - * Passes the arguments on to the renderMarker method of the StateType and - * collects all of them in a MarkerArray message. - * - * - * @param n_space Namespace of the rendered markers - * @param frame Frame in which the rendered markers are published - * @param lifetime Lifetime of the rendered markers - * @param color Color of the rendered markers - * @return A MarkerArray message containing representations of each - * particle. The specific Marker type depends on the StateType - */ - visualization_msgs::msg::MarkerArray renderMarkerArray(std::string n_space, - std::string frame, - rclcpp::Duration lifetime, - std_msgs::msg::ColorRGBA color, - rclcpp::Time stamp); - - /** - * Computes a GMM representation of the current state. - * The GMM will have a predefined number of components. - * - * @param num_components number of components of the GMM - * @param delta minimal improvement of the log-likelihood in an iteration - * of the EM-Algorithm to continue - * @param num_iterations maximal number of iterations of the EM-Algorithm - * computing the GMM - * @param ignore_explorers ignore explorer particles (you should always do - * this!) - * @return - */ - gmms::GaussianMixtureModel getGMM(int num_components, - const double delta = 0.01, - const int num_iterations = 100, - const bool ignore_explorers = true); - - /** - * Computes a GMM representation of the current state. - * The GMM will have a number of components between min_num_components and - * max_num_components. - * - * @param min_num_components minimal number of components of the GMM - * @param max_num_components maximal number of components of the GMM - * @param component_delta minimal improvement of the log-likelihood between - * two GMMS to decide to go on with the one with more components - * @param iteration_delta minimal improvement of the log-likelihood in an - * iteration of the EM-Algorithm to continue - * @param num_iterations maximal number of iterations of the EM-Algorithm - * computing the GMM - * @param ignore_explorers ignore explorer particles (you should always do - * this!) - * @return - */ - gmms::GaussianMixtureModel getDynGMM(int min_num_components, - int max_num_components, - const double component_delta, - const double iteration_delta = 0.01, - const int num_iterations = 100, - const bool ignore_explorers = true); - - std::vector> getCovarianceMatrix(const bool ignore_explorers=true); - - std::vector getCovariance(float percentage) const; - -protected: - /** - * This method sorts the particles according to their weight. STL's - * std::sort() is used together with the custom compare function - * CompareParticleWeights(). The particle with the highest weight is at - * position 0 after calling this function. - */ - void sort(); - - /** - * This method normalizes the weights of the particles. After calling this - * function, the sum of the weights of all particles in the current particle - * list equals 1.0. In this function the sum of all weights of the particles - * of the current particle list is computed and each weight of each particle - * is devided through this sum. - */ - void normalize(); - -private: - // Particle lists. - // The particles are drawn from m_LastList to m_CurrentList to avoid new and - // delete commands. In each run, the pointers m_CurrentList and m_LastList - // are switched in resample(). - ParticleList m_CurrentList; - ParticleList m_LastList; - - // Stores the number of particles. - unsigned int m_NumParticles; - - // Holds a pointer to the observation strategy (used for weighting) - std::shared_ptr> m_ObservationModel; - - // Holds a pointer to the movement strategy (used for moving and diffusing) - std::shared_ptr> m_MovementModel; - - // Stores a pointer to the resampling strategy. - std::shared_ptr> m_ResamplingStrategy; - - // The default resampling strategy. - - // Stores the last filter time to have the right dt value for drift. - clock_t m_LastDriftTime; - - // Flag that stores if the filter has run once or not) - bool m_FirstRun; - - // Stores which resampling mode is set, default is - // ResamplingMode::RESAMPLE_NEFF - ResamplingMode m_ResamplingMode; + public: + /** + * A ParticleList is an array of pointers to Particles. + */ + typedef std::vector*> ParticleList; + + /** + * Typedef for an iterator over particles + */ + typedef typename ParticleList::iterator ParticleIterator; + + /** + * Typedef for a const iterator over particles + */ + typedef typename ParticleList::const_iterator ConstParticleIterator; + + /** + * The constructor allocates the memory for the particle lists and saves + * pointers to ObservationModel and MovementModel in member + * variables. Be sure that these objects are valid through the lifetime of + * ParticleFilter! The default constructor of StateType will be used to + * create the initial particles. The particle lists will have @a + * numParticles elements of type StateType. + * @param numParticles Number of particles for the filter. Has to be greater + * than zero. + * @param os ObservationModel to use for weighting particles + * @param ms MovementModel to use for propagation of particles + */ + ParticleFilter(unsigned int numParticles, std::shared_ptr> os, + std::shared_ptr> ms); + + /** + * The destructor releases the particle lists. + */ + virtual ~ParticleFilter(); + + /** + * @return Number of particles used in this filter + */ + unsigned int numParticles() const; + + /** + * @param os new observation model + */ + void setObservationModel(std::shared_ptr> os); + + /** + * @return the observation model the particle filter currently uses + */ + std::shared_ptr> getObservationModel() const; + + /** + * @param ms new movement model + */ + void setMovementModel(std::shared_ptr> ms); + + /** + * @return the movement model the particle filter currently uses + */ + std::shared_ptr> getMovementModel() const; + + /** + * @param rs new resampling strategy + */ + void setResamplingStrategy(std::shared_ptr> rs); + + /** + * @return the resampling strategy the particle filter currently uses + */ + std::shared_ptr> getResamplingStrategy() const; + + /** + * Changes the resampling mode + * @param mode new resampling mode. + */ + void setResamplingMode(ResamplingMode mode); + + /** + * @return the currently set resampling mode + */ + ResamplingMode getResamplingMode() const; + + /** + * Computes and returns the number of effective particles. + * @return The estimated number of effective particles according to the + * formula: \f[ N_{eff} = \frac{1}{\sum_{i=1}^{N_s} (w_k^i)^2} \f] + */ + unsigned int getNumEffectiveParticles() const; + + /** + * Sets all particle states to the given state. Useful for integrating a + * known prior state to begin with tracking. + * @param priorState State that will be copied to all particles. + */ + void setPriorState(const StateType& priorState); + + /** + * Draws all particle states from the given distribution. + * @param distribution The state distribution to draw the states from. + */ + void drawAllFromDistribution(const std::shared_ptr>& distribution); + + /** + * Resets the filter timer. Call this function after pausing the filter + * to avoid a drift step with a high delta t. + */ + void resetTimer(); + + /** + * @return Pointer to the particle that has the highest weight. + */ + const Particle* getBestParticle() const; + + /** + * @return State that is carried by the particle with highest weight. + */ + const StateType& getBestState() const; + + /** + * @param i Particle index + * @return weight of Particle i. + */ + double getWeight(unsigned int i) const; + + /** + * Returns the weight of the highest rated particle. + * @return weight of the highest rated particle. + */ + double getMaxParticleWeight() const; + + /** + * Performs an entire filter procedure. + * filter() also saves the last filter time to be able to compute the + * appropriate dt for the drift step. To reset the time manually, call + * resetTimer(). + * The functions resample(), + * drift(), diffuse() and measure() are called. + */ + void filter(); + + /** + * Returns a pointer to a particle with a given index. + * @param particleNo Index of requested particle + * @return Pointer to particle with index particleNo + */ + const Particle* getParticle(unsigned int particleNo) const; + + /** + * Returns a const reference to the state of particle with given index. + * @param particleNo Index of particle + * @return Pointer to the state of particle at index particleNo. + */ + const StateType& getState(unsigned int particleNo) const; + + /** + * Returns the "mean" state, i.e. the sum of the weighted states. You can + * use this only if you implemented operator*(double) and + * operator+=(MyState) in your derived State MyState. + * @return "mean" state. Best estimation. + */ + StateType getMmseEstimate() const; + + /** + * Same as getMmseEstimate(), but uses only the best x% of the particles. + * @param x percentage of particles to use. Has to be positive and greater + * than zero. + * @return "mean" state of the best x% particles. If x <= 0, the state + * with the highest weight is returned. + */ + StateType getBestXPercentEstimate(float x) const; + + /** + * This method selects a new set of particles out of an old set according to + * their weight (importance resampling). The particles from the list + * m_CurrentList points to are used as source, m_LastList points to the + * destination list. The pointers m_CurrentList and m_LastList are switched. + * The higher the weight of a particle, the more particles are drawn + * (copied) from this particle. The weight remains untouched, because + * measure() will be called afterwards. This method only works on a sorted + * m_CurrentList, therefore sort() is called first. + */ + void resample(); + + /** + * This method drifts the particles (second step of a filter process) using + * the movement model of the particle filter. dt defines the time interval + * that has to be used in drifting (in seconds). + * + * @param linear Linear movement of the robot during the last filter step + * @param angular Angular movement of the robot during the last filter step + */ + void drift(geometry_msgs::msg::Vector3 linear, geometry_msgs::msg::Vector3 angular); + + /** + * This method "diffuses" the particles using the movement model of the + * particle filter to add a small jitter to the particle states. + */ + void diffuse(); + + /** + * This method assigns weights to the particles using the observation model + * of the particle filter. + */ + virtual void measure(); + + /** + * Returns an iterator to the particle list's beginning. + * + * @returns Iterator to the particle list's beginning + */ + ConstParticleIterator particleListBegin(); + + /** + * Returns an iterator to the end of the particle list. + * + * @returns Iterator to the end of the particle list + */ + ConstParticleIterator particleListEnd(); + + /** + * Passes the arguments on to the renderPointsMarker method of the StateType. + * + * + * @param n_space Namespace of the rendered marker + * @param frame Frame in which the rendered marker is published + * @param lifetime Lifetime of the rendered marker + * @param color Color of the rendered marker + * @return A Marker message containing a point representation of each particle + */ + visualization_msgs::msg::Marker renderPointsMarker(std::string n_space, std::string frame, rclcpp::Duration lifetime, + std_msgs::msg::ColorRGBA color); + + /** + * Passes the arguments on to the renderMarker method of the StateType and + * collects all of them in a MarkerArray message. + * + * + * @param n_space Namespace of the rendered markers + * @param frame Frame in which the rendered markers are published + * @param lifetime Lifetime of the rendered markers + * @param color Color of the rendered markers + * @return A MarkerArray message containing representations of each + * particle. The specific Marker type depends on the StateType + */ + visualization_msgs::msg::MarkerArray renderMarkerArray(std::string n_space, std::string frame, + rclcpp::Duration lifetime, std_msgs::msg::ColorRGBA color, + rclcpp::Time stamp); + + /** + * Computes a GMM representation of the current state. + * The GMM will have a predefined number of components. + * + * @param num_components number of components of the GMM + * @param delta minimal improvement of the log-likelihood in an iteration + * of the EM-Algorithm to continue + * @param num_iterations maximal number of iterations of the EM-Algorithm + * computing the GMM + * @param ignore_explorers ignore explorer particles (you should always do + * this!) + * @return + */ + gmms::GaussianMixtureModel getGMM(int num_components, const double delta = 0.01, const int num_iterations = 100, + const bool ignore_explorers = true); + + /** + * Computes a GMM representation of the current state. + * The GMM will have a number of components between min_num_components and + * max_num_components. + * + * @param min_num_components minimal number of components of the GMM + * @param max_num_components maximal number of components of the GMM + * @param component_delta minimal improvement of the log-likelihood between + * two GMMS to decide to go on with the one with more components + * @param iteration_delta minimal improvement of the log-likelihood in an + * iteration of the EM-Algorithm to continue + * @param num_iterations maximal number of iterations of the EM-Algorithm + * computing the GMM + * @param ignore_explorers ignore explorer particles (you should always do + * this!) + * @return + */ + gmms::GaussianMixtureModel getDynGMM(int min_num_components, int max_num_components, const double component_delta, + const double iteration_delta = 0.01, const int num_iterations = 100, + const bool ignore_explorers = true); + + std::vector> getCovarianceMatrix(const bool ignore_explorers = true); + + std::vector getCovariance(float percentage) const; + + protected: + /** + * This method sorts the particles according to their weight. STL's + * std::sort() is used together with the custom compare function + * CompareParticleWeights(). The particle with the highest weight is at + * position 0 after calling this function. + */ + void sort(); + + /** + * This method normalizes the weights of the particles. After calling this + * function, the sum of the weights of all particles in the current particle + * list equals 1.0. In this function the sum of all weights of the particles + * of the current particle list is computed and each weight of each particle + * is devided through this sum. + */ + void normalize(); + + private: + // Particle lists. + // The particles are drawn from m_LastList to m_CurrentList to avoid new and + // delete commands. In each run, the pointers m_CurrentList and m_LastList + // are switched in resample(). + ParticleList m_CurrentList; + ParticleList m_LastList; + + // Stores the number of particles. + unsigned int m_NumParticles; + + // Holds a pointer to the observation strategy (used for weighting) + std::shared_ptr> m_ObservationModel; + + // Holds a pointer to the movement strategy (used for moving and diffusing) + std::shared_ptr> m_MovementModel; + + // Stores a pointer to the resampling strategy. + std::shared_ptr> m_ResamplingStrategy; + + // The default resampling strategy. + + // Stores the last filter time to have the right dt value for drift. + clock_t m_LastDriftTime; + + // Flag that stores if the filter has run once or not) + bool m_FirstRun; + + // Stores which resampling mode is set, default is + // ResamplingMode::RESAMPLE_NEFF + ResamplingMode m_ResamplingMode; }; } // namespace particle_filter diff --git a/include/particle_filter/ParticleFilter.hxx b/include/particle_filter/ParticleFilter.hxx index f19b6a8..2728903 100644 --- a/include/particle_filter/ParticleFilter.hxx +++ b/include/particle_filter/ParticleFilter.hxx @@ -1,494 +1,446 @@ namespace particle_filter { - template -ParticleFilter::ParticleFilter(unsigned int numParticles, - std::shared_ptr> os, - std::shared_ptr> ms) : - m_NumParticles(numParticles), - m_ObservationModel(os), - m_MovementModel(ms), - m_FirstRun(true), - m_ResamplingMode(RESAMPLE_NEFF) { - m_ResamplingStrategy.reset(new ImportanceResampling()); +ParticleFilter::ParticleFilter(unsigned int numParticles, std::shared_ptr> os, + std::shared_ptr> ms) + : m_NumParticles(numParticles), + m_ObservationModel(os), + m_MovementModel(ms), + m_FirstRun(true), + m_ResamplingMode(RESAMPLE_NEFF) { + m_ResamplingStrategy.reset(new ImportanceResampling()); - assert(numParticles > 0); + assert(numParticles > 0); - // allocate memory for particle lists - m_CurrentList.resize(numParticles); - m_LastList.resize(numParticles); + // allocate memory for particle lists + m_CurrentList.resize(numParticles); + m_LastList.resize(numParticles); - double initialWeight = 1.0 / numParticles; - // fill particle lists - for (unsigned int i = 0; i < numParticles; i++) { - m_CurrentList[i] = new Particle(StateType(), initialWeight); - m_LastList[i] = new Particle(StateType(), initialWeight); - } + double initialWeight = 1.0 / numParticles; + // fill particle lists + for (unsigned int i = 0; i < numParticles; i++) { + m_CurrentList[i] = new Particle(StateType(), initialWeight); + m_LastList[i] = new Particle(StateType(), initialWeight); + } } - template ParticleFilter::~ParticleFilter() { - // release particles - ConstParticleIterator iter; - for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { - delete *iter; - } - for (iter = m_LastList.begin(); iter != m_LastList.end(); ++iter) { - delete *iter; - } + // release particles + ConstParticleIterator iter; + for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { + delete *iter; + } + for (iter = m_LastList.begin(); iter != m_LastList.end(); ++iter) { + delete *iter; + } } - template unsigned int ParticleFilter::numParticles() const { - return m_NumParticles; + return m_NumParticles; } template -void ParticleFilter::setObservationModel( - std::shared_ptr> os) { - m_ObservationModel = os; +void ParticleFilter::setObservationModel(std::shared_ptr> os) { + m_ObservationModel = os; } template -std::shared_ptr> -ParticleFilter::getObservationModel() const { - return m_ObservationModel; +std::shared_ptr> ParticleFilter::getObservationModel() const { + return m_ObservationModel; } template -void ParticleFilter::setMovementModel( - std::shared_ptr> ms) { - m_MovementModel = ms; +void ParticleFilter::setMovementModel(std::shared_ptr> ms) { + m_MovementModel = ms; } template -std::shared_ptr> -ParticleFilter::getMovementModel() const { - return m_MovementModel; +std::shared_ptr> ParticleFilter::getMovementModel() const { + return m_MovementModel; } template -void ParticleFilter::setResamplingStrategy( - std::shared_ptr> rs) { - m_ResamplingStrategy = rs; +void ParticleFilter::setResamplingStrategy(std::shared_ptr> rs) { + m_ResamplingStrategy = rs; } template -std::shared_ptr> -ParticleFilter::getResamplingStrategy() const { - return m_ResamplingStrategy; +std::shared_ptr> ParticleFilter::getResamplingStrategy() const { + return m_ResamplingStrategy; } template void ParticleFilter::setResamplingMode(ResamplingMode mode) { - m_ResamplingMode = mode; + m_ResamplingMode = mode; } template ResamplingMode ParticleFilter::getResamplingMode() const { - return m_ResamplingMode; + return m_ResamplingMode; } template void ParticleFilter::setPriorState(const StateType& priorState) { - ConstParticleIterator iter; - for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { - (*iter)->setState(priorState); - } + ConstParticleIterator iter; + for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { + (*iter)->setState(priorState); + } } template void ParticleFilter::drawAllFromDistribution( - const std::shared_ptr>& distribution) { - ConstParticleIterator iter; - for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { - (*iter)->setState(distribution->draw()); - } + const std::shared_ptr>& distribution) { + ConstParticleIterator iter; + for (iter = m_CurrentList.begin(); iter != m_CurrentList.end(); ++iter) { + (*iter)->setState(distribution->draw()); + } } template void ParticleFilter::resetTimer() { - m_FirstRun = true; + m_FirstRun = true; } template void ParticleFilter::filter() { - if (m_ResamplingMode == RESAMPLE_NEFF) { - if (getNumEffectiveParticles() < m_NumParticles / 2) { - resample(); - } - } else if (m_ResamplingMode == RESAMPLE_ALWAYS) { - resample(); - } // else do not resample + if (m_ResamplingMode == RESAMPLE_NEFF) { + if (getNumEffectiveParticles() < m_NumParticles / 2) { + resample(); + } + } else if (m_ResamplingMode == RESAMPLE_ALWAYS) { + resample(); + } // else do not resample - drift(); - diffuse(); - measure(); + drift(); + diffuse(); + measure(); } template -const Particle* -ParticleFilter::getParticle(unsigned int particleNo) const { - assert(particleNo < m_NumParticles); - return m_CurrentList[particleNo]; +const Particle* ParticleFilter::getParticle(unsigned int particleNo) const { + assert(particleNo < m_NumParticles); + return m_CurrentList[particleNo]; } template -const StateType& -ParticleFilter::getState(unsigned int particleNo) const { - assert(particleNo < m_NumParticles); - return m_CurrentList[particleNo]->getState(); +const StateType& ParticleFilter::getState(unsigned int particleNo) const { + assert(particleNo < m_NumParticles); + return m_CurrentList[particleNo]->getState(); } template double ParticleFilter::getWeight(unsigned int particleNo) const { - assert(particleNo < m_NumParticles); - return m_CurrentList[particleNo]->getWeight(); + assert(particleNo < m_NumParticles); + return m_CurrentList[particleNo]->getWeight(); } template void ParticleFilter::sort() { - std::sort(m_CurrentList.begin(), m_CurrentList.end(), - CompareParticleWeights()); + std::sort(m_CurrentList.begin(), m_CurrentList.end(), CompareParticleWeights()); } - template void ParticleFilter::resample() { - // swap lists - m_CurrentList.swap(m_LastList); - // call resampling strategy - m_ResamplingStrategy->resample(m_LastList, m_CurrentList); + // swap lists + m_CurrentList.swap(m_LastList); + // call resampling strategy + m_ResamplingStrategy->resample(m_LastList, m_CurrentList); } - template -void ParticleFilter::drift(geometry_msgs::msg::Vector3 linear, - geometry_msgs::msg::Vector3 angular) { - for (unsigned int i = 0; i < m_NumParticles; i++) { - m_MovementModel->drift(m_CurrentList[i]->m_State, linear, angular); - } +void ParticleFilter::drift(geometry_msgs::msg::Vector3 linear, geometry_msgs::msg::Vector3 angular) { + for (unsigned int i = 0; i < m_NumParticles; i++) { + m_MovementModel->drift(m_CurrentList[i]->m_State, linear, angular); + } } template void ParticleFilter::diffuse() { -//#pragma omp parallel for - for (unsigned int i = 0; i < m_NumParticles; i++) { - m_MovementModel->diffuse(m_CurrentList[i]->m_State); - } + // #pragma omp parallel for + for (unsigned int i = 0; i < m_NumParticles; i++) { + m_MovementModel->diffuse(m_CurrentList[i]->m_State); + } } template void ParticleFilter::measure() { - // measure only, if there are measurements available - if (!m_ObservationModel->measurements_available()) { - // return; - // currently, this results in a problem, as the particle weight does - // not decay - } - double weight, weights_sum = 0; -//#pragma omp parallel for - for (size_t i = 0; i < m_NumParticles; i++) { - // apply observation model - - // accumulate the weight when it is defined in the observation model - if (m_ObservationModel->accumulate_weights_) { - weight = m_CurrentList[i]->getWeightUnnormalized(); - } else { - weight = 0; - } - - // set explorer particle weight to minimal value if there are no - // measurements available to reduce noise - if (m_CurrentList[i]->is_explorer_ && - !m_ObservationModel->measurements_available()) { - weight += m_ObservationModel->get_min_weight(); - } else { - weight += m_ObservationModel->measure(m_CurrentList[i]->getState()); - } - m_CurrentList[i]->setWeight(weight); - // Update the weight sum - weights_sum += weight; + // measure only, if there are measurements available + if (!m_ObservationModel->measurements_available()) { + // return; + // currently, this results in a problem, as the particle weight does + // not decay + } + double weight, weights_sum = 0; + // #pragma omp parallel for + for (size_t i = 0; i < m_NumParticles; i++) { + // apply observation model + + // accumulate the weight when it is defined in the observation model + if (m_ObservationModel->accumulate_weights_) { + weight = m_CurrentList[i]->getWeightUnnormalized(); + } else { + weight = 0; } -//#pragma omp parallel for - for (size_t i = 0; i < m_NumParticles; i++) { - m_CurrentList[i]->setNormalization(1.0/weights_sum); + + // set explorer particle weight to minimal value if there are no + // measurements available to reduce noise + if (m_CurrentList[i]->is_explorer_ && !m_ObservationModel->measurements_available()) { + weight += m_ObservationModel->get_min_weight(); + } else { + weight += m_ObservationModel->measure(m_CurrentList[i]->getState()); } - // re-sort the particles - sort(); + m_CurrentList[i]->setWeight(weight); + // Update the weight sum + weights_sum += weight; + } + // #pragma omp parallel for + for (size_t i = 0; i < m_NumParticles; i++) { + m_CurrentList[i]->setNormalization(1.0 / weights_sum); + } + // re-sort the particles + sort(); } template unsigned int ParticleFilter::getNumEffectiveParticles() const { - double squareSum = 0; - for (unsigned int i = 0; i < m_NumParticles; i++) { - double weight = m_CurrentList[i]->getWeight(); - squareSum += weight * weight; - } - return static_cast(1.0f / squareSum); + double squareSum = 0; + for (unsigned int i = 0; i < m_NumParticles; i++) { + double weight = m_CurrentList[i]->getWeight(); + squareSum += weight * weight; + } + return static_cast(1.0f / squareSum); } template const Particle* ParticleFilter::getBestParticle() const { - return m_CurrentList[0]; + return m_CurrentList[0]; } template double ParticleFilter::getMaxParticleWeight() const { - return m_CurrentList[0]->getWeight(); + return m_CurrentList[0]->getWeight(); } template const StateType& ParticleFilter::getBestState() const { - return m_CurrentList[0]->getState(); + return m_CurrentList[0]->getState(); } template StateType ParticleFilter::getMmseEstimate() const { - StateType estimate = - m_CurrentList[0]->getState() * m_CurrentList[0]->getWeight(); - for (unsigned int i = 1; i < m_NumParticles; i++) { - estimate += - m_CurrentList[i]->getState() * m_CurrentList[i]->getWeight(); - } - return estimate; + StateType estimate = m_CurrentList[0]->getState() * m_CurrentList[0]->getWeight(); + for (unsigned int i = 1; i < m_NumParticles; i++) { + estimate += m_CurrentList[i]->getState() * m_CurrentList[i]->getWeight(); + } + return estimate; } template -StateType -ParticleFilter::getBestXPercentEstimate(float percentage) const { - StateType estimate = - m_CurrentList[0]->getState() * m_CurrentList[0]->getWeight(); - double weightSum = m_CurrentList[0]->getWeight(); - unsigned int numToConsider = m_NumParticles / 100.0f * percentage; - for (unsigned int i = 1; i < numToConsider; i++) { - estimate += - m_CurrentList[i]->getState() * m_CurrentList[i]->getWeight(); - weightSum += m_CurrentList[i]->getWeight(); - } - estimate = estimate * (1.0 / weightSum); - return estimate; +StateType ParticleFilter::getBestXPercentEstimate(float percentage) const { + StateType estimate = m_CurrentList[0]->getState() * m_CurrentList[0]->getWeight(); + double weightSum = m_CurrentList[0]->getWeight(); + unsigned int numToConsider = m_NumParticles / 100.0f * percentage; + for (unsigned int i = 1; i < numToConsider; i++) { + estimate += m_CurrentList[i]->getState() * m_CurrentList[i]->getWeight(); + weightSum += m_CurrentList[i]->getWeight(); + } + estimate = estimate * (1.0 / weightSum); + return estimate; } template -typename ParticleFilter::ConstParticleIterator -ParticleFilter::particleListBegin() { - return m_CurrentList.begin(); +typename ParticleFilter::ConstParticleIterator ParticleFilter::particleListBegin() { + return m_CurrentList.begin(); } template -typename ParticleFilter::ConstParticleIterator -ParticleFilter::particleListEnd() { - return m_CurrentList.end(); +typename ParticleFilter::ConstParticleIterator ParticleFilter::particleListEnd() { + return m_CurrentList.end(); } template -visualization_msgs::msg::Marker -ParticleFilter::renderPointsMarker(std::string n_space, - std::string frame, - rclcpp::Duration lifetime, - std_msgs::msg::ColorRGBA color) { - return StateType::renderPointsMarker( - m_CurrentList, n_space, frame, lifetime, color); +visualization_msgs::msg::Marker ParticleFilter::renderPointsMarker(std::string n_space, std::string frame, + rclcpp::Duration lifetime, + std_msgs::msg::ColorRGBA color) { + return StateType::renderPointsMarker(m_CurrentList, n_space, frame, lifetime, color); } template -visualization_msgs::msg::MarkerArray -ParticleFilter::renderMarkerArray(std::string n_space, - std::string frame, - rclcpp::Duration lifetime, - std_msgs::msg::ColorRGBA color, - rclcpp::Time stamp) { - visualization_msgs::msg::MarkerArray marker_array; +visualization_msgs::msg::MarkerArray ParticleFilter::renderMarkerArray(std::string n_space, + std::string frame, + rclcpp::Duration lifetime, + std_msgs::msg::ColorRGBA color, + rclcpp::Time stamp) { + visualization_msgs::msg::MarkerArray marker_array; - for (unsigned int i = 0; i < m_NumParticles; i++) { - double weight = m_CurrentList[i]->getWeight(); - color.r = weight / getMaxParticleWeight(); - marker_array.markers.push_back(m_CurrentList[i]->getState().renderMarker( - n_space, frame, lifetime, color, stamp)); - } + for (unsigned int i = 0; i < m_NumParticles; i++) { + double weight = m_CurrentList[i]->getWeight(); + color.r = weight / getMaxParticleWeight(); + marker_array.markers.push_back(m_CurrentList[i]->getState().renderMarker(n_space, frame, lifetime, color, stamp)); + } - return marker_array; -} - -template -gmms::GaussianMixtureModel ParticleFilter::getGMM(int num_components, - const double delta, - const int num_iterations, - const bool ignore_explorers) { - assert(num_components >= 1); - gmms::GaussianMixtureModel gmm(num_components, delta, num_iterations); - Eigen::MatrixXd dataset; - StateType::convertParticleListToEigen( - m_CurrentList, dataset, ignore_explorers); - gmm.initialize(dataset); - gmm.expectationMaximization(dataset); - return gmm; -} - -template -gmms::GaussianMixtureModel -ParticleFilter::getDynGMM(int min_num_components, - int max_num_components, - const double component_delta, - const double iteration_delta, - const int num_iterations, - const bool ignore_explorers) { - assert(min_num_components < max_num_components); - - // set up dataset - Eigen::MatrixXd dataset; - StateType::convertParticleListToEigen( - m_CurrentList, dataset, ignore_explorers); - - gmms::GaussianMixtureModel last_gmm, last_last_gmm; - int component_count = 0; - int component_number = min_num_components; - double old_log_likelihood; - double log_likelihood = 0.0; - do { - component_number = min_num_components + component_count; - old_log_likelihood = log_likelihood; - last_last_gmm = last_gmm; - last_gmm = gmms::GaussianMixtureModel( - component_number, iteration_delta, num_iterations); - last_gmm.initialize(dataset); - last_gmm.expectationMaximization(dataset); - - log_likelihood = last_gmm.logLikelihood(dataset); - component_count++; - } while (component_number < max_num_components and - std::abs(old_log_likelihood - log_likelihood) > component_delta); - - if (std::abs(old_log_likelihood - log_likelihood) > component_delta or component_count <= 1) { - return last_gmm; - } else { - return last_last_gmm; - } + return marker_array; } template -std::vector> ParticleFilter::getCovarianceMatrix(const bool ignore_explorers) { - // get an Eigen matrix of the particles - Eigen::MatrixXd dataset; - StateType::convertParticleListToEigen( - m_CurrentList, dataset, ignore_explorers); - Eigen::VectorXd component_means = dataset.colwise().mean(); - Eigen::VectorXd component_sums = dataset.colwise().sum(); - int component_num = dataset.cols(); - int particle_num = dataset.rows(); - Eigen::VectorXd variances = ((particle_num * component_means) - component_sums).cwiseAbs(); - std::vector> cov_mat(component_num, std::vector(component_num, 0)); - for (int x = 0; x < component_num; x++) { - for (int y = 0; y < component_num; y++) { - cov_mat[x][y] = variances.coeff(x) * variances.coeff(y) / particle_num; - } - } - return cov_mat; +gmms::GaussianMixtureModel ParticleFilter::getGMM(int num_components, const double delta, + const int num_iterations, const bool ignore_explorers) { + assert(num_components >= 1); + gmms::GaussianMixtureModel gmm(num_components, delta, num_iterations); + Eigen::MatrixXd dataset; + StateType::convertParticleListToEigen(m_CurrentList, dataset, ignore_explorers); + gmm.initialize(dataset); + gmm.expectationMaximization(dataset); + return gmm; } - template - std::vector ParticleFilter::getCovariance(float percentage) const{ +template +gmms::GaussianMixtureModel ParticleFilter::getDynGMM(int min_num_components, int max_num_components, + const double component_delta, + const double iteration_delta, const int num_iterations, + const bool ignore_explorers) { + assert(min_num_components < max_num_components); - double xI = 0; - double yI = 0; - double thetaSinI = 0; - double thetaCosI = 0; + // set up dataset + Eigen::MatrixXd dataset; + StateType::convertParticleListToEigen(m_CurrentList, dataset, ignore_explorers); - unsigned int numToConsider = m_NumParticles * (percentage / 100.0f); - //std::cout << "number particles : " << m_NumParticles; - //std::cout << "percentage: " << percentage; - //std::cout << "nums to consider: " << numToConsider; - unsigned int i = 0; - for (i; i < numToConsider; i++){ - xI += std::round(m_CurrentList[i]->getState().getXPos() * 10000.0) / 10000.0; - yI += std::round(m_CurrentList[i]->getState().getYPos() * 10000.0) / 10000.0; - thetaSinI += std::round(m_CurrentList[i]->getState().getSinTheta() * 10000.0) / 10000.0; - thetaCosI += std::round(m_CurrentList[i]->getState().getCosTheta() * 10000.0) / 10000.0; - } + gmms::GaussianMixtureModel last_gmm, last_last_gmm; + int component_count = 0; + int component_number = min_num_components; + double old_log_likelihood; + double log_likelihood = 0.0; + do { + component_number = min_num_components + component_count; + old_log_likelihood = log_likelihood; + last_last_gmm = last_gmm; + last_gmm = gmms::GaussianMixtureModel(component_number, iteration_delta, num_iterations); + last_gmm.initialize(dataset); + last_gmm.expectationMaximization(dataset); - // linear components - StateType mean = getBestXPercentEstimate(percentage); + log_likelihood = last_gmm.logLikelihood(dataset); + component_count++; + } while (component_number < max_num_components and std::abs(old_log_likelihood - log_likelihood) > component_delta); - double xMean = std::round(mean.getXPos() * 10000.0) / 10000.0; - double yMean = std::round(mean.getYPos() * 10000.0) / 10000.0; + if (std::abs(old_log_likelihood - log_likelihood) > component_delta or component_count <= 1) { + return last_gmm; + } else { + return last_last_gmm; + } +} - double xIMean = xI - (numToConsider * xMean); - double yIMean = yI - (numToConsider * yMean); +template +std::vector> ParticleFilter::getCovarianceMatrix(const bool ignore_explorers) { + // get an Eigen matrix of the particles + Eigen::MatrixXd dataset; + StateType::convertParticleListToEigen(m_CurrentList, dataset, ignore_explorers); + Eigen::VectorXd component_means = dataset.colwise().mean(); + Eigen::VectorXd component_sums = dataset.colwise().sum(); + int component_num = dataset.cols(); + int particle_num = dataset.rows(); + Eigen::VectorXd variances = ((particle_num * component_means) - component_sums).cwiseAbs(); + std::vector> cov_mat(component_num, std::vector(component_num, 0)); + for (int x = 0; x < component_num; x++) { + for (int y = 0; y < component_num; y++) { + cov_mat[x][y] = variances.coeff(x) * variances.coeff(y) / particle_num; + } + } + return cov_mat; +} - xIMean = std::round(xIMean * 10000.0) / 10000.0; - yIMean = std::round(yIMean * 10000.0) / 10000.0; - double pos0 = (xIMean * xIMean) / numToConsider; - double pos1 = (xIMean * yIMean) / numToConsider; - double pos7 = (yIMean * yIMean) / numToConsider; +template +std::vector ParticleFilter::getCovariance(float percentage) const { + double xI = 0; + double yI = 0; + double thetaSinI = 0; + double thetaCosI = 0; - //angular component - //double s = thetaSinI; - //double c = thetaCosI; + unsigned int numToConsider = m_NumParticles * (percentage / 100.0f); + // std::cout << "number particles : " << m_NumParticles; + // std::cout << "percentage: " << percentage; + // std::cout << "nums to consider: " << numToConsider; + unsigned int i = 0; + for (i; i < numToConsider; i++) { + xI += std::round(m_CurrentList[i]->getState().getXPos() * 10000.0) / 10000.0; + yI += std::round(m_CurrentList[i]->getState().getYPos() * 10000.0) / 10000.0; + thetaSinI += std::round(m_CurrentList[i]->getState().getSinTheta() * 10000.0) / 10000.0; + thetaCosI += std::round(m_CurrentList[i]->getState().getCosTheta() * 10000.0) / 10000.0; + } - thetaCosI = std::round(thetaCosI * 10000.0) / 10000.0; - thetaSinI = std::round(thetaSinI * 10000.0) / 10000.0; + // linear components + StateType mean = getBestXPercentEstimate(percentage); - double R = std::hypot(thetaCosI,thetaSinI);//sqrt(c * c + s * s); - R = std::round(R * 10000.0) / 10000.0; + double xMean = std::round(mean.getXPos() * 10000.0) / 10000.0; + double yMean = std::round(mean.getYPos() * 10000.0) / 10000.0; - double Rmean = R / numToConsider; - Rmean = std::round(Rmean * 10000.0) / 10000.0; + double xIMean = xI - (numToConsider * xMean); + double yIMean = yI - (numToConsider * yMean); + xIMean = std::round(xIMean * 10000.0) / 10000.0; + yIMean = std::round(yIMean * 10000.0) / 10000.0; + double pos0 = (xIMean * xIMean) / numToConsider; + double pos1 = (xIMean * yIMean) / numToConsider; + double pos7 = (yIMean * yIMean) / numToConsider; - //float pos36 = sqrt(-2 * log(sqrt(c * c + s * s))); // covariance not working and wrong - //double pos36 = sqrt(-2 * log(Rmean)); + // angular component + // double s = thetaSinI; + // double c = thetaCosI; - double pos35 = 1 - Rmean; // variance + thetaCosI = std::round(thetaCosI * 10000.0) / 10000.0; + thetaSinI = std::round(thetaSinI * 10000.0) / 10000.0; + double R = std::hypot(thetaCosI, thetaSinI); // sqrt(c * c + s * s); + R = std::round(R * 10000.0) / 10000.0; - pos0 = std::round(pos0 * 10000.0) / 10000.0; - pos1 = std::round(pos1 * 10000.0) / 10000.0; - pos7 = std::round(pos7 * 10000.0) / 10000.0; - pos35 = std::round(pos35 * 10000.0) / 10000.0; + double Rmean = R / numToConsider; + Rmean = std::round(Rmean * 10000.0) / 10000.0; - /** + // float pos36 = sqrt(-2 * log(sqrt(c * c + s * s))); // covariance not working and wrong + // double pos36 = sqrt(-2 * log(Rmean)); - if (pos36 < 0){ - std::cout << "nums to consider: " << numToConsider << std::endl; + double pos35 = 1 - Rmean; // variance - std::cout << "i: " << i << std::endl; - std::cout << "theta sin I: " << thetaSinI << std::endl; - std::cout << "theta cos I: " << thetaCosI << std::endl; - std::cout << "pos36: " << pos36 << std::endl; + pos0 = std::round(pos0 * 10000.0) / 10000.0; + pos1 = std::round(pos1 * 10000.0) / 10000.0; + pos7 = std::round(pos7 * 10000.0) / 10000.0; + pos35 = std::round(pos35 * 10000.0) / 10000.0; + /** - } - **/ + if (pos36 < 0){ + std::cout << "nums to consider: " << numToConsider << std::endl; - if (pos0 == -0.0) { - pos0 = 0.0; - } - if (pos1 == -0.0) { - pos1 = 0.0; - } - if (pos7 == -0.0) { - pos7 = 0.0; - } - if (pos35 < 0.0){ - pos35 = 0.0; - } - else if (pos35 > 1){ - pos35 = 1.0; - } + std::cout << "i: " << i << std::endl; + std::cout << "theta sin I: " << thetaSinI << std::endl; + std::cout << "theta cos I: " << thetaCosI << std::endl; + std::cout << "pos36: " << pos36 << std::endl; - std::vector covariance = {pos0, pos1, 0, 0, 0, 0, - pos1, pos7, 0, 0, 0, 0, //pos6 == pos1 - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, pos35}; + } + **/ - return covariance; - } + if (pos0 == -0.0) { + pos0 = 0.0; + } + if (pos1 == -0.0) { + pos1 = 0.0; + } + if (pos7 == -0.0) { + pos7 = 0.0; + } + if (pos35 < 0.0) { + pos35 = 0.0; + } else if (pos35 > 1) { + pos35 = 1.0; + } + std::vector covariance = {pos0, pos1, 0, 0, 0, 0, pos1, pos7, 0, 0, 0, 0, // pos6 == pos1 + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, pos35}; + + return covariance; +} } // namespace particle_filter diff --git a/include/particle_filter/RandomNumberGenerationStrategy.h b/include/particle_filter/RandomNumberGenerationStrategy.h index 2d8389a..96264e7 100644 --- a/include/particle_filter/RandomNumberGenerationStrategy.h +++ b/include/particle_filter/RandomNumberGenerationStrategy.h @@ -14,36 +14,36 @@ namespace particle_filter { * @author Stephan Wirth */ class RandomNumberGenerationStrategy { -public: - /** - * Empty constructor. - */ - RandomNumberGenerationStrategy(){}; - - /** - * Empty destructor. - */ - virtual ~RandomNumberGenerationStrategy(){}; - - /** - * Interface for the generation function of gaussian distributed numbers. - * @param standardDeviation Standard deviation d of the random number to - * generate. - * @return N(0, d*d)-distributed random number - */ - virtual double getGaussian(double standardDeviation) const = 0; - - /** - * Interface for the generation of a uniform distributed random number - * between min and max. - * @param min the minimum value, default is 0.0 - * @param max the maximum value, default is 1.0 - * @return random number between min and max, uniform distributed. - */ - virtual double getUniform(double min = 0.0, double max = 1.0) const = 0; - -protected: -private: + public: + /** + * Empty constructor. + */ + RandomNumberGenerationStrategy(){}; + + /** + * Empty destructor. + */ + virtual ~RandomNumberGenerationStrategy(){}; + + /** + * Interface for the generation function of gaussian distributed numbers. + * @param standardDeviation Standard deviation d of the random number to + * generate. + * @return N(0, d*d)-distributed random number + */ + virtual double getGaussian(double standardDeviation) const = 0; + + /** + * Interface for the generation of a uniform distributed random number + * between min and max. + * @param min the minimum value, default is 0.0 + * @param max the maximum value, default is 1.0 + * @return random number between min and max, uniform distributed. + */ + virtual double getUniform(double min = 0.0, double max = 1.0) const = 0; + + protected: + private: }; } // namespace particle_filter diff --git a/include/particle_filter/ResamplingStrategy.h b/include/particle_filter/ResamplingStrategy.h index b47926c..973cb33 100644 --- a/include/particle_filter/ResamplingStrategy.h +++ b/include/particle_filter/ResamplingStrategy.h @@ -1,10 +1,10 @@ #ifndef RESAMPLINGSTRATEGY_H #define RESAMPLINGSTRATEGY_H -#include - #include +#include + namespace particle_filter { /** @@ -23,32 +23,31 @@ namespace particle_filter { template class ResamplingStrategy { - /** - * A ParticleList is an array of pointers to Particles. - */ - typedef std::vector*> ParticleList; - -public: - /** - * The destructor is empty. - */ - virtual ~ResamplingStrategy(); - - /** - * This is the main method of ResamplingStrategy. It takes two references to - * particle lists. The first reference refers to the old particle list, the - * second to the new one. The strategy has to define which particles have to - * be copied to the new list. Use the assignment operator to copy a - * particle. Be careful that you don't copy the pointer to the particle! - * Never change the size of the lists! Define this function in your - * sub-class! - * @param source the source list to draw new particles from. - * @param destination the destination list where to put the copies. - */ - virtual void resample(const ParticleList& source, - const ParticleList& destination) const = 0; - -private: + /** + * A ParticleList is an array of pointers to Particles. + */ + typedef std::vector*> ParticleList; + + public: + /** + * The destructor is empty. + */ + virtual ~ResamplingStrategy(); + + /** + * This is the main method of ResamplingStrategy. It takes two references to + * particle lists. The first reference refers to the old particle list, the + * second to the new one. The strategy has to define which particles have to + * be copied to the new list. Use the assignment operator to copy a + * particle. Be careful that you don't copy the pointer to the particle! + * Never change the size of the lists! Define this function in your + * sub-class! + * @param source the source list to draw new particles from. + * @param destination the destination list where to put the copies. + */ + virtual void resample(const ParticleList& source, const ParticleList& destination) const = 0; + + private: }; template diff --git a/include/particle_filter/StateDistribution.h b/include/particle_filter/StateDistribution.h index cb2d95d..97e7ac9 100644 --- a/include/particle_filter/StateDistribution.h +++ b/include/particle_filter/StateDistribution.h @@ -22,30 +22,29 @@ namespace particle_filter { template class StateDistribution { -public: - /** - * The constructor of this base class is empty. - */ - StateDistribution(); - - /** - * The destructor is empty. - */ - virtual ~StateDistribution(); - - /** - * This is the main method of StateDistribution. It has to return a (random) - * state, drawn from your distribution. ParticleFilter will copy the state - * using the assignment operator, so be sure your state supports it! - * Define this method in your sub-class! - * @return Drawn state from the distribution. - */ - virtual const StateType draw() const = 0; - -private: + public: + /** + * The constructor of this base class is empty. + */ + StateDistribution(); + + /** + * The destructor is empty. + */ + virtual ~StateDistribution(); + + /** + * This is the main method of StateDistribution. It has to return a (random) + * state, drawn from your distribution. ParticleFilter will copy the state + * using the assignment operator, so be sure your state supports it! + * Define this method in your sub-class! + * @return Drawn state from the distribution. + */ + virtual const StateType draw() const = 0; + + private: }; - template StateDistribution::StateDistribution() {} diff --git a/include/particle_filter/defs.h b/include/particle_filter/defs.h index ffa088e..76f6803 100644 --- a/include/particle_filter/defs.h +++ b/include/particle_filter/defs.h @@ -1,9 +1,9 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models defs.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models defs.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _DEFS_H_ #define _DEFS_H_ diff --git a/include/particle_filter/gaussian.h b/include/particle_filter/gaussian.h index 0fc5ee2..06f12da 100644 --- a/include/particle_filter/gaussian.h +++ b/include/particle_filter/gaussian.h @@ -1,83 +1,62 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gaussian.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gaussian.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _GAUSSIAN_H_ #define _GAUSSIAN_H_ -#include #include -#include +#include #include -#include #include #include #include - -#include +#include +#include +#include namespace gmms { class Gaussian { -public: - // constructors - Gaussian() { - dimensionality_ = 0; - }; - Gaussian(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance) { - setMeanCovariance(mean, covariance); + public: + // constructors + Gaussian() { dimensionality_ = 0; }; + Gaussian(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance) { setMeanCovariance(mean, covariance); } + + // useful functions + double evaluate_point(const Eigen::VectorXd& pt) const; + + // setter & getter + inline int dimensionality() const { return dimensionality_; } + inline void setMean(const Eigen::VectorXd& mean) { + if (mean.size() != dimensionality_) { + throw std::runtime_error(dimensionality_mismatch()); } - // useful functions - double evaluate_point(const Eigen::VectorXd& pt) const; + mean_ = mean; + } + inline Eigen::VectorXd mean() const { return mean_; } + void setCovariance(const Eigen::MatrixXd& covariance); + inline Eigen::MatrixXd covariance() const { return covariance_; } + void setMeanCovariance(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance); + std::string toString() const; + void addToEigenMatrix(Eigen::MatrixXd& matrix, float x0, float y0, float x1, float y1, int stepcount) const; - // setter & getter - inline int dimensionality() const { - return dimensionality_; - } - inline void setMean(const Eigen::VectorXd& mean) { - if (mean.size() != dimensionality_) { - throw std::runtime_error(dimensionality_mismatch()); - } - - mean_ = mean; - } - inline Eigen::VectorXd mean() const { - return mean_; - } - void setCovariance(const Eigen::MatrixXd& covariance); - inline Eigen::MatrixXd covariance() const { - return covariance_; - } - void setMeanCovariance(const Eigen::VectorXd& mean, - const Eigen::MatrixXd& covariance); - std::string toString() const; - void addToEigenMatrix(Eigen::MatrixXd& matrix, - float x0, - float y0, - float x1, - float y1, - int stepcount) const; - - double calcDistance(const Eigen::VectorXd& mean) const; + double calcDistance(const Eigen::VectorXd& mean) const; -private: - int dimensionality_; - Eigen::VectorXd mean_; - Eigen::MatrixXd covariance_; - Eigen::MatrixXd inv_covariance_; - double cov_abs_determinant_; + private: + int dimensionality_; + Eigen::VectorXd mean_; + Eigen::MatrixXd covariance_; + Eigen::MatrixXd inv_covariance_; + double cov_abs_determinant_; - inline static const std::string dimensionality_mismatch() { - return "Dimensionality mismatch"; - } + inline static const std::string dimensionality_mismatch() { return "Dimensionality mismatch"; } - inline static const std::string covariance_not_invertible() { - return "Covariance matrix is not invertible"; - } + inline static const std::string covariance_not_invertible() { return "Covariance matrix is not invertible"; } }; } // namespace gmms diff --git a/include/particle_filter/gaussian_mixture_model.h b/include/particle_filter/gaussian_mixture_model.h index eb99fa6..a4bc66e 100644 --- a/include/particle_filter/gaussian_mixture_model.h +++ b/include/particle_filter/gaussian_mixture_model.h @@ -1,211 +1,172 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gaussian_mixture_model.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gaussian_mixture_model.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _GAUSSIAN_MIXTURE_MODEL_H_ #define _GAUSSIAN_MIXTURE_MODEL_H_ -#include +#include +#include +#include + #include +#include #include +#include #include #include #include #include #include -#include - -#include -#include -#include - namespace gmms { - /** - * @class GaussianMixtureModel - * - * @brief Class representing a Gaussian Mixture Model (GMM) including - * EM-fitting - * - * @author Niklas Fiedler - * @author Roberto Capobianco - * - */ +/** + * @class GaussianMixtureModel + * + * @brief Class representing a Gaussian Mixture Model (GMM) including + * EM-fitting + * + * @author Niklas Fiedler + * @author Roberto Capobianco + * + */ class GaussianMixtureModel { -public: - GaussianMixtureModel() { - num_components_ = 1; - delta_ = 0.01; - num_iterations_ = 100; - initialized_ = false; - } - explicit GaussianMixtureModel(const int num_components, - const double delta = 0.01, - const int num_iterations = 100) { - num_components_ = num_components; - prior_vec_.resize(num_components_); - gaussian_vec_.resize(num_components_); - delta_ = delta; - num_iterations_ = num_iterations; - initialized_ = false; - } - - /* - explicit GaussianMixtureModel(const GaussianMixtureModel& gmm) { - num_components_ = gmm.numComponents(); - delta_ = gmm.delta(); - num_iterations_ = gmm.numIterations(); - initialized_ = gmm.initialized(); - prior_vec_ = std::vector(gmm.priorVec()); - gaussian_vec_ = std::vector(gmm.gaussianVec()); - expectations_ = gmm.expectations().replicate(1, 1); - } - */ - - // dataset with each datapoint per row and each dimension per column - void initialize(const Eigen::MatrixXd& dataset); - void expectationMaximization(const Eigen::MatrixXd& dataset); - double logLikelihood(const Eigen::MatrixXd& dataset); - double bayesianInformationCriterion(const Eigen::MatrixXd& dataset); - void load(Eigen::MatrixXd model); - std::pair - getClosestGaussian(const Eigen::VectorXd& mean) const; - GaussianMixtureModel copy() const; - Eigen::MatrixXd save(); - - inline void load(const std::string filename) { - MatrixIO mio; - Eigen::MatrixXd model; - mio.readFromFile(filename, model); - load(model); - } - - inline void save(const std::string filename) { - MatrixIO mio; - mio.writeToFile(filename, save()); - } - - // setter & getter - inline int numComponents() const { - return num_components_; - } - inline void setNumComponents(const int num_components) { - num_components_ = num_components; - prior_vec_.resize(num_components_); - gaussian_vec_.resize(num_components_); - } - inline double delta() const { - return delta_; - } - inline void setDelta(const double delta) { - delta_ = delta; - } - inline int numIterations() const { - return num_iterations_; - } - inline bool initialized() const { - return initialized_; - } - inline Eigen::MatrixXd expectations() const { - return expectations_; - } - inline std::vector priorVec() const { - return prior_vec_; - } - inline std::vector gaussianVec() const { - return gaussian_vec_; - } - inline void setNumIterations(const int num_iterations) { - num_iterations_ = num_iterations; - } - inline const Gaussian& component(int i) { - if (i > num_components_) { - throw std::runtime_error(notexisting_()); - } - - return gaussian_vec_[i]; - } - inline const std::vector gaussianMeans() const { - std::vector means(gaussian_vec_.size()); - for (size_t g = 0; g < gaussian_vec_.size(); ++g) { - means.at(g) = gaussian_vec_.at(g).mean(); - } - return means; - } - inline const std::vector gaussianCovariances() const { - std::vector covs(gaussian_vec_.size()); - for (size_t g = 0; g < gaussian_vec_.size(); ++g) { - covs.at(g) = gaussian_vec_.at(g).covariance(); - } - return covs; - } - std::string toString() const; - - visualization_msgs::msg::Marker renderMarker(float x0, - float y0, - float x1, - float y1, - int stepcount, - rclcpp::Time stamp, - std::string n_space, - std::string frame, - rclcpp::Duration lifetime, - bool use_color = true, - bool use_height = true, - float z_offset = 0.0) const; - -private: - bool initialized_; - double delta_; - int num_iterations_; - int num_components_; - Eigen::MatrixXd expectations_; - std::vector prior_vec_; - std::vector gaussian_vec_; - - GaussianMixtureModel(int num_components, - double delta, - int num_iterations, - std::vector prior_vec_, - std::vector gaussian_vec_, - Eigen::MatrixXd expectations_) { - num_components_ = num_components; - prior_vec_ = std::vector(prior_vec_); - gaussian_vec_ = std::vector(gaussian_vec_); - expectations_ = expectations_.replicate(1, 1); - delta_ = delta; - num_iterations_ = num_iterations; - initialized_ = false; - } - - inline double probability_density_function(const Eigen::VectorXd point) { - double probability = 0; - - for (int k = 0; k < num_components_; ++k) { - probability += - prior_vec_[k] * gaussian_vec_[k].evaluate_point(point); - } - - return probability; - } - - void expectation(const Eigen::MatrixXd& dataset); - void maximization(const Eigen::MatrixXd& dataset); - - inline static const std::string zero_components() { - return "The number of components of the GMM has not been specified"; - } - - inline static const std::string notinitialized_() { - return "The GMM has not been initialized"; - } - - inline static const std::string notexisting_() { - return "The desired component does not exist"; - } + public: + GaussianMixtureModel() { + num_components_ = 1; + delta_ = 0.01; + num_iterations_ = 100; + initialized_ = false; + } + explicit GaussianMixtureModel(const int num_components, const double delta = 0.01, const int num_iterations = 100) { + num_components_ = num_components; + prior_vec_.resize(num_components_); + gaussian_vec_.resize(num_components_); + delta_ = delta; + num_iterations_ = num_iterations; + initialized_ = false; + } + + /* + explicit GaussianMixtureModel(const GaussianMixtureModel& gmm) { + num_components_ = gmm.numComponents(); + delta_ = gmm.delta(); + num_iterations_ = gmm.numIterations(); + initialized_ = gmm.initialized(); + prior_vec_ = std::vector(gmm.priorVec()); + gaussian_vec_ = std::vector(gmm.gaussianVec()); + expectations_ = gmm.expectations().replicate(1, 1); + } + */ + + // dataset with each datapoint per row and each dimension per column + void initialize(const Eigen::MatrixXd& dataset); + void expectationMaximization(const Eigen::MatrixXd& dataset); + double logLikelihood(const Eigen::MatrixXd& dataset); + double bayesianInformationCriterion(const Eigen::MatrixXd& dataset); + void load(Eigen::MatrixXd model); + std::pair getClosestGaussian(const Eigen::VectorXd& mean) const; + GaussianMixtureModel copy() const; + Eigen::MatrixXd save(); + + inline void load(const std::string filename) { + MatrixIO mio; + Eigen::MatrixXd model; + mio.readFromFile(filename, model); + load(model); + } + + inline void save(const std::string filename) { + MatrixIO mio; + mio.writeToFile(filename, save()); + } + + // setter & getter + inline int numComponents() const { return num_components_; } + inline void setNumComponents(const int num_components) { + num_components_ = num_components; + prior_vec_.resize(num_components_); + gaussian_vec_.resize(num_components_); + } + inline double delta() const { return delta_; } + inline void setDelta(const double delta) { delta_ = delta; } + inline int numIterations() const { return num_iterations_; } + inline bool initialized() const { return initialized_; } + inline Eigen::MatrixXd expectations() const { return expectations_; } + inline std::vector priorVec() const { return prior_vec_; } + inline std::vector gaussianVec() const { return gaussian_vec_; } + inline void setNumIterations(const int num_iterations) { num_iterations_ = num_iterations; } + inline const Gaussian& component(int i) { + if (i > num_components_) { + throw std::runtime_error(notexisting_()); + } + + return gaussian_vec_[i]; + } + inline const std::vector gaussianMeans() const { + std::vector means(gaussian_vec_.size()); + for (size_t g = 0; g < gaussian_vec_.size(); ++g) { + means.at(g) = gaussian_vec_.at(g).mean(); + } + return means; + } + inline const std::vector gaussianCovariances() const { + std::vector covs(gaussian_vec_.size()); + for (size_t g = 0; g < gaussian_vec_.size(); ++g) { + covs.at(g) = gaussian_vec_.at(g).covariance(); + } + return covs; + } + std::string toString() const; + + visualization_msgs::msg::Marker renderMarker(float x0, float y0, float x1, float y1, int stepcount, + rclcpp::Time stamp, std::string n_space, std::string frame, + rclcpp::Duration lifetime, bool use_color = true, bool use_height = true, + float z_offset = 0.0) const; + + private: + bool initialized_; + double delta_; + int num_iterations_; + int num_components_; + Eigen::MatrixXd expectations_; + std::vector prior_vec_; + std::vector gaussian_vec_; + + GaussianMixtureModel(int num_components, double delta, int num_iterations, std::vector prior_vec_, + std::vector gaussian_vec_, Eigen::MatrixXd expectations_) { + num_components_ = num_components; + prior_vec_ = std::vector(prior_vec_); + gaussian_vec_ = std::vector(gaussian_vec_); + expectations_ = expectations_.replicate(1, 1); + delta_ = delta; + num_iterations_ = num_iterations; + initialized_ = false; + } + + inline double probability_density_function(const Eigen::VectorXd point) { + double probability = 0; + + for (int k = 0; k < num_components_; ++k) { + probability += prior_vec_[k] * gaussian_vec_[k].evaluate_point(point); + } + + return probability; + } + + void expectation(const Eigen::MatrixXd& dataset); + void maximization(const Eigen::MatrixXd& dataset); + + inline static const std::string zero_components() { + return "The number of components of the GMM has not been specified"; + } + + inline static const std::string notinitialized_() { return "The GMM has not been initialized"; } + + inline static const std::string notexisting_() { return "The desired component does not exist"; } }; } // namespace gmms diff --git a/include/particle_filter/gmm_classifier.h b/include/particle_filter/gmm_classifier.h index e51dc11..23a334f 100644 --- a/include/particle_filter/gmm_classifier.h +++ b/include/particle_filter/gmm_classifier.h @@ -1,75 +1,59 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_classifier.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_classifier.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _GMM_CLASSIFIER_H_ #define _GMM_CLASSIFIER_H_ +#include + #include #include #include -#include - namespace gmms { class GMMClassifier { -public: - GMMClassifier() { - delta_ = 0.01; - max_iterations_ = 100; - trained_ = false; - } - GMMClassifier(const double delta, const int max_iterations) { - delta_ = delta; - max_iterations_ = max_iterations; - trained_ = false; - } - - // dataset with each datapoint per row and each dimension per column - // label must be in the last column - void train(const Eigen::MatrixXd& dataset, - bool evaluate_bic = true, - int gmm_components = 10); - std::vector predict(const Eigen::MatrixXd& dataset) const; - void load(const std::string filename); - void save(const std::string filename); - - // setter & getter - inline double delta() const { - return delta_; - } - inline void setDelta(const double delta) { - delta_ = delta; - } - inline int maxIterations() const { - return max_iterations_; - } - inline void setMaxIterations(const int max_iterations) { - max_iterations_ = max_iterations; - } - -private: - bool trained_; - double delta_; - int max_iterations_; - int input_size_; - std::vector classes_; - std::vector> gmm_vec_; - - inline static const std::string nottrained_() { - return "The model has not been trained"; - } - - inline static const std::string dimensionality_mismatch_() { - return "Dimensionality mismatch"; - } - - inline static const std::string notsufficient_() { - return "Class data size not sufficient: skipping class"; - } + public: + GMMClassifier() { + delta_ = 0.01; + max_iterations_ = 100; + trained_ = false; + } + GMMClassifier(const double delta, const int max_iterations) { + delta_ = delta; + max_iterations_ = max_iterations; + trained_ = false; + } + + // dataset with each datapoint per row and each dimension per column + // label must be in the last column + void train(const Eigen::MatrixXd& dataset, bool evaluate_bic = true, int gmm_components = 10); + std::vector predict(const Eigen::MatrixXd& dataset) const; + void load(const std::string filename); + void save(const std::string filename); + + // setter & getter + inline double delta() const { return delta_; } + inline void setDelta(const double delta) { delta_ = delta; } + inline int maxIterations() const { return max_iterations_; } + inline void setMaxIterations(const int max_iterations) { max_iterations_ = max_iterations; } + + private: + bool trained_; + double delta_; + int max_iterations_; + int input_size_; + std::vector classes_; + std::vector> gmm_vec_; + + inline static const std::string nottrained_() { return "The model has not been trained"; } + + inline static const std::string dimensionality_mismatch_() { return "Dimensionality mismatch"; } + + inline static const std::string notsufficient_() { return "Class data size not sufficient: skipping class"; } }; } // namespace gmms diff --git a/include/particle_filter/gmm_regressor.h b/include/particle_filter/gmm_regressor.h index 5c6c836..f82db80 100644 --- a/include/particle_filter/gmm_regressor.h +++ b/include/particle_filter/gmm_regressor.h @@ -1,88 +1,68 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_regressor.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_regressor.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _GMM_REGRESSOR_H_ #define _GMM_REGRESSOR_H_ +#include + #include #include #include #include -#include - namespace gmms { class GMMRegressor { -public: - GMMRegressor() { - delta_ = 0.01; - max_iterations_ = 100; - trained_ = false; - } - GMMRegressor(const double delta, const int max_iterations) { - delta_ = delta; - max_iterations_ = max_iterations; - trained_ = false; - } + public: + GMMRegressor() { + delta_ = 0.01; + max_iterations_ = 100; + trained_ = false; + } + GMMRegressor(const double delta, const int max_iterations) { + delta_ = delta; + max_iterations_ = max_iterations; + trained_ = false; + } - // dataset with each datapoint per row and each dimension per column - void train(const Eigen::MatrixXd& dataset, - bool evaluate_bic = true, - int gmm_components = 10); - inline Eigen::MatrixXd predict(const Eigen::MatrixXd& dataset) const { - int diff = input_size_ - dataset.cols(); - Eigen::VectorXi indices = Eigen::VectorXi::LinSpaced( - diff, dataset.cols(), input_size_ - 1); + // dataset with each datapoint per row and each dimension per column + void train(const Eigen::MatrixXd& dataset, bool evaluate_bic = true, int gmm_components = 10); + inline Eigen::MatrixXd predict(const Eigen::MatrixXd& dataset) const { + int diff = input_size_ - dataset.cols(); + Eigen::VectorXi indices = Eigen::VectorXi::LinSpaced(diff, dataset.cols(), input_size_ - 1); - return predict(dataset, indices); - } - Eigen::MatrixXd predict(const Eigen::MatrixXd& dataset, - const Eigen::VectorXi& output_indices) const; - void load(const std::string filename); - void save(const std::string filename); + return predict(dataset, indices); + } + Eigen::MatrixXd predict(const Eigen::MatrixXd& dataset, const Eigen::VectorXi& output_indices) const; + void load(const std::string filename); + void save(const std::string filename); - // setter & getter - inline double delta() const { - return delta_; - } - inline void setDelta(const double delta) { - delta_ = delta; - } - inline int maxIterations() const { - return max_iterations_; - } - inline void setMaxIterations(const int max_iterations) { - max_iterations_ = max_iterations; - } - inline const std::vector gmmMeans() const { - return gmm_->gaussianMeans(); - } - inline const std::vector gmmCovariances() const { - return gmm_->gaussianCovariances(); - } + // setter & getter + inline double delta() const { return delta_; } + inline void setDelta(const double delta) { delta_ = delta; } + inline int maxIterations() const { return max_iterations_; } + inline void setMaxIterations(const int max_iterations) { max_iterations_ = max_iterations; } + inline const std::vector gmmMeans() const { return gmm_->gaussianMeans(); } + inline const std::vector gmmCovariances() const { return gmm_->gaussianCovariances(); } -private: - bool trained_; - double delta_; - int max_iterations_; - int input_size_; - std::shared_ptr gmm_; + private: + bool trained_; + double delta_; + int max_iterations_; + int input_size_; + std::shared_ptr gmm_; - inline static const std::string nottrained_() { - return "The model has not been trained"; - } + inline static const std::string nottrained_() { return "The model has not been trained"; } - inline static const std::string notconsistent_() { - return "The requested output size is incosistent with the input data"; - } + inline static const std::string notconsistent_() { + return "The requested output size is incosistent with the input data"; + } - inline static const std::string notvalid_() { - return "The requested output size is 0"; - } + inline static const std::string notvalid_() { return "The requested output size is 0"; } }; } // namespace gmms diff --git a/include/particle_filter/k_means.h b/include/particle_filter/k_means.h index 3e31784..88c122e 100644 --- a/include/particle_filter/k_means.h +++ b/include/particle_filter/k_means.h @@ -1,9 +1,9 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models k_means.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models k_means.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _K_MEANS_H_ #define _K_MEANS_H_ @@ -13,55 +13,45 @@ namespace gmms { class KMeans { -public: - // constructors - KMeans() { - num_clusters_ = 0; - termination_flag_ = false; - } - KMeans(const int num_clusters) { - setNumClusters(num_clusters); - termination_flag_ = false; - } - - // dataset with each datapoint per row and each dimension per column - void cluster(const Eigen::MatrixXd& dataset); - - // setter & getter - inline int numClusters() const { - return num_clusters_; - } - inline void setNumClusters(const int num_clusters) { - num_clusters_ = num_clusters; - means_.resize(num_clusters_); - covariances_.resize(num_clusters_); - cluster_cardinalities_.assign(num_clusters_, 0); - } - inline std::vector means() const { - return means_; - } - inline std::vector covariances() const { - return covariances_; - } - inline std::vector assignments() const { - return assignments_; - } - inline std::vector clusterCardinalities() const { - return cluster_cardinalities_; - } - -private: - bool termination_flag_; - int num_clusters_; - std::vector means_; - std::vector covariances_; - std::vector assignments_; - std::vector cluster_cardinalities_; - - void forgyInitialization(const Eigen::MatrixXd& dataset); - void assign(const Eigen::MatrixXd& dataset); - void update(const Eigen::MatrixXd& dataset); - void computeCovariances(const Eigen::MatrixXd& dataset); + public: + // constructors + KMeans() { + num_clusters_ = 0; + termination_flag_ = false; + } + KMeans(const int num_clusters) { + setNumClusters(num_clusters); + termination_flag_ = false; + } + + // dataset with each datapoint per row and each dimension per column + void cluster(const Eigen::MatrixXd& dataset); + + // setter & getter + inline int numClusters() const { return num_clusters_; } + inline void setNumClusters(const int num_clusters) { + num_clusters_ = num_clusters; + means_.resize(num_clusters_); + covariances_.resize(num_clusters_); + cluster_cardinalities_.assign(num_clusters_, 0); + } + inline std::vector means() const { return means_; } + inline std::vector covariances() const { return covariances_; } + inline std::vector assignments() const { return assignments_; } + inline std::vector clusterCardinalities() const { return cluster_cardinalities_; } + + private: + bool termination_flag_; + int num_clusters_; + std::vector means_; + std::vector covariances_; + std::vector assignments_; + std::vector cluster_cardinalities_; + + void forgyInitialization(const Eigen::MatrixXd& dataset); + void assign(const Eigen::MatrixXd& dataset); + void update(const Eigen::MatrixXd& dataset); + void computeCovariances(const Eigen::MatrixXd& dataset); }; } // namespace gmms diff --git a/include/particle_filter/matrix_io.h b/include/particle_filter/matrix_io.h index 473c6b4..ab2fde0 100644 --- a/include/particle_filter/matrix_io.h +++ b/include/particle_filter/matrix_io.h @@ -1,9 +1,9 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models matrix_io.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models matrix_io.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _MATRIX_IO_H_ #define _MATRIX_IO_H_ @@ -12,12 +12,12 @@ #include class MatrixIO { -public: - MatrixIO() {} - ~MatrixIO() {} + public: + MatrixIO() {} + ~MatrixIO() {} - void readFromFile(const std::string filename, Eigen::MatrixXd& matrix); - void writeToFile(const std::string filename, const Eigen::MatrixXd& matrix); + void readFromFile(const std::string filename, Eigen::MatrixXd& matrix); + void writeToFile(const std::string filename, const Eigen::MatrixXd& matrix); }; #endif // _MATRIX_IO_H_ diff --git a/include/particle_filter/pca.h b/include/particle_filter/pca.h index cb47d7c..7688ee6 100644 --- a/include/particle_filter/pca.h +++ b/include/particle_filter/pca.h @@ -1,9 +1,9 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models pca.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models pca.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #ifndef _PCA_H_ #define _PCA_H_ @@ -13,34 +13,25 @@ namespace gmms { class PCA { -public: - // constructors - PCA() { - num_components_ = 0; - } - PCA(const int num_components) { - num_components_ = num_components; - } + public: + // constructors + PCA() { num_components_ = 0; } + PCA(const int num_components) { num_components_ = num_components; } - // setters & getters - inline void setNumComponents(const int num_components) { - num_components_ = num_components; - } - inline int numComponents() const { - return num_components_; - } + // setters & getters + inline void setNumComponents(const int num_components) { num_components_ = num_components; } + inline int numComponents() const { return num_components_; } - // useful methods - // dataset with each datapoint per row and each dimension per column - inline Eigen::MatrixXd pca(const Eigen::MatrixXd& dataset) { - double variance_retained = 0.0; - return pca(dataset, variance_retained); - } - Eigen::MatrixXd - pca(const Eigen::MatrixXd& dataset, double& retained_variance); + // useful methods + // dataset with each datapoint per row and each dimension per column + inline Eigen::MatrixXd pca(const Eigen::MatrixXd& dataset) { + double variance_retained = 0.0; + return pca(dataset, variance_retained); + } + Eigen::MatrixXd pca(const Eigen::MatrixXd& dataset, double& retained_variance); -private: - int num_components_; + private: + int num_components_; }; } // namespace gmms diff --git a/src/CRandomNumberGenerator.cpp b/src/CRandomNumberGenerator.cpp index 1180f8b..ad219d8 100644 --- a/src/CRandomNumberGenerator.cpp +++ b/src/CRandomNumberGenerator.cpp @@ -1,10 +1,9 @@ +#include + #include #include #include - -#include - using namespace particle_filter; /** @@ -19,44 +18,42 @@ using namespace particle_filter; * @author Stephan Wirth */ -CRandomNumberGenerator::CRandomNumberGenerator() { - init(); -} +CRandomNumberGenerator::CRandomNumberGenerator() { init(); } CRandomNumberGenerator::~CRandomNumberGenerator() {} void CRandomNumberGenerator::init() { - srand(time(0)); - m_GaussianBufferFilled = false; + srand(time(0)); + m_GaussianBufferFilled = false; } double CRandomNumberGenerator::getGaussian(double standardDeviation) const { - if (standardDeviation < 0) { - standardDeviation = -standardDeviation; - } - - if (m_GaussianBufferFilled == true) { - m_GaussianBufferFilled = false; - return standardDeviation * m_GaussianBufferVariable; - } - double x1, x2, w, y1, y2; - do { - x1 = getUniform(-1.0, 1.0); - x2 = getUniform(-1.0, 1.0); - w = x1 * x1 + x2 * x2; - } while (w >= 1.0); - - w = sqrt((-2.0 * log(w)) / w); - y1 = x1 * w; - y2 = x2 * w; - // now y1 and y2 are N(0,1) distributed - // we use only one, so we store the other - m_GaussianBufferVariable = y2; - m_GaussianBufferFilled = true; - return standardDeviation * y1; + if (standardDeviation < 0) { + standardDeviation = -standardDeviation; + } + + if (m_GaussianBufferFilled == true) { + m_GaussianBufferFilled = false; + return standardDeviation * m_GaussianBufferVariable; + } + double x1, x2, w, y1, y2; + do { + x1 = getUniform(-1.0, 1.0); + x2 = getUniform(-1.0, 1.0); + w = x1 * x1 + x2 * x2; + } while (w >= 1.0); + + w = sqrt((-2.0 * log(w)) / w); + y1 = x1 * w; + y2 = x2 * w; + // now y1 and y2 are N(0,1) distributed + // we use only one, so we store the other + m_GaussianBufferVariable = y2; + m_GaussianBufferFilled = true; + return standardDeviation * y1; } double CRandomNumberGenerator::getUniform(double min, double max) const { - double range = max - min; - return 1.0 * rand() / RAND_MAX * range + min; + double range = max - min; + return 1.0 * rand() / RAND_MAX * range + min; } diff --git a/src/gaussian.cpp b/src/gaussian.cpp index 013e4e4..8dc81b0 100644 --- a/src/gaussian.cpp +++ b/src/gaussian.cpp @@ -1,126 +1,110 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gaussian.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gaussian.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#include #include #include - -#include - namespace gmms { double Gaussian::evaluate_point(const Eigen::VectorXd& pt) const { - if (pt.size() != dimensionality_) { - throw std::runtime_error(dimensionality_mismatch()); - } + if (pt.size() != dimensionality_) { + throw std::runtime_error(dimensionality_mismatch()); + } - Eigen::VectorXd dist = pt - mean_; + Eigen::VectorXd dist = pt - mean_; - double factor = std::sqrt( - std::pow(2 * M_PI, dimensionality_) * cov_abs_determinant_); - double exp = std::exp(-0.5 * dist.transpose() * inv_covariance_ * dist); + double factor = std::sqrt(std::pow(2 * M_PI, dimensionality_) * cov_abs_determinant_); + double exp = std::exp(-0.5 * dist.transpose() * inv_covariance_ * dist); - double result = exp / factor; + double result = exp / factor; - if (result == 0) { - srand(time(NULL)); - double random = ((double) rand() / (RAND_MAX)); - result = random * 1e-15; - } + if (result == 0) { + srand(time(NULL)); + double random = ((double)rand() / (RAND_MAX)); + result = random * 1e-15; + } - return result; + return result; } void Gaussian::setCovariance(const Eigen::MatrixXd& covariance) { - if (covariance.rows() != covariance.cols() || - covariance.rows() != dimensionality_) { - throw std::runtime_error(dimensionality_mismatch()); - } + if (covariance.rows() != covariance.cols() || covariance.rows() != dimensionality_) { + throw std::runtime_error(dimensionality_mismatch()); + } - Eigen::SelfAdjointEigenSolver solver(covariance.cols()); - solver.compute(covariance); + Eigen::SelfAdjointEigenSolver solver(covariance.cols()); + solver.compute(covariance); - Eigen::VectorXd eigenvalues = solver.eigenvalues(); - Eigen::MatrixXd eigenvectors = solver.eigenvectors(); + Eigen::VectorXd eigenvalues = solver.eigenvalues(); + Eigen::MatrixXd eigenvectors = solver.eigenvectors(); - for (int i = 0; i < eigenvalues.rows(); ++i) { - if (eigenvalues(i) < M_EPS) { - eigenvalues(i) = M_EPS; - } + for (int i = 0; i < eigenvalues.rows(); ++i) { + if (eigenvalues(i) < M_EPS) { + eigenvalues(i) = M_EPS; } + } - covariance_ = - eigenvectors * eigenvalues.asDiagonal() * eigenvectors.inverse(); - double abs_determinant = std::fabs(covariance_.determinant()); + covariance_ = eigenvectors * eigenvalues.asDiagonal() * eigenvectors.inverse(); + double abs_determinant = std::fabs(covariance_.determinant()); - inv_covariance_ = covariance_.inverse(); - cov_abs_determinant_ = abs_determinant; + inv_covariance_ = covariance_.inverse(); + cov_abs_determinant_ = abs_determinant; } -void Gaussian::setMeanCovariance(const Eigen::VectorXd& mean, - const Eigen::MatrixXd& covariance) { - int dimensionality = mean.size(); - dimensionality_ = dimensionality; - setMean(mean); - setCovariance(covariance); +void Gaussian::setMeanCovariance(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance) { + int dimensionality = mean.size(); + dimensionality_ = dimensionality; + setMean(mean); + setCovariance(covariance); } std::string Gaussian::toString() const { - Eigen::IOFormat mean_format( - 5, 0, ", ", "\n", "[", "]", "Mean: ", "\n"); - Eigen::IOFormat cov_format( - 5, 0, ", ", "\n", "[", "]", "Covariance: ", "\n"); - std::stringstream gaussian_string; - gaussian_string << "Gaussian: \n"; - gaussian_string << mean_.format(mean_format); - gaussian_string << covariance_.format(cov_format); - return gaussian_string.str(); + Eigen::IOFormat mean_format(5, 0, ", ", "\n", "[", "]", "Mean: ", "\n"); + Eigen::IOFormat cov_format(5, 0, ", ", "\n", "[", "]", "Covariance: ", "\n"); + std::stringstream gaussian_string; + gaussian_string << "Gaussian: \n"; + gaussian_string << mean_.format(mean_format); + gaussian_string << covariance_.format(cov_format); + return gaussian_string.str(); } -void Gaussian::addToEigenMatrix(Eigen::MatrixXd& matrix, - float x0, - float y0, - float x1, - float y1, - int stepcount) const { - assert(x0 < x1); - assert(y0 < y1); - float x_delta = std::abs(x1 - x0); - float y_delta = std::abs(y1 - y0); - float x, y; - float x_mean = mean_(0, 0); - float y_mean = mean_(1, 0); - // TODO: save stepsize - - for (int y_step = 0; y_step < stepcount; y_step++) { - y = y0 + (y_delta / stepcount * y_step); - for (int x_step = 0; x_step < stepcount; x_step++) { - x = x0 + (x_delta / stepcount * x_step); - // Function taken from - // https://en.wikipedia.org/wiki/Gaussian_function#Two-dimensional_Gaussian_function - // matrix(y_step, x_step) += 1 * std::exp(-(std::pow(x - x_mean, 2) - // / covariance_(0, 0) + 2 * (x - x_mean) * (y - y_mean) / - // covariance_(1, 0) + std::pow(y - y_mean, 2) / covariance_(1, - // 1))); - matrix(y_step, x_step) += - 1 * std::exp(-((std::pow(x - x_mean, 2) / - (2 * covariance_(0, 0))) + - (std::pow(y - y_mean, 2) / - (2 * covariance_(1, 1))))); - } +void Gaussian::addToEigenMatrix(Eigen::MatrixXd& matrix, float x0, float y0, float x1, float y1, int stepcount) const { + assert(x0 < x1); + assert(y0 < y1); + float x_delta = std::abs(x1 - x0); + float y_delta = std::abs(y1 - y0); + float x, y; + float x_mean = mean_(0, 0); + float y_mean = mean_(1, 0); + // TODO: save stepsize + + for (int y_step = 0; y_step < stepcount; y_step++) { + y = y0 + (y_delta / stepcount * y_step); + for (int x_step = 0; x_step < stepcount; x_step++) { + x = x0 + (x_delta / stepcount * x_step); + // Function taken from + // https://en.wikipedia.org/wiki/Gaussian_function#Two-dimensional_Gaussian_function + // matrix(y_step, x_step) += 1 * std::exp(-(std::pow(x - x_mean, 2) + // / covariance_(0, 0) + 2 * (x - x_mean) * (y - y_mean) / + // covariance_(1, 0) + std::pow(y - y_mean, 2) / covariance_(1, + // 1))); + matrix(y_step, x_step) += 1 * std::exp(-((std::pow(x - x_mean, 2) / (2 * covariance_(0, 0))) + + (std::pow(y - y_mean, 2) / (2 * covariance_(1, 1))))); } + } } double Gaussian::calcDistance(const Eigen::VectorXd& mean) const { - if (mean.size() != dimensionality_) { - throw std::runtime_error(dimensionality_mismatch()); - } - Eigen::VectorXd diff_vec = mean_ - mean; - return diff_vec.cwiseAbs().sum() / static_cast(dimensionality_); + if (mean.size() != dimensionality_) { + throw std::runtime_error(dimensionality_mismatch()); + } + Eigen::VectorXd diff_vec = mean_ - mean; + return diff_vec.cwiseAbs().sum() / static_cast(dimensionality_); } - } // namespace gmms diff --git a/src/gaussian_mixture_model.cpp b/src/gaussian_mixture_model.cpp index efa0968..7277167 100644 --- a/src/gaussian_mixture_model.cpp +++ b/src/gaussian_mixture_model.cpp @@ -1,320 +1,295 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gaussian_mixture_model.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// -//#########################################################// -//# #// -//# gaussian_mixture_models gaussian_mixture_model.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gaussian_mixture_model.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gaussian_mixture_model.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #include +#include #include #include #include -#include - namespace gmms { void GaussianMixtureModel::initialize(const Eigen::MatrixXd& dataset) { - if (num_components_ == 0) { - throw std::runtime_error(zero_components()); - } + if (num_components_ == 0) { + throw std::runtime_error(zero_components()); + } - int dataset_size = dataset.rows(); + int dataset_size = dataset.rows(); - KMeans km(num_components_); - km.cluster(dataset); + KMeans km(num_components_); + km.cluster(dataset); - const std::vector& means = km.means(); - const std::vector& covariances = km.covariances(); - const std::vector& cluster_cardinalities = km.clusterCardinalities(); + const std::vector& means = km.means(); + const std::vector& covariances = km.covariances(); + const std::vector& cluster_cardinalities = km.clusterCardinalities(); - for (int i = 0; i < num_components_; ++i) { - gaussian_vec_[i].setMeanCovariance(means[i], covariances[i]); - prior_vec_[i] = double(cluster_cardinalities[i]) / double(dataset_size); - } + for (int i = 0; i < num_components_; ++i) { + gaussian_vec_[i].setMeanCovariance(means[i], covariances[i]); + prior_vec_[i] = double(cluster_cardinalities[i]) / double(dataset_size); + } - initialized_ = true; + initialized_ = true; } double GaussianMixtureModel::logLikelihood(const Eigen::MatrixXd& dataset) { - if (!initialized_) { - throw std::runtime_error(notinitialized_()); - } + if (!initialized_) { + throw std::runtime_error(notinitialized_()); + } - double log_likelihood = 0.0; - int dataset_size = dataset.rows(); + double log_likelihood = 0.0; + int dataset_size = dataset.rows(); #pragma omp parallel for reduction(+ : log_likelihood) - for (int i = 0; i < dataset_size; ++i) { - log_likelihood += std::log( - probability_density_function(dataset.row(i).transpose())); - } + for (int i = 0; i < dataset_size; ++i) { + log_likelihood += std::log(probability_density_function(dataset.row(i).transpose())); + } - return log_likelihood; + return log_likelihood; } -void GaussianMixtureModel::expectationMaximization( - const Eigen::MatrixXd& dataset) { - if (!initialized_) { - throw std::runtime_error(notinitialized_()); - } - - double old_log_likelihood; - double log_likelihood = 0.0; - double delta = 0.0; - int iteration = 0; - - do { - expectation(dataset); - maximization(dataset); - old_log_likelihood = log_likelihood; - log_likelihood = logLikelihood(dataset); - delta = log_likelihood - old_log_likelihood; - ++iteration; - } while (std::abs(delta) > delta_ && iteration < num_iterations_); +void GaussianMixtureModel::expectationMaximization(const Eigen::MatrixXd& dataset) { + if (!initialized_) { + throw std::runtime_error(notinitialized_()); + } + + double old_log_likelihood; + double log_likelihood = 0.0; + double delta = 0.0; + int iteration = 0; + + do { + expectation(dataset); + maximization(dataset); + old_log_likelihood = log_likelihood; + log_likelihood = logLikelihood(dataset); + delta = log_likelihood - old_log_likelihood; + ++iteration; + } while (std::abs(delta) > delta_ && iteration < num_iterations_); } void GaussianMixtureModel::expectation(const Eigen::MatrixXd& dataset) { - int dataset_size = dataset.rows(); + int dataset_size = dataset.rows(); - expectations_.setZero(dataset_size, num_components_); + expectations_.setZero(dataset_size, num_components_); #pragma omp parallel for - for (int i = 0; i < dataset_size; ++i) { - double normalization_factor = 0.f; + for (int i = 0; i < dataset_size; ++i) { + double normalization_factor = 0.f; - for (int j = 0; j < num_components_; ++j) { - expectations_(i, j) = - prior_vec_[j] * - gaussian_vec_[j].evaluate_point(dataset.row(i)); + for (int j = 0; j < num_components_; ++j) { + expectations_(i, j) = prior_vec_[j] * gaussian_vec_[j].evaluate_point(dataset.row(i)); - if (i == dataset_size - 1 && expectations_.col(j).sum() == 0) { - expectations_(i, j) = M_EPS; - } - } + if (i == dataset_size - 1 && expectations_.col(j).sum() == 0) { + expectations_(i, j) = M_EPS; + } + } - double expectations_sum = expectations_.row(i).sum(); + double expectations_sum = expectations_.row(i).sum(); - if (expectations_sum != 0) { - normalization_factor = std::min(1 / expectations_sum, 1e15); - expectations_.row(i) = normalization_factor * expectations_.row(i); - } + if (expectations_sum != 0) { + normalization_factor = std::min(1 / expectations_sum, 1e15); + expectations_.row(i) = normalization_factor * expectations_.row(i); } + } } void GaussianMixtureModel::maximization(const Eigen::MatrixXd& dataset) { - int dataset_size = dataset.rows(); - double expectation_sum = 0.f; + int dataset_size = dataset.rows(); + double expectation_sum = 0.f; - for (int i = 0; i < num_components_; ++i) { - prior_vec_[i] = expectations_.col(i).sum(); - double inverse_cluster_expectation = 1.f / prior_vec_[i]; + for (int i = 0; i < num_components_; ++i) { + prior_vec_[i] = expectations_.col(i).sum(); + double inverse_cluster_expectation = 1.f / prior_vec_[i]; - Eigen::VectorXd mean = inverse_cluster_expectation * - expectations_.col(i).transpose() * dataset; - Eigen::MatrixXd covariance = - Eigen::MatrixXd::Zero(dataset.cols(), dataset.cols()); - std::vector thread_covariance; + Eigen::VectorXd mean = inverse_cluster_expectation * expectations_.col(i).transpose() * dataset; + Eigen::MatrixXd covariance = Eigen::MatrixXd::Zero(dataset.cols(), dataset.cols()); + std::vector thread_covariance; - omp_lock_t lck; - omp_init_lock(&lck); + omp_lock_t lck; + omp_init_lock(&lck); #pragma omp parallel - { - int num_threads = omp_get_max_threads(); - int thread_dataset_size = - floor(double(dataset_size) / double(num_threads)); - int thread_id = omp_get_thread_num(); - int thread_j_min = thread_dataset_size * thread_id; - int thread_j_max; - - Eigen::MatrixXd t_covariance = - Eigen::MatrixXd::Zero(dataset.cols(), dataset.cols()); - ; - - if (thread_id == num_threads - 1) { - thread_j_max = dataset_size; - } else { - thread_j_max = thread_j_min + thread_dataset_size; - } - - for (int j = thread_j_min; j < thread_j_max; ++j) { - Eigen::VectorXd diff = dataset.row(j).transpose() - mean; - t_covariance += expectations_(j, i) * diff * diff.transpose(); - } - - omp_set_lock(&lck); - thread_covariance.push_back(t_covariance); - omp_unset_lock(&lck); - } - - omp_destroy_lock(&lck); - - for (size_t t = 0; t < thread_covariance.size(); ++t) { - covariance += thread_covariance.at(t); - } - - covariance = inverse_cluster_expectation * covariance; - - gaussian_vec_[i].setMeanCovariance(mean, covariance); - prior_vec_[i] = prior_vec_[i] / double(dataset_size); + { + int num_threads = omp_get_max_threads(); + int thread_dataset_size = floor(double(dataset_size) / double(num_threads)); + int thread_id = omp_get_thread_num(); + int thread_j_min = thread_dataset_size * thread_id; + int thread_j_max; + + Eigen::MatrixXd t_covariance = Eigen::MatrixXd::Zero(dataset.cols(), dataset.cols()); + ; + + if (thread_id == num_threads - 1) { + thread_j_max = dataset_size; + } else { + thread_j_max = thread_j_min + thread_dataset_size; + } + + for (int j = thread_j_min; j < thread_j_max; ++j) { + Eigen::VectorXd diff = dataset.row(j).transpose() - mean; + t_covariance += expectations_(j, i) * diff * diff.transpose(); + } + + omp_set_lock(&lck); + thread_covariance.push_back(t_covariance); + omp_unset_lock(&lck); } + + omp_destroy_lock(&lck); + + for (size_t t = 0; t < thread_covariance.size(); ++t) { + covariance += thread_covariance.at(t); + } + + covariance = inverse_cluster_expectation * covariance; + + gaussian_vec_[i].setMeanCovariance(mean, covariance); + prior_vec_[i] = prior_vec_[i] / double(dataset_size); + } } -double GaussianMixtureModel::bayesianInformationCriterion( - const Eigen::MatrixXd& dataset) { - int dataset_size = dataset.rows(); - int point_dimensionality = dataset.cols(); - double free_parameters = - (num_components_ - 1) * num_components_ * - (point_dimensionality + - 0.5 * point_dimensionality * (point_dimensionality + 1)); +double GaussianMixtureModel::bayesianInformationCriterion(const Eigen::MatrixXd& dataset) { + int dataset_size = dataset.rows(); + int point_dimensionality = dataset.cols(); + double free_parameters = (num_components_ - 1) * num_components_ * + (point_dimensionality + 0.5 * point_dimensionality * (point_dimensionality + 1)); - double bic = -logLikelihood(dataset) + - 0.5 * free_parameters * std::log(dataset_size); + double bic = -logLikelihood(dataset) + 0.5 * free_parameters * std::log(dataset_size); - return bic; + return bic; } GaussianMixtureModel GaussianMixtureModel::copy() const { - return GaussianMixtureModel(num_components_, delta_, num_iterations_, - prior_vec_, gaussian_vec_, expectations_); + return GaussianMixtureModel(num_components_, delta_, num_iterations_, prior_vec_, gaussian_vec_, expectations_); } void GaussianMixtureModel::load(Eigen::MatrixXd model) { - int idx = 0; - // covariance rows plus one mean and prior - int dimensionality = model.cols(); - int size = dimensionality + 2; - int num_components = model.rows() / size; - setNumComponents(num_components); - - for (int i = 0; i < num_components; ++i) { - double prior = model(idx, 0); - Eigen::VectorXd mean = model.row(idx + 1); - Eigen::MatrixXd covariance = - model.block(idx + 2, 0, dimensionality, dimensionality); - - gaussian_vec_[i].setMeanCovariance(mean, covariance); - prior_vec_[i] = prior; - - idx += size; - } - - initialized_ = true; -} - -Eigen::MatrixXd GaussianMixtureModel::save() { - if (!initialized_) { - throw std::runtime_error(notinitialized_()); - } + int idx = 0; + // covariance rows plus one mean and prior + int dimensionality = model.cols(); + int size = dimensionality + 2; + int num_components = model.rows() / size; + setNumComponents(num_components); - int dimensionality = gaussian_vec_[0].dimensionality(); - // num_components covariances, means and priors - int model_rows = num_components_ * dimensionality + 2 * num_components_; - Eigen::MatrixXd model = Eigen::MatrixXd::Zero(model_rows, dimensionality); + for (int i = 0; i < num_components; ++i) { + double prior = model(idx, 0); + Eigen::VectorXd mean = model.row(idx + 1); + Eigen::MatrixXd covariance = model.block(idx + 2, 0, dimensionality, dimensionality); - int idx = 0; - // covariance rows plus one mean and prior - int size = dimensionality + 2; + gaussian_vec_[i].setMeanCovariance(mean, covariance); + prior_vec_[i] = prior; - for (int i = 0; i < num_components_; ++i) { - Eigen::VectorXd mean = gaussian_vec_[i].mean(); - Eigen::MatrixXd covariance = gaussian_vec_[i].covariance(); - model(idx, 0) = prior_vec_[i]; + idx += size; + } - model.block(idx + 1, 0, 1, dimensionality) = mean.transpose(); - model.block(idx + 2, 0, dimensionality, dimensionality) = covariance; - idx += size; - } + initialized_ = true; +} - return model; +Eigen::MatrixXd GaussianMixtureModel::save() { + if (!initialized_) { + throw std::runtime_error(notinitialized_()); + } + + int dimensionality = gaussian_vec_[0].dimensionality(); + // num_components covariances, means and priors + int model_rows = num_components_ * dimensionality + 2 * num_components_; + Eigen::MatrixXd model = Eigen::MatrixXd::Zero(model_rows, dimensionality); + + int idx = 0; + // covariance rows plus one mean and prior + int size = dimensionality + 2; + + for (int i = 0; i < num_components_; ++i) { + Eigen::VectorXd mean = gaussian_vec_[i].mean(); + Eigen::MatrixXd covariance = gaussian_vec_[i].covariance(); + model(idx, 0) = prior_vec_[i]; + + model.block(idx + 1, 0, 1, dimensionality) = mean.transpose(); + model.block(idx + 2, 0, dimensionality, dimensionality) = covariance; + idx += size; + } + + return model; } std::string GaussianMixtureModel::toString() const { - std::stringstream gmm_string; - gmm_string << "GMM: \n"; - for (const Gaussian& gaussian : gaussian_vec_) { - gmm_string << gaussian.toString(); - } - return gmm_string.str(); + std::stringstream gmm_string; + gmm_string << "GMM: \n"; + for (const Gaussian& gaussian : gaussian_vec_) { + gmm_string << gaussian.toString(); + } + return gmm_string.str(); } -visualization_msgs::msg::Marker GaussianMixtureModel::renderMarker(float x0, - float y0, - float x1, - float y1, - int stepcount, - rclcpp::Time stamp, - std::string n_space, - std::string frame, - rclcpp::Duration lifetime, - bool use_color, - bool use_height, - float z_offset) const { - assert(x0 < x1); - assert(y0 < y1); - Eigen::MatrixXd matrix = Eigen::MatrixXd::Zero(stepcount, stepcount); - // calculate values for each position by summing up the Gaussians in the GMM - for (const Gaussian& gaussian : gaussian_vec_) { - gaussian.addToEigenMatrix(matrix, x0, y0, x1, y1, stepcount); - } - // normalizing the matrix to values between 0 and 1 - matrix = matrix.array() - matrix.minCoeff(); - matrix = matrix / matrix.maxCoeff(); - float x_delta = std::abs(x1 - x0); - float y_delta = std::abs(y1 - y0); - float x, y; - float x_stepsize = x_delta / stepcount; - float y_stepsize = y_delta / stepcount; - visualization_msgs::msg::Marker marker; - marker.header.frame_id = frame; - marker.header.stamp = stamp; - marker.ns = n_space; - marker.type = visualization_msgs::msg::Marker::POINTS; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = lifetime; - marker.scale.x = x_stepsize; - marker.scale.y = y_stepsize; - marker.scale.z = 0.05; - marker.pose.orientation.w = 1; - if (!use_color) { - marker.color.b = 1; - marker.color.a = .8; - } - - for (int y_step = 0; y_step < stepcount; y_step++) { - y = y0 + (y_stepsize * y_step); - for (int x_step = 0; x_step < stepcount; x_step++) { - x = x0 + (x_stepsize * x_step); - geometry_msgs::msg::Point point; - point.x = x; - point.y = y; - if (use_height) { - point.z = matrix(y_step, x_step); - } else { - point.z = 0; - } - point.z += z_offset; - marker.points.push_back(point); - } +visualization_msgs::msg::Marker GaussianMixtureModel::renderMarker(float x0, float y0, float x1, float y1, + int stepcount, rclcpp::Time stamp, + std::string n_space, std::string frame, + rclcpp::Duration lifetime, bool use_color, + bool use_height, float z_offset) const { + assert(x0 < x1); + assert(y0 < y1); + Eigen::MatrixXd matrix = Eigen::MatrixXd::Zero(stepcount, stepcount); + // calculate values for each position by summing up the Gaussians in the GMM + for (const Gaussian& gaussian : gaussian_vec_) { + gaussian.addToEigenMatrix(matrix, x0, y0, x1, y1, stepcount); + } + // normalizing the matrix to values between 0 and 1 + matrix = matrix.array() - matrix.minCoeff(); + matrix = matrix / matrix.maxCoeff(); + float x_delta = std::abs(x1 - x0); + float y_delta = std::abs(y1 - y0); + float x, y; + float x_stepsize = x_delta / stepcount; + float y_stepsize = y_delta / stepcount; + visualization_msgs::msg::Marker marker; + marker.header.frame_id = frame; + marker.header.stamp = stamp; + marker.ns = n_space; + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = lifetime; + marker.scale.x = x_stepsize; + marker.scale.y = y_stepsize; + marker.scale.z = 0.05; + marker.pose.orientation.w = 1; + if (!use_color) { + marker.color.b = 1; + marker.color.a = .8; + } + + for (int y_step = 0; y_step < stepcount; y_step++) { + y = y0 + (y_stepsize * y_step); + for (int x_step = 0; x_step < stepcount; x_step++) { + x = x0 + (x_stepsize * x_step); + geometry_msgs::msg::Point point; + point.x = x; + point.y = y; + if (use_height) { + point.z = matrix(y_step, x_step); + } else { + point.z = 0; + } + point.z += z_offset; + marker.points.push_back(point); } - return marker; + } + return marker; } -std::pair -GaussianMixtureModel::getClosestGaussian(const Eigen::VectorXd& mean) const { - Gaussian closest_gaussian = *std::min_element(gaussian_vec_.begin(), - gaussian_vec_.end(), [&mean](Gaussian a, Gaussian b) { - return a.calcDistance(mean) < b.calcDistance(mean); - }); - return std::make_pair( - closest_gaussian, closest_gaussian.calcDistance(mean)); +std::pair GaussianMixtureModel::getClosestGaussian(const Eigen::VectorXd& mean) const { + Gaussian closest_gaussian = + *std::min_element(gaussian_vec_.begin(), gaussian_vec_.end(), + [&mean](Gaussian a, Gaussian b) { return a.calcDistance(mean) < b.calcDistance(mean); }); + return std::make_pair(closest_gaussian, closest_gaussian.calcDistance(mean)); } } // namespace gmms diff --git a/src/gmm_classifier.cpp b/src/gmm_classifier.cpp index b3264c1..b13ae57 100644 --- a/src/gmm_classifier.cpp +++ b/src/gmm_classifier.cpp @@ -1,9 +1,13 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_classifier.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_classifier.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#include +#include +#include #include #include @@ -12,221 +16,201 @@ #include #include -#include - -#include -#include - namespace gmms { -void GMMClassifier::train(const Eigen::MatrixXd& dataset, - bool evaluate_bic, - int gmm_components) { - int dataset_size = dataset.rows(); - - Eigen::VectorXi labels = dataset.rightCols(1).cast(); - Eigen::MatrixXd training_data = dataset.leftCols(dataset.cols() - 1); - std::set classes(labels.data(), labels.data() + labels.size()); - classes_.clear(); - classes_.insert(classes_.begin(), classes.begin(), classes.end()); - - gmm_vec_.clear(); - gmm_vec_.resize(classes_.size()); - - input_size_ = training_data.cols(); - int class_idx = 0; - - for (std::vector::iterator it = classes_.begin(); it != classes_.end(); - ++it) { - Eigen::MatrixXd class_data( - (labels.array() == *it).count(), training_data.cols()); - - std::cout << "Training class number " << class_idx << ": " << *it - << std::endl; - std::cout << "\t\tClass data size: " << class_data.rows() << std::endl; - - if (class_data.rows() < 10) { - std::cerr << notsufficient_() << std::endl; - gmm_vec_[class_idx] = nullptr; - continue; - } +void GMMClassifier::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int gmm_components) { + int dataset_size = dataset.rows(); - int class_data_idx = 0; + Eigen::VectorXi labels = dataset.rightCols(1).cast(); + Eigen::MatrixXd training_data = dataset.leftCols(dataset.cols() - 1); + std::set classes(labels.data(), labels.data() + labels.size()); + classes_.clear(); + classes_.insert(classes_.begin(), classes.begin(), classes.end()); - for (long data_idx = 0; data_idx < labels.size(); ++data_idx) { - if (labels(data_idx) == *it) { - class_data.row(class_data_idx++) = training_data.row(data_idx); - } - } + gmm_vec_.clear(); + gmm_vec_.resize(classes_.size()); - int num_components; - double bic = 0.0; - bool first_trial = true; + input_size_ = training_data.cols(); + int class_idx = 0; - std::cout << "\t\tEvaluate BIC: "; + for (std::vector::iterator it = classes_.begin(); it != classes_.end(); ++it) { + Eigen::MatrixXd class_data((labels.array() == *it).count(), training_data.cols()); - if (evaluate_bic) { - std::cout << "ON" << std::endl; - num_components = 1; - } else { - std::cout << "OFF" << std::endl; - num_components = gmm_components; - } + std::cout << "Training class number " << class_idx << ": " << *it << std::endl; + std::cout << "\t\tClass data size: " << class_data.rows() << std::endl; - for (int c = num_components; c <= gmm_components; ++c) { - std::cout << "\t\t\t\tUsing " << c << " GMM components" - << std::endl; - - std::shared_ptr model( - new GaussianMixtureModel); - model->setNumComponents(c); - model->initialize(class_data); - model->setNumIterations(max_iterations_); - model->setDelta(delta_); - - try { - model->expectationMaximization(class_data); - } catch (std::runtime_error &e) { - std::cout << "\t\t\t\t" << c << " components are not usable" - << std::endl; - break; - } - - double trial_bic = model->bayesianInformationCriterion(class_data); - std::cout << "\t\t\t\tTrial BIC: " << trial_bic << std::endl; - - if (first_trial || trial_bic < bic) { - bic = trial_bic; - gmm_vec_[class_idx] = model; - - if (first_trial) { - first_trial = false; - } - } - } + if (class_data.rows() < 10) { + std::cerr << notsufficient_() << std::endl; + gmm_vec_[class_idx] = nullptr; + continue; + } - std::cout << "\t\tFinal class model with " - << gmm_vec_[class_idx]->numComponents() << "; "; - std::cout << "BIC " << bic << std::endl; - ++class_idx; + int class_data_idx = 0; + + for (long data_idx = 0; data_idx < labels.size(); ++data_idx) { + if (labels(data_idx) == *it) { + class_data.row(class_data_idx++) = training_data.row(data_idx); + } } - trained_ = true; -} + int num_components; + double bic = 0.0; + bool first_trial = true; -std::vector GMMClassifier::predict(const Eigen::MatrixXd& dataset) const { - if (!trained_) { - throw std::runtime_error(nottrained_()); + std::cout << "\t\tEvaluate BIC: "; + + if (evaluate_bic) { + std::cout << "ON" << std::endl; + num_components = 1; + } else { + std::cout << "OFF" << std::endl; + num_components = gmm_components; } - if (dataset.cols() != input_size_) { - throw std::runtime_error(dimensionality_mismatch_()); + for (int c = num_components; c <= gmm_components; ++c) { + std::cout << "\t\t\t\tUsing " << c << " GMM components" << std::endl; + + std::shared_ptr model(new GaussianMixtureModel); + model->setNumComponents(c); + model->initialize(class_data); + model->setNumIterations(max_iterations_); + model->setDelta(delta_); + + try { + model->expectationMaximization(class_data); + } catch (std::runtime_error& e) { + std::cout << "\t\t\t\t" << c << " components are not usable" << std::endl; + break; + } + + double trial_bic = model->bayesianInformationCriterion(class_data); + std::cout << "\t\t\t\tTrial BIC: " << trial_bic << std::endl; + + if (first_trial || trial_bic < bic) { + bic = trial_bic; + gmm_vec_[class_idx] = model; + + if (first_trial) { + first_trial = false; + } + } } - int dataset_size = dataset.rows(); - std::vector assignments(dataset_size); + std::cout << "\t\tFinal class model with " << gmm_vec_[class_idx]->numComponents() << "; "; + std::cout << "BIC " << bic << std::endl; + ++class_idx; + } + + trained_ = true; +} + +std::vector GMMClassifier::predict(const Eigen::MatrixXd& dataset) const { + if (!trained_) { + throw std::runtime_error(nottrained_()); + } + if (dataset.cols() != input_size_) { + throw std::runtime_error(dimensionality_mismatch_()); + } - int num_threads = omp_get_max_threads(); - int thread_dataset_size = floor(double(dataset_size) / double(num_threads)); + int dataset_size = dataset.rows(); + std::vector assignments(dataset_size); - int thread_log_likelihood[num_threads]; + int num_threads = omp_get_max_threads(); + int thread_dataset_size = floor(double(dataset_size) / double(num_threads)); + + int thread_log_likelihood[num_threads]; #pragma omp parallel for - for (int i = 0; i < dataset_size; ++i) { - bool first = true; - double log_likelihood = 0.0; - - for (size_t k = 0; k < gmm_vec_.size(); ++k) { - if (gmm_vec_[k] == nullptr) { - continue; - } - - double class_log_likelihood = - gmm_vec_[k]->logLikelihood(dataset.row(i)); - - if (first || log_likelihood < class_log_likelihood) { - assignments[i] = classes_[k]; - log_likelihood = class_log_likelihood; - - if (first) { - first = false; - } - } + for (int i = 0; i < dataset_size; ++i) { + bool first = true; + double log_likelihood = 0.0; + + for (size_t k = 0; k < gmm_vec_.size(); ++k) { + if (gmm_vec_[k] == nullptr) { + continue; + } + + double class_log_likelihood = gmm_vec_[k]->logLikelihood(dataset.row(i)); + + if (first || log_likelihood < class_log_likelihood) { + assignments[i] = classes_[k]; + log_likelihood = class_log_likelihood; + + if (first) { + first = false; } + } } + } - return assignments; + return assignments; } void GMMClassifier::load(const std::string filename) { - MatrixIO mio; - Eigen::MatrixXd model; + MatrixIO mio; + Eigen::MatrixXd model; - mio.readFromFile(filename, model); + mio.readFromFile(filename, model); - trained_ = model(0, 0); - delta_ = model(1, 0); - max_iterations_ = model(2, 0); - input_size_ = model(3, 0); + trained_ = model(0, 0); + delta_ = model(1, 0); + max_iterations_ = model(2, 0); + input_size_ = model(3, 0); - classes_.clear(); - gmm_vec_.clear(); - int idx = 4; + classes_.clear(); + gmm_vec_.clear(); + int idx = 4; - while (idx < model.rows() - 1) { - classes_.push_back(model(idx, 0)); - int rows = model(idx + 1, 0); - gmm_vec_.push_back(std::shared_ptr( - new GaussianMixtureModel)); - gmm_vec_[gmm_vec_.size() - 1]->load( - model.block(idx + 2, 0, rows, model.cols())); + while (idx < model.rows() - 1) { + classes_.push_back(model(idx, 0)); + int rows = model(idx + 1, 0); + gmm_vec_.push_back(std::shared_ptr(new GaussianMixtureModel)); + gmm_vec_[gmm_vec_.size() - 1]->load(model.block(idx + 2, 0, rows, model.cols())); - idx += rows + 2; - } + idx += rows + 2; + } } void GMMClassifier::save(const std::string filename) { - MatrixIO mio; - int rows = 0; - int cols = 0; - std::vector gmm_models; - - for (size_t k = 0; k < gmm_vec_.size(); ++k) { - if (gmm_vec_[k] == nullptr) { - continue; - } + MatrixIO mio; + int rows = 0; + int cols = 0; + std::vector gmm_models; + + for (size_t k = 0; k < gmm_vec_.size(); ++k) { + if (gmm_vec_[k] == nullptr) { + continue; + } - gmm_models.push_back(gmm_vec_[k]->save()); - rows += gmm_models[k].rows(); + gmm_models.push_back(gmm_vec_[k]->save()); + rows += gmm_models[k].rows(); - if (cols == 0) { - cols = gmm_models[k].cols(); - } + if (cols == 0) { + cols = gmm_models[k].cols(); } + } - // gmm model + classes + num rows per classes - Eigen::MatrixXd model = - Eigen::MatrixXd::Zero(rows + 2 * classes_.size() + 4, cols); + // gmm model + classes + num rows per classes + Eigen::MatrixXd model = Eigen::MatrixXd::Zero(rows + 2 * classes_.size() + 4, cols); - model(0, 0) = trained_; - model(1, 0) = delta_; - model(2, 0) = max_iterations_; - model(3, 0) = input_size_; + model(0, 0) = trained_; + model(1, 0) = delta_; + model(2, 0) = max_iterations_; + model(3, 0) = input_size_; - int idx = 4; + int idx = 4; - for (size_t k = 0; k < gmm_vec_.size(); ++k) { - if (gmm_vec_[k] == nullptr) { - continue; - } - - model(idx, 0) = classes_[k]; - model(idx + 1, 0) = gmm_models[k].rows(); - model.block(idx + 2, 0, gmm_models[k].rows(), gmm_models[k].cols()) = - gmm_models[k]; - idx += gmm_models[k].rows() + 2; + for (size_t k = 0; k < gmm_vec_.size(); ++k) { + if (gmm_vec_[k] == nullptr) { + continue; } - mio.writeToFile(filename, model); + model(idx, 0) = classes_[k]; + model(idx + 1, 0) = gmm_models[k].rows(); + model.block(idx + 2, 0, gmm_models[k].rows(), gmm_models[k].cols()) = gmm_models[k]; + idx += gmm_models[k].rows() + 2; + } + + mio.writeToFile(filename, model); } } // namespace gmms diff --git a/src/gmm_regressor.cpp b/src/gmm_regressor.cpp index 45be67c..9a1e8cb 100644 --- a/src/gmm_regressor.cpp +++ b/src/gmm_regressor.cpp @@ -1,9 +1,13 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_regressor.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_regressor.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#include +#include +#include #include #include @@ -11,182 +15,162 @@ #include #include -#include - -#include -#include - namespace gmms { -void GMMRegressor::train(const Eigen::MatrixXd& dataset, - bool evaluate_bic, - int gmm_components) { - int num_components; - double bic = 0.0; - bool first_trial = true; - input_size_ = dataset.cols(); - - std::cout << "Training regressor" << std::endl; - std::cout << "\t\tEvaluate BIC: "; - - if (evaluate_bic) { - std::cout << "ON" << std::endl; - num_components = 1; - } else { - std::cout << "OFF" << std::endl; - num_components = gmm_components; +void GMMRegressor::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int gmm_components) { + int num_components; + double bic = 0.0; + bool first_trial = true; + input_size_ = dataset.cols(); + + std::cout << "Training regressor" << std::endl; + std::cout << "\t\tEvaluate BIC: "; + + if (evaluate_bic) { + std::cout << "ON" << std::endl; + num_components = 1; + } else { + std::cout << "OFF" << std::endl; + num_components = gmm_components; + } + + for (int c = num_components; c <= gmm_components; ++c) { + std::cout << "\t\t\t\tUsing " << c << " GMM components" << std::endl; + + std::shared_ptr model(new GaussianMixtureModel); + model->setNumComponents(c); + model->initialize(dataset); + model->setNumIterations(max_iterations_); + model->setDelta(delta_); + + try { + model->expectationMaximization(dataset); + } catch (std::runtime_error& e) { + std::cout << "\t\t\t\t" << c << " components are not usable" << std::endl; + break; } - for (int c = num_components; c <= gmm_components; ++c) { - std::cout << "\t\t\t\tUsing " << c << " GMM components" << std::endl; + double trial_bic = model->bayesianInformationCriterion(dataset); - std::shared_ptr model(new GaussianMixtureModel); - model->setNumComponents(c); - model->initialize(dataset); - model->setNumIterations(max_iterations_); - model->setDelta(delta_); + std::cout << "\t\t\t\tTrial BIC: " << trial_bic << std::endl; - try { - model->expectationMaximization(dataset); - } catch (std::runtime_error &e) { - std::cout << "\t\t\t\t" << c << " components are not usable" - << std::endl; - break; - } + if (first_trial || trial_bic < bic) { + bic = trial_bic; + gmm_ = model; - double trial_bic = model->bayesianInformationCriterion(dataset); - - std::cout << "\t\t\t\tTrial BIC: " << trial_bic << std::endl; - - if (first_trial || trial_bic < bic) { - bic = trial_bic; - gmm_ = model; - - if (first_trial) { - first_trial = false; - } - } + if (first_trial) { + first_trial = false; + } } + } - std::cout << "\t\tFinal model with " << gmm_->numComponents() << "; "; - std::cout << "BIC " << bic << std::endl; + std::cout << "\t\tFinal model with " << gmm_->numComponents() << "; "; + std::cout << "BIC " << bic << std::endl; - trained_ = true; + trained_ = true; } -Eigen::MatrixXd GMMRegressor::predict(const Eigen::MatrixXd& dataset, - const Eigen::VectorXi& output_indices) const { - if (!trained_ || gmm_ == nullptr) { - throw std::runtime_error(nottrained_()); - } +Eigen::MatrixXd GMMRegressor::predict(const Eigen::MatrixXd& dataset, const Eigen::VectorXi& output_indices) const { + if (!trained_ || gmm_ == nullptr) { + throw std::runtime_error(nottrained_()); + } - int query_size = dataset.cols(); - int target_size = output_indices.size(); + int query_size = dataset.cols(); + int target_size = output_indices.size(); - if (query_size + target_size != input_size_) { - throw std::runtime_error(notconsistent_()); - } + if (query_size + target_size != input_size_) { + throw std::runtime_error(notconsistent_()); + } - if (target_size == 0) { - throw std::runtime_error(notvalid_()); - } + if (target_size == 0) { + throw std::runtime_error(notvalid_()); + } - int dataset_size = dataset.rows(); - int num_components = gmm_->numComponents(); - Eigen::VectorXi input_indices(query_size); + int dataset_size = dataset.rows(); + int num_components = gmm_->numComponents(); + Eigen::VectorXi input_indices(query_size); - int idx = 0; - for (int i = 0; i < input_size_; ++i) { - if (!(output_indices.array() == i).any()) { - input_indices(idx++) = i; - } + int idx = 0; + for (int i = 0; i < input_size_; ++i) { + if (!(output_indices.array() == i).any()) { + input_indices(idx++) = i; } + } - Eigen::VectorXi indices(input_size_); - indices.head(query_size) = input_indices; - indices.tail(target_size) = output_indices; - Eigen::PermutationMatrix P = - indices.asPermutation().transpose(); + Eigen::VectorXi indices(input_size_); + indices.head(query_size) = input_indices; + indices.tail(target_size) = output_indices; + Eigen::PermutationMatrix P = indices.asPermutation().transpose(); - Eigen::MatrixXd regressions = - Eigen::MatrixXd::Zero(dataset_size, target_size); + Eigen::MatrixXd regressions = Eigen::MatrixXd::Zero(dataset_size, target_size); - std::vector reduced_gaussians(num_components); - std::vector normalization_factor(dataset_size, 0.0); + std::vector reduced_gaussians(num_components); + std::vector normalization_factor(dataset_size, 0.0); - for (int k = 0; k < num_components; ++k) { - Eigen::VectorXd mean_k = gmm_->component(k).mean(); - Eigen::MatrixXd covariance_k = gmm_->component(k).covariance(); + for (int k = 0; k < num_components; ++k) { + Eigen::VectorXd mean_k = gmm_->component(k).mean(); + Eigen::MatrixXd covariance_k = gmm_->component(k).covariance(); - mean_k = P * mean_k; - covariance_k = P * covariance_k * P.transpose(); + mean_k = P * mean_k; + covariance_k = P * covariance_k * P.transpose(); - Eigen::VectorXd query_mean = mean_k.head(query_size); - Eigen::VectorXd target_mean = mean_k.tail(target_size); - Eigen::MatrixXd query_covariance = - covariance_k.topLeftCorner(query_size, query_size); - Eigen::MatrixXd target_covariance = - covariance_k.bottomRightCorner(target_size, target_size); - Eigen::MatrixXd query_target_covariance = - covariance_k.topRightCorner(query_size, target_size); - Eigen::MatrixXd target_query_covariance = - query_target_covariance.transpose(); - Eigen::MatrixXd inv_query_covariance = query_covariance.inverse(); + Eigen::VectorXd query_mean = mean_k.head(query_size); + Eigen::VectorXd target_mean = mean_k.tail(target_size); + Eigen::MatrixXd query_covariance = covariance_k.topLeftCorner(query_size, query_size); + Eigen::MatrixXd target_covariance = covariance_k.bottomRightCorner(target_size, target_size); + Eigen::MatrixXd query_target_covariance = covariance_k.topRightCorner(query_size, target_size); + Eigen::MatrixXd target_query_covariance = query_target_covariance.transpose(); + Eigen::MatrixXd inv_query_covariance = query_covariance.inverse(); - reduced_gaussians[k].setMeanCovariance(query_mean, query_covariance); + reduced_gaussians[k].setMeanCovariance(query_mean, query_covariance); - Eigen::MatrixXd conditional_covariance = - target_covariance - target_query_covariance * - inv_query_covariance * - query_target_covariance; + Eigen::MatrixXd conditional_covariance = + target_covariance - target_query_covariance * inv_query_covariance * query_target_covariance; #pragma omp parallel for - for (int i = 0; i < dataset_size; ++i) { - Eigen::VectorXd query = dataset.row(i); - double query_probability = - reduced_gaussians[k].evaluate_point(query); - - Eigen::VectorXd tmp(target_size); - tmp = target_mean + target_query_covariance * inv_query_covariance * - (query - query_mean); - tmp *= query_probability; - - regressions.row(i) += tmp; - normalization_factor[i] += query_probability; - - if (k == num_components - 1) { - regressions.row(i) /= normalization_factor[i]; - } - } + for (int i = 0; i < dataset_size; ++i) { + Eigen::VectorXd query = dataset.row(i); + double query_probability = reduced_gaussians[k].evaluate_point(query); + + Eigen::VectorXd tmp(target_size); + tmp = target_mean + target_query_covariance * inv_query_covariance * (query - query_mean); + tmp *= query_probability; + + regressions.row(i) += tmp; + normalization_factor[i] += query_probability; + + if (k == num_components - 1) { + regressions.row(i) /= normalization_factor[i]; + } } + } - return regressions; + return regressions; } void GMMRegressor::load(const std::string filename) { - MatrixIO mio; - Eigen::MatrixXd model; + MatrixIO mio; + Eigen::MatrixXd model; - mio.readFromFile(filename, model); + mio.readFromFile(filename, model); - trained_ = model(0, 0); - delta_ = model(1, 0); - max_iterations_ = model(2, 0); - input_size_ = model(3, 0); - gmm_ = std::shared_ptr(new GaussianMixtureModel); - gmm_->load(model.block(4, 0, model.rows() - 4, model.cols())); + trained_ = model(0, 0); + delta_ = model(1, 0); + max_iterations_ = model(2, 0); + input_size_ = model(3, 0); + gmm_ = std::shared_ptr(new GaussianMixtureModel); + gmm_->load(model.block(4, 0, model.rows() - 4, model.cols())); } void GMMRegressor::save(const std::string filename) { - MatrixIO mio; - Eigen::MatrixXd gmm_model = gmm_->save(); - Eigen::MatrixXd model = - Eigen::MatrixXd::Zero(gmm_model.rows() + 4, gmm_model.cols()); - - model(0, 0) = trained_; - model(1, 0) = delta_; - model(2, 0) = max_iterations_; - model(3, 0) = input_size_; - model.block(4, 0, gmm_model.rows(), gmm_model.cols()) = gmm_model; - mio.writeToFile(filename, model); + MatrixIO mio; + Eigen::MatrixXd gmm_model = gmm_->save(); + Eigen::MatrixXd model = Eigen::MatrixXd::Zero(gmm_model.rows() + 4, gmm_model.cols()); + + model(0, 0) = trained_; + model(1, 0) = delta_; + model(2, 0) = max_iterations_; + model(3, 0) = input_size_; + model.block(4, 0, gmm_model.rows(), gmm_model.cols()) = gmm_model; + mio.writeToFile(filename, model); } } // namespace gmms diff --git a/src/k_means.cpp b/src/k_means.cpp index 4650af6..2faa5d6 100644 --- a/src/k_means.cpp +++ b/src/k_means.cpp @@ -1,126 +1,121 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models k_means.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models k_means.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#include #include #include #include - #include #include #include -#include - namespace gmms { // dataset with each datapoint per row void KMeans::cluster(const Eigen::MatrixXd& dataset) { - if (num_clusters_ == 0) { - throw std::runtime_error("Number of clusters not set"); - } + if (num_clusters_ == 0) { + throw std::runtime_error("Number of clusters not set"); + } - forgyInitialization(dataset); - int iterations = 0; + forgyInitialization(dataset); + int iterations = 0; - do { - assign(dataset); - update(dataset); - } while (!termination_flag_); + do { + assign(dataset); + update(dataset); + } while (!termination_flag_); - computeCovariances(dataset); + computeCovariances(dataset); } void KMeans::forgyInitialization(const Eigen::MatrixXd& dataset) { - srand(time(NULL)); - int dataset_size = dataset.rows(); - assignments_.assign(dataset_size, 0); + srand(time(NULL)); + int dataset_size = dataset.rows(); + assignments_.assign(dataset_size, 0); - std::set assigned_datapoint_idx; + std::set assigned_datapoint_idx; - for (int i = 0; i < num_clusters_; ++i) { - int random_datapoint_idx; + for (int i = 0; i < num_clusters_; ++i) { + int random_datapoint_idx; - do { - random_datapoint_idx = rand() % dataset_size; - } while (assigned_datapoint_idx.find(random_datapoint_idx) != - assigned_datapoint_idx.end()); + do { + random_datapoint_idx = rand() % dataset_size; + } while (assigned_datapoint_idx.find(random_datapoint_idx) != assigned_datapoint_idx.end()); - means_[i] = dataset.row(random_datapoint_idx).transpose(); - assigned_datapoint_idx.insert(random_datapoint_idx); - } + means_[i] = dataset.row(random_datapoint_idx).transpose(); + assigned_datapoint_idx.insert(random_datapoint_idx); + } } void KMeans::assign(const Eigen::MatrixXd& dataset) { - termination_flag_ = true; + termination_flag_ = true; - for (int i = 0; i < dataset.rows(); ++i) { - double min_distance = std::numeric_limits::max(); - int assignment = 0; + for (int i = 0; i < dataset.rows(); ++i) { + double min_distance = std::numeric_limits::max(); + int assignment = 0; - for (int j = 0; j < num_clusters_; ++j) { - double distance = - (dataset.row(i).transpose() - means_[j]).squaredNorm(); + for (int j = 0; j < num_clusters_; ++j) { + double distance = (dataset.row(i).transpose() - means_[j]).squaredNorm(); - if (distance < min_distance) { - min_distance = distance; - assignment = j; - } - } + if (distance < min_distance) { + min_distance = distance; + assignment = j; + } + } - if (assignments_[i] != assignment) { - assignments_[i] = assignment; - termination_flag_ = false; - } + if (assignments_[i] != assignment) { + assignments_[i] = assignment; + termination_flag_ = false; } + } } void KMeans::update(const Eigen::MatrixXd& dataset) { - int dataset_size = dataset.rows(); - - for (int i = 0; i < num_clusters_; ++i) { - Eigen::VectorXd point_sum; - cluster_cardinalities_[i] = 0; - - for (int j = 0; j < dataset_size; ++j) { - if (assignments_[j] == i) { - if (cluster_cardinalities_[i] == 0) { - point_sum = dataset.row(j).transpose(); - } else { - point_sum += dataset.row(j).transpose(); - } - - ++cluster_cardinalities_[i]; - } - } + int dataset_size = dataset.rows(); + for (int i = 0; i < num_clusters_; ++i) { + Eigen::VectorXd point_sum; + cluster_cardinalities_[i] = 0; + + for (int j = 0; j < dataset_size; ++j) { + if (assignments_[j] == i) { if (cluster_cardinalities_[i] == 0) { - int random_datapoint_idx = rand() % dataset_size; - assignments_[random_datapoint_idx] = i; - point_sum = dataset.row(random_datapoint_idx).transpose(); - cluster_cardinalities_[i] = 1; + point_sum = dataset.row(j).transpose(); + } else { + point_sum += dataset.row(j).transpose(); } - means_[i] = point_sum / cluster_cardinalities_[i]; + ++cluster_cardinalities_[i]; + } + } + + if (cluster_cardinalities_[i] == 0) { + int random_datapoint_idx = rand() % dataset_size; + assignments_[random_datapoint_idx] = i; + point_sum = dataset.row(random_datapoint_idx).transpose(); + cluster_cardinalities_[i] = 1; } + + means_[i] = point_sum / cluster_cardinalities_[i]; + } } void KMeans::computeCovariances(const Eigen::MatrixXd& dataset) { - for (int i = 0; i < num_clusters_; ++i) { - covariances_[i].setZero(dataset.cols(), dataset.cols()); - - for (int j = 0; j < dataset.rows(); ++j) { - if (assignments_[j] == i) { - Eigen::VectorXd distance_vector = - dataset.row(j).transpose() - means_[i]; - covariances_[i] += - distance_vector * distance_vector.transpose(); - } - } - - covariances_[i] = covariances_[i] / (cluster_cardinalities_[i]); + for (int i = 0; i < num_clusters_; ++i) { + covariances_[i].setZero(dataset.cols(), dataset.cols()); + + for (int j = 0; j < dataset.rows(); ++j) { + if (assignments_[j] == i) { + Eigen::VectorXd distance_vector = dataset.row(j).transpose() - means_[i]; + covariances_[i] += distance_vector * distance_vector.transpose(); + } } + + covariances_[i] = covariances_[i] / (cluster_cardinalities_[i]); + } } } // namespace gmms diff --git a/src/matrix_io.cpp b/src/matrix_io.cpp index 0352b28..81f3348 100644 --- a/src/matrix_io.cpp +++ b/src/matrix_io.cpp @@ -1,83 +1,80 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models matrix_io.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models matrix_io.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#include #include #include #include #include -#include +void MatrixIO::readFromFile(const std::string filename, Eigen::MatrixXd& matrix) { + std::ifstream input_file; + std::string line; + int rows = 0; + int cols = 0; -void MatrixIO::readFromFile(const std::string filename, - Eigen::MatrixXd& matrix) { - std::ifstream input_file; - std::string line; - int rows = 0; - int cols = 0; + input_file.open(filename); - input_file.open(filename); + if (input_file.fail()) { + throw std::runtime_error("Impossible to open the file"); + } - if (input_file.fail()) { - throw std::runtime_error("Impossible to open the file"); - } + int row_idx = 0; - int row_idx = 0; + while (!input_file.fail() && !input_file.eof()) { + std::getline(input_file, line); + std::istringstream iss(line); + std::string buffer; - while (!input_file.fail() && !input_file.eof()) { - std::getline(input_file, line); - std::istringstream iss(line); - std::string buffer; + iss >> buffer; + iss.clear(); + iss.seekg(0, std::ios::beg); - iss >> buffer; - iss.clear(); - iss.seekg(0, std::ios::beg); - - if (buffer.empty() || (buffer[0] == '#' && buffer != "#size:")) { - continue; - } + if (buffer.empty() || (buffer[0] == '#' && buffer != "#size:")) { + continue; + } - if (buffer == "#size:" && rows == 0 && cols == 0) { - std::string str; - iss >> str; - iss >> rows; - iss >> cols; - matrix = Eigen::MatrixXd::Zero(rows, cols); - row_idx = 0; - } else { - double value; + if (buffer == "#size:" && rows == 0 && cols == 0) { + std::string str; + iss >> str; + iss >> rows; + iss >> cols; + matrix = Eigen::MatrixXd::Zero(rows, cols); + row_idx = 0; + } else { + double value; - for (int i = 0; i < cols && !iss.eof(); ++i) { - iss >> value; + for (int i = 0; i < cols && !iss.eof(); ++i) { + iss >> value; - matrix(row_idx, i) = value; - } + matrix(row_idx, i) = value; + } - ++row_idx; - } + ++row_idx; } + } - input_file.close(); + input_file.close(); } -void MatrixIO::writeToFile(const std::string filename, - const Eigen::MatrixXd& matrix) { - std::ofstream output_file; +void MatrixIO::writeToFile(const std::string filename, const Eigen::MatrixXd& matrix) { + std::ofstream output_file; - output_file.open(filename); + output_file.open(filename); - if (output_file.fail()) { - throw std::runtime_error("Impossible to open the file"); - } + if (output_file.fail()) { + throw std::runtime_error("Impossible to open the file"); + } - output_file << "#file automatically generated" << std::endl; - output_file << "#size: " << matrix.rows() << " " << matrix.cols() - << std::endl; + output_file << "#file automatically generated" << std::endl; + output_file << "#size: " << matrix.rows() << " " << matrix.cols() << std::endl; - output_file << matrix << std::endl; + output_file << matrix << std::endl; - output_file.close(); + output_file.close(); } diff --git a/src/pca.cpp b/src/pca.cpp index 57c6fca..6a94cef 100644 --- a/src/pca.cpp +++ b/src/pca.cpp @@ -1,42 +1,37 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models pca.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models pca.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// -#include +#include #include #include - -#include +#include namespace gmms { -Eigen::MatrixXd -PCA::pca(const Eigen::MatrixXd& dataset, double& retained_variance) { - if (num_components_ > dataset.cols()) { - throw std::runtime_error( - "Number of components greater than dataset size"); - } +Eigen::MatrixXd PCA::pca(const Eigen::MatrixXd& dataset, double& retained_variance) { + if (num_components_ > dataset.cols()) { + throw std::runtime_error("Number of components greater than dataset size"); + } - Eigen::MatrixXd centered = dataset.rowwise() - dataset.colwise().mean(); + Eigen::MatrixXd centered = dataset.rowwise() - dataset.colwise().mean(); - Eigen::RowVectorXd maxValues = centered.colwise().maxCoeff(); - Eigen::RowVectorXd minValues = centered.colwise().minCoeff(); - Eigen::MatrixXd normalized_dataset = - centered.array().rowwise() / (maxValues - minValues).array(); + Eigen::RowVectorXd maxValues = centered.colwise().maxCoeff(); + Eigen::RowVectorXd minValues = centered.colwise().minCoeff(); + Eigen::MatrixXd normalized_dataset = centered.array().rowwise() / (maxValues - minValues).array(); - Eigen::JacobiSVD svd( - normalized_dataset, Eigen::ComputeThinV); + Eigen::JacobiSVD svd(normalized_dataset, Eigen::ComputeThinV); - Eigen::VectorXd S = svd.singularValues(); - Eigen::MatrixXd W = svd.matrixV().leftCols(num_components_); + Eigen::VectorXd S = svd.singularValues(); + Eigen::MatrixXd W = svd.matrixV().leftCols(num_components_); - retained_variance = S.head(num_components_).sum() / S.sum(); + retained_variance = S.head(num_components_).sum() / S.sum(); - Eigen::MatrixXd projected = normalized_dataset * W; + Eigen::MatrixXd projected = normalized_dataset * W; - return projected; + return projected; } } // namespace gmms From 055c124c6e4439747e439ed21d10f0b4e87bfc57 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 4 Jan 2025 19:57:23 +0000 Subject: [PATCH 3/7] Git blame ignore formatting --- .git-blame-ignore-revs | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .git-blame-ignore-revs diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs new file mode 100644 index 0000000..2f260d1 --- /dev/null +++ b/.git-blame-ignore-revs @@ -0,0 +1,2 @@ +# Add git commit hashes to ignore for blame +f723d2814904a75a4f59f30bc401f6271bf800ec From de8c70225e47bd21b6b1125ca5cb8f3352f03967 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 4 Jan 2025 20:16:57 +0000 Subject: [PATCH 4/7] Update pre-commit config and format CMakeLists.txt --- .pre-commit-config.yaml | 57 +++++++++++++++++++++++++---------------- CMakeLists.txt | 46 ++++++++++++++++++--------------- 2 files changed, 61 insertions(+), 42 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 561c3bd..06f738c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,23 +1,36 @@ repos: -- repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.1.6 - hooks: - - id: ruff - args: - - "--fix" - - "--exit-non-zero-on-fix" -- repo: https://github.com/psf/black - rev: 23.11.0 - hooks: - - id: black -- repo: https://github.com/pocc/pre-commit-hooks - rev: v1.3.5 - hooks: - - id: clang-format - args: - - "-i" - - id: cppcheck - args: - - "--suppress=missingInclude" - - "--suppress=unmatchedSuppression" - - "--suppress=unusedFunction" + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.1.6 + hooks: + - id: ruff + args: + - "--fix" + - "--exit-non-zero-on-fix" + - id: ruff-format + - repo: https://github.com/pocc/pre-commit-hooks + rev: v1.3.5 + hooks: + - id: clang-format + args: + - "-i" + - id: cppcheck + args: + - "--inline-suppr" + - "--suppress=missingIncludeSystem" + - "--suppress=unmatchedSuppression" + - "--suppress=unusedFunction" + - "--suppress=unusedStructMember" + - "--suppress=useStlAlgorithm" + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.10 + hooks: + - id: cmake-format + - id: cmake-lint + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.5.0 + hooks: + - id: check-merge-conflict + - id: check-toml + - id: check-xml + - id: check-yaml + - id: detect-private-key diff --git a/CMakeLists.txt b/CMakeLists.txt index c688c3c..2cb3520 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,9 +34,9 @@ if(OPENMP_FOUND) set(OPENMP_DLL VCOMP100) endif(MSVC90) set(CMAKE_SHARED_LINKER_FLAGS_DEBUG - "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll") + "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} /DELAYLOAD:${OPENMP_DLL}D.dll") set(CMAKE_SHARED_LINKER_FLAGS_RELEASE - "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll") + "${CMAKE_SHARED_LINKER_FLAGS_RELEASE} /DELAYLOAD:${OPENMP_DLL}.dll") endif(MSVC90 OR MSVC10) else(OPENMP_FOUND) @@ -54,29 +54,34 @@ endif(MSVC) # set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}") -set(SOURCES src/CRandomNumberGenerator.cpp src/gaussian.cpp - src/gaussian_mixture_model.cpp src/gmm_classifier.cpp src/gmm_regressor.cpp - src/k_means.cpp src/matrix_io.cpp src/pca.cpp) +set(SOURCES + src/CRandomNumberGenerator.cpp + src/gaussian.cpp + src/gaussian_mixture_model.cpp + src/gmm_classifier.cpp + src/gmm_regressor.cpp + src/k_means.cpp + src/matrix_io.cpp + src/pca.cpp) set(CODE_LIBRARIES) add_library(${PROJECT_NAME} ${SOURCES}) -ament_target_dependencies(${PROJECT_NAME} SYSTEM - Eigen3 - geometry_msgs - rclcpp - std_msgs - visualization_msgs) +ament_target_dependencies( + ${PROJECT_NAME} + SYSTEM + Eigen3 + geometry_msgs + rclcpp + std_msgs + visualization_msgs) target_link_libraries(${PROJECT_NAME} ${LIBS}) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME}) -install(TARGETS ${PROJECT_NAME} - LIBRARY - DESTINATION lib) +install(TARGETS ${PROJECT_NAME} LIBRARY DESTINATION lib) ament_export_dependencies(std_msgs) ament_export_dependencies(geometry_msgs) @@ -87,12 +92,13 @@ ament_export_dependencies(visualization_msgs) ament_export_include_directories(${INCLUDE_DIRS}) ament_export_libraries(${PROJECT_NAME}) -install(DIRECTORY include/ - DESTINATION include) +install(DIRECTORY include/ DESTINATION include) -install(TARGETS ${PROJECT_NAME} +install( + TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} LIBRARY DESTINATION lib - INCLUDES DESTINATION include) + INCLUDES + DESTINATION include) ament_package() From d1a31a1a1f07f95b4526b5cc9abf09a5a9bab180 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 4 Jan 2025 20:51:18 +0000 Subject: [PATCH 5/7] Update .h files to new .hpp style and reapply formatting --- ...Generator.h => CRandomNumberGenerator.hpp} | 8 +++---- ...leWeights.h => CompareParticleWeights.hpp} | 8 +++---- ...eResampling.h => ImportanceResampling.hpp} | 13 +++++----- .../{MovementModel.h => MovementModel.hpp} | 6 ++--- ...bservationModel.h => ObservationModel.hpp} | 6 ++--- .../{Particle.h => Particle.hpp} | 8 +++---- .../{ParticleFilter.h => ParticleFilter.hpp} | 24 +++++++++---------- ...y.h => RandomNumberGenerationStrategy.hpp} | 6 ++--- ...plingStrategy.h => ResamplingStrategy.hpp} | 9 ++++--- ...teDistribution.h => StateDistribution.hpp} | 7 +++--- include/particle_filter/{defs.h => defs.hpp} | 6 ++--- .../{gaussian.h => gaussian.hpp} | 8 +++---- ...ure_model.h => gaussian_mixture_model.hpp} | 13 +++++----- .../{gmm_classifier.h => gmm_classifier.hpp} | 9 ++++--- .../{gmm_regressor.h => gmm_regressor.hpp} | 9 ++++--- .../{k_means.h => k_means.hpp} | 6 ++--- .../{matrix_io.h => matrix_io.hpp} | 6 ++--- include/particle_filter/{pca.h => pca.hpp} | 6 ++--- src/CRandomNumberGenerator.cpp | 3 +-- src/gaussian.cpp | 3 +-- src/gaussian_mixture_model.cpp | 2 +- src/gmm_classifier.cpp | 4 ++-- src/gmm_regressor.cpp | 4 ++-- src/k_means.cpp | 3 +-- src/matrix_io.cpp | 3 +-- src/pca.cpp | 2 +- 26 files changed, 87 insertions(+), 95 deletions(-) rename include/particle_filter/{CRandomNumberGenerator.h => CRandomNumberGenerator.hpp} (90%) rename include/particle_filter/{CompareParticleWeights.h => CompareParticleWeights.hpp} (85%) rename include/particle_filter/{ImportanceResampling.h => ImportanceResampling.hpp} (92%) rename include/particle_filter/{MovementModel.h => MovementModel.hpp} (95%) rename include/particle_filter/{ObservationModel.h => ObservationModel.hpp} (94%) rename include/particle_filter/{Particle.h => Particle.hpp} (96%) rename include/particle_filter/{ParticleFilter.h => ParticleFilter.hpp} (97%) rename include/particle_filter/{RandomNumberGenerationStrategy.h => RandomNumberGenerationStrategy.hpp} (90%) rename include/particle_filter/{ResamplingStrategy.h => ResamplingStrategy.hpp} (91%) rename include/particle_filter/{StateDistribution.h => StateDistribution.hpp} (93%) rename include/particle_filter/{defs.h => defs.hpp} (87%) rename include/particle_filter/{gaussian.h => gaussian.hpp} (94%) rename include/particle_filter/{gaussian_mixture_model.h => gaussian_mixture_model.hpp} (96%) rename include/particle_filter/{gmm_classifier.h => gmm_classifier.hpp} (92%) rename include/particle_filter/{gmm_regressor.h => gmm_regressor.hpp} (94%) rename include/particle_filter/{k_means.h => k_means.hpp} (96%) rename include/particle_filter/{matrix_io.h => matrix_io.hpp} (89%) rename include/particle_filter/{pca.h => pca.hpp} (95%) diff --git a/include/particle_filter/CRandomNumberGenerator.h b/include/particle_filter/CRandomNumberGenerator.hpp similarity index 90% rename from include/particle_filter/CRandomNumberGenerator.h rename to include/particle_filter/CRandomNumberGenerator.hpp index f077297..585204a 100644 --- a/include/particle_filter/CRandomNumberGenerator.h +++ b/include/particle_filter/CRandomNumberGenerator.hpp @@ -1,7 +1,7 @@ -#ifndef RANDOMNUMBERGENERATOR_H -#define RANDOMNUMBERGENERATOR_H +#ifndef RANDOMNUMBERGENERATOR_HPP +#define RANDOMNUMBERGENERATOR_HPP -#include "particle_filter/RandomNumberGenerationStrategy.h" +#include namespace particle_filter { @@ -62,4 +62,4 @@ class CRandomNumberGenerator : public RandomNumberGenerationStrategy { } // namespace particle_filter -#endif // RANDOMNUMBERGENERATOR_H +#endif // RANDOMNUMBERGENERATOR_HPP diff --git a/include/particle_filter/CompareParticleWeights.h b/include/particle_filter/CompareParticleWeights.hpp similarity index 85% rename from include/particle_filter/CompareParticleWeights.h rename to include/particle_filter/CompareParticleWeights.hpp index 3afb77b..aeac543 100644 --- a/include/particle_filter/CompareParticleWeights.h +++ b/include/particle_filter/CompareParticleWeights.hpp @@ -1,7 +1,7 @@ -#ifndef COMPAREPARTICLEWEIGHTS_H -#define COMPAREPARTICLEWEIGHTS_H +#ifndef COMPAREPARTICLEWEIGHTS_HPP +#define COMPAREPARTICLEWEIGHTS_HPP -#include "particle_filter/Particle.h" +#include namespace particle_filter { @@ -34,4 +34,4 @@ class CompareParticleWeights { } // namespace particle_filter -#endif +#endif // COMPAREPARTICLEWEIGHTS_HPP diff --git a/include/particle_filter/ImportanceResampling.h b/include/particle_filter/ImportanceResampling.hpp similarity index 92% rename from include/particle_filter/ImportanceResampling.h rename to include/particle_filter/ImportanceResampling.hpp index e8bf77b..5452c03 100644 --- a/include/particle_filter/ImportanceResampling.h +++ b/include/particle_filter/ImportanceResampling.hpp @@ -1,10 +1,9 @@ -#ifndef IMPORTANCERESAMPLING_H -#define IMPORTANCERESAMPLING_H +#ifndef IMPORTANCERESAMPLING_HPP +#define IMPORTANCERESAMPLING_HPP -#include - -#include "particle_filter/CRandomNumberGenerator.h" -#include "particle_filter/Particle.h" +#include +#include +#include namespace particle_filter { @@ -96,4 +95,4 @@ void ImportanceResampling::resample(const ParticleList& sourceList, } } } // namespace particle_filter -#endif // IMPORTANCERESAMPLING_H +#endif // IMPORTANCERESAMPLING_HPP diff --git a/include/particle_filter/MovementModel.h b/include/particle_filter/MovementModel.hpp similarity index 95% rename from include/particle_filter/MovementModel.h rename to include/particle_filter/MovementModel.hpp index 7d48eb3..92d7e19 100644 --- a/include/particle_filter/MovementModel.h +++ b/include/particle_filter/MovementModel.hpp @@ -1,5 +1,5 @@ -#ifndef MOVEMENTSTRATEGY_H -#define MOVEMENTSTRATEGY_H +#ifndef MOVEMENTSTRATEGY_HPP +#define MOVEMENTSTRATEGY_HPP #include #include @@ -63,4 +63,4 @@ template MovementModel::~MovementModel() {} } // namespace particle_filter -#endif +#endif // MOVEMENTSTRATEGY_HPP diff --git a/include/particle_filter/ObservationModel.h b/include/particle_filter/ObservationModel.hpp similarity index 94% rename from include/particle_filter/ObservationModel.h rename to include/particle_filter/ObservationModel.hpp index f89045b..6c64b55 100644 --- a/include/particle_filter/ObservationModel.h +++ b/include/particle_filter/ObservationModel.hpp @@ -1,5 +1,5 @@ -#ifndef OBSERVATIONSTRATEGY_H -#define OBSERVATIONSTRATEGY_H +#ifndef OBSERVATIONSTRATEGY_HPP +#define OBSERVATIONSTRATEGY_HPP #include @@ -57,4 +57,4 @@ template ObservationModel::~ObservationModel() {} } // namespace particle_filter -#endif +#endif // OBSERVATIONSTRATEGY_HPP diff --git a/include/particle_filter/Particle.h b/include/particle_filter/Particle.hpp similarity index 96% rename from include/particle_filter/Particle.h rename to include/particle_filter/Particle.hpp index 6f2fae6..9444c5e 100644 --- a/include/particle_filter/Particle.h +++ b/include/particle_filter/Particle.hpp @@ -1,7 +1,7 @@ -#ifndef PARTICLE_H -#define PARTICLE_H +#ifndef PARTICLE_HPP +#define PARTICLE_HPP -#include "particle_filter/ParticleFilter.h" +#include namespace particle_filter { /** @@ -124,4 +124,4 @@ void Particle::setNormalization(double normalization) { } } // namespace particle_filter -#endif +#endif // PARTICLE_HPP diff --git a/include/particle_filter/ParticleFilter.h b/include/particle_filter/ParticleFilter.hpp similarity index 97% rename from include/particle_filter/ParticleFilter.h rename to include/particle_filter/ParticleFilter.hpp index 35ae553..d50c8a5 100644 --- a/include/particle_filter/ParticleFilter.h +++ b/include/particle_filter/ParticleFilter.hpp @@ -1,15 +1,7 @@ -#ifndef PARTICLEFILTER_H -#define PARTICLEFILTER_H +#ifndef PARTICLEFILTER_HPP +#define PARTICLEFILTER_HPP #include -#include -#include -#include -#include -#include -#include -#include -#include #include #include @@ -18,6 +10,14 @@ #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -504,6 +504,6 @@ class ParticleFilter { } // namespace particle_filter // include template implementation -#include "ParticleFilter.hxx" +#include -#endif +#endif // PARTICLEFILTER_HPP diff --git a/include/particle_filter/RandomNumberGenerationStrategy.h b/include/particle_filter/RandomNumberGenerationStrategy.hpp similarity index 90% rename from include/particle_filter/RandomNumberGenerationStrategy.h rename to include/particle_filter/RandomNumberGenerationStrategy.hpp index 96264e7..4460fda 100644 --- a/include/particle_filter/RandomNumberGenerationStrategy.h +++ b/include/particle_filter/RandomNumberGenerationStrategy.hpp @@ -1,5 +1,5 @@ -#ifndef RANDOMNUMBERGENERATIONSTRATEGY_H -#define RANDOMNUMBERGENERATIONSTRATEGY_H +#ifndef RANDOMNUMBERGENERATIONSTRATEGY_HPP +#define RANDOMNUMBERGENERATIONSTRATEGY_HPP namespace particle_filter { @@ -48,4 +48,4 @@ class RandomNumberGenerationStrategy { } // namespace particle_filter -#endif // RANDOMNUMBERGENERATIONSTRATEGY_H +#endif // RANDOMNUMBERGENERATIONSTRATEGY_HPP diff --git a/include/particle_filter/ResamplingStrategy.h b/include/particle_filter/ResamplingStrategy.hpp similarity index 91% rename from include/particle_filter/ResamplingStrategy.h rename to include/particle_filter/ResamplingStrategy.hpp index 973cb33..7aba8a2 100644 --- a/include/particle_filter/ResamplingStrategy.h +++ b/include/particle_filter/ResamplingStrategy.hpp @@ -1,8 +1,7 @@ -#ifndef RESAMPLINGSTRATEGY_H -#define RESAMPLINGSTRATEGY_H - -#include +#ifndef RESAMPLINGSTRATEGY_HPP +#define RESAMPLINGSTRATEGY_HPP +#include #include namespace particle_filter { @@ -54,4 +53,4 @@ template ResamplingStrategy::~ResamplingStrategy() {} } // namespace particle_filter -#endif // RESAMPLINGSTRATEGY_H +#endif // RESAMPLINGSTRATEGY_HPP diff --git a/include/particle_filter/StateDistribution.h b/include/particle_filter/StateDistribution.hpp similarity index 93% rename from include/particle_filter/StateDistribution.h rename to include/particle_filter/StateDistribution.hpp index 97e7ac9..2700b2c 100644 --- a/include/particle_filter/StateDistribution.h +++ b/include/particle_filter/StateDistribution.hpp @@ -1,5 +1,6 @@ -#ifndef STATEDISTRIBUTION_H -#define STATEDISTRIBUTION_H +#ifndef STATEDISTRIBUTION_HPP +#define STATEDISTRIBUTION_HPP + namespace particle_filter { /** @@ -53,4 +54,4 @@ StateDistribution::~StateDistribution() {} } // namespace particle_filter -#endif // STATEDISTRIBUTION_H +#endif // STATEDISTRIBUTION_HPP diff --git a/include/particle_filter/defs.h b/include/particle_filter/defs.hpp similarity index 87% rename from include/particle_filter/defs.h rename to include/particle_filter/defs.hpp index 76f6803..d09465b 100644 --- a/include/particle_filter/defs.h +++ b/include/particle_filter/defs.hpp @@ -5,9 +5,9 @@ // # #// // #########################################################// -#ifndef _DEFS_H_ -#define _DEFS_H_ +#ifndef _DEFS_HPP_ +#define _DEFS_HPP_ #define M_EPS (1E-7) -#endif // _DEFS_H_ +#endif // _DEFS_HPP_ diff --git a/include/particle_filter/gaussian.h b/include/particle_filter/gaussian.hpp similarity index 94% rename from include/particle_filter/gaussian.h rename to include/particle_filter/gaussian.hpp index 06f12da..7fdd184 100644 --- a/include/particle_filter/gaussian.h +++ b/include/particle_filter/gaussian.hpp @@ -5,17 +5,17 @@ // # #// // #########################################################// -#ifndef _GAUSSIAN_H_ -#define _GAUSSIAN_H_ +#ifndef _GAUSSIAN_HPP_ +#define _GAUSSIAN_HPP_ #include -#include #include #include #include #include #include +#include #include #include @@ -60,4 +60,4 @@ class Gaussian { }; } // namespace gmms -#endif // _GAUSSIAN_H_ +#endif // _GAUSSIAN_HPP_ diff --git a/include/particle_filter/gaussian_mixture_model.h b/include/particle_filter/gaussian_mixture_model.hpp similarity index 96% rename from include/particle_filter/gaussian_mixture_model.h rename to include/particle_filter/gaussian_mixture_model.hpp index a4bc66e..9c2f899 100644 --- a/include/particle_filter/gaussian_mixture_model.h +++ b/include/particle_filter/gaussian_mixture_model.hpp @@ -5,16 +5,15 @@ // # #// // #########################################################// -#ifndef _GAUSSIAN_MIXTURE_MODEL_H_ -#define _GAUSSIAN_MIXTURE_MODEL_H_ - -#include -#include -#include +#ifndef _GAUSSIAN_MIXTURE_MODEL_HPP_ +#define _GAUSSIAN_MIXTURE_MODEL_HPP_ #include #include #include +#include +#include +#include #include #include #include @@ -170,4 +169,4 @@ class GaussianMixtureModel { }; } // namespace gmms -#endif // _GAUSSIAN_MIXTURE_MODEL_H_ +#endif // _GAUSSIAN_MIXTURE_MODEL_HPP_ diff --git a/include/particle_filter/gmm_classifier.h b/include/particle_filter/gmm_classifier.hpp similarity index 92% rename from include/particle_filter/gmm_classifier.h rename to include/particle_filter/gmm_classifier.hpp index 23a334f..2e89449 100644 --- a/include/particle_filter/gmm_classifier.h +++ b/include/particle_filter/gmm_classifier.hpp @@ -5,13 +5,12 @@ // # #// // #########################################################// -#ifndef _GMM_CLASSIFIER_H_ -#define _GMM_CLASSIFIER_H_ - -#include +#ifndef _GMM_CLASSIFIER_HPP_ +#define _GMM_CLASSIFIER_HPP_ #include #include +#include #include namespace gmms { @@ -57,4 +56,4 @@ class GMMClassifier { }; } // namespace gmms -#endif // _GMM_CLASSIFIER_H_ +#endif // _GMM_CLASSIFIER_HPP_ diff --git a/include/particle_filter/gmm_regressor.h b/include/particle_filter/gmm_regressor.hpp similarity index 94% rename from include/particle_filter/gmm_regressor.h rename to include/particle_filter/gmm_regressor.hpp index f82db80..61b75fd 100644 --- a/include/particle_filter/gmm_regressor.h +++ b/include/particle_filter/gmm_regressor.hpp @@ -5,13 +5,12 @@ // # #// // #########################################################// -#ifndef _GMM_REGRESSOR_H_ -#define _GMM_REGRESSOR_H_ - -#include +#ifndef _GMM_REGRESSOR_HPP_ +#define _GMM_REGRESSOR_HPP_ #include #include +#include #include #include @@ -66,4 +65,4 @@ class GMMRegressor { }; } // namespace gmms -#endif // _GMM_REGRESSOR_H_ +#endif // _GMM_REGRESSOR_HPP_ diff --git a/include/particle_filter/k_means.h b/include/particle_filter/k_means.hpp similarity index 96% rename from include/particle_filter/k_means.h rename to include/particle_filter/k_means.hpp index 88c122e..dd4edd2 100644 --- a/include/particle_filter/k_means.h +++ b/include/particle_filter/k_means.hpp @@ -5,8 +5,8 @@ // # #// // #########################################################// -#ifndef _K_MEANS_H_ -#define _K_MEANS_H_ +#ifndef _K_MEANS_HPP_ +#define _K_MEANS_HPP_ #include #include @@ -55,4 +55,4 @@ class KMeans { }; } // namespace gmms -#endif // _K_MEANS_H_ +#endif // _K_MEANS_HPP_ diff --git a/include/particle_filter/matrix_io.h b/include/particle_filter/matrix_io.hpp similarity index 89% rename from include/particle_filter/matrix_io.h rename to include/particle_filter/matrix_io.hpp index ab2fde0..0025fcb 100644 --- a/include/particle_filter/matrix_io.h +++ b/include/particle_filter/matrix_io.hpp @@ -5,8 +5,8 @@ // # #// // #########################################################// -#ifndef _MATRIX_IO_H_ -#define _MATRIX_IO_H_ +#ifndef _MATRIX_IO_HPP_ +#define _MATRIX_IO_HPP_ #include #include @@ -20,4 +20,4 @@ class MatrixIO { void writeToFile(const std::string filename, const Eigen::MatrixXd& matrix); }; -#endif // _MATRIX_IO_H_ +#endif // _MATRIX_IO_HPP_ diff --git a/include/particle_filter/pca.h b/include/particle_filter/pca.hpp similarity index 95% rename from include/particle_filter/pca.h rename to include/particle_filter/pca.hpp index 7688ee6..84dac50 100644 --- a/include/particle_filter/pca.h +++ b/include/particle_filter/pca.hpp @@ -5,8 +5,8 @@ // # #// // #########################################################// -#ifndef _PCA_H_ -#define _PCA_H_ +#ifndef _PCA_HPP_ +#define _PCA_HPP_ #include #include @@ -35,4 +35,4 @@ class PCA { }; } // namespace gmms -#endif // _PCA_H_ +#endif // _PCA_HPP_ diff --git a/src/CRandomNumberGenerator.cpp b/src/CRandomNumberGenerator.cpp index ad219d8..5bcbc17 100644 --- a/src/CRandomNumberGenerator.cpp +++ b/src/CRandomNumberGenerator.cpp @@ -1,8 +1,7 @@ -#include - #include #include #include +#include using namespace particle_filter; diff --git a/src/gaussian.cpp b/src/gaussian.cpp index 8dc81b0..0e30d53 100644 --- a/src/gaussian.cpp +++ b/src/gaussian.cpp @@ -5,10 +5,9 @@ // # #// // #########################################################// -#include - #include #include +#include namespace gmms { double Gaussian::evaluate_point(const Eigen::VectorXd& pt) const { diff --git a/src/gaussian_mixture_model.cpp b/src/gaussian_mixture_model.cpp index 7277167..27bf582 100644 --- a/src/gaussian_mixture_model.cpp +++ b/src/gaussian_mixture_model.cpp @@ -12,11 +12,11 @@ // #########################################################// #include -#include #include #include #include +#include namespace gmms { void GaussianMixtureModel::initialize(const Eigen::MatrixXd& dataset) { diff --git a/src/gmm_classifier.cpp b/src/gmm_classifier.cpp index b13ae57..c3148cb 100644 --- a/src/gmm_classifier.cpp +++ b/src/gmm_classifier.cpp @@ -6,12 +6,12 @@ // #########################################################// #include -#include -#include #include #include #include +#include +#include #include #include #include diff --git a/src/gmm_regressor.cpp b/src/gmm_regressor.cpp index 9a1e8cb..af5f10e 100644 --- a/src/gmm_regressor.cpp +++ b/src/gmm_regressor.cpp @@ -6,11 +6,11 @@ // #########################################################// #include -#include -#include #include #include +#include +#include #include #include #include diff --git a/src/k_means.cpp b/src/k_means.cpp index 2faa5d6..9e95208 100644 --- a/src/k_means.cpp +++ b/src/k_means.cpp @@ -5,12 +5,11 @@ // # #// // #########################################################// -#include - #include #include #include #include +#include #include #include diff --git a/src/matrix_io.cpp b/src/matrix_io.cpp index 81f3348..ea8b407 100644 --- a/src/matrix_io.cpp +++ b/src/matrix_io.cpp @@ -5,10 +5,9 @@ // # #// // #########################################################// -#include - #include #include +#include #include #include diff --git a/src/pca.cpp b/src/pca.cpp index 6a94cef..47f028a 100644 --- a/src/pca.cpp +++ b/src/pca.cpp @@ -5,7 +5,7 @@ // # #// // #########################################################// -#include +#include pp #include #include From 5c62618429fc221e5b4684d6c0c8ca47728ff7b4 Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 4 Jan 2025 22:24:24 +0000 Subject: [PATCH 6/7] Fix CppCheck --- .../particle_filter/ImportanceResampling.hpp | 2 +- include/particle_filter/ParticleFilter.hpp | 2 +- include/particle_filter/ParticleFilter.hxx | 2 +- include/particle_filter/gaussian.hpp | 5 +++- .../gaussian_mixture_model.hpp | 23 +++++++++---------- include/particle_filter/gmm_classifier.hpp | 6 +++-- include/particle_filter/gmm_regressor.hpp | 6 +++-- include/particle_filter/k_means.hpp | 2 +- include/particle_filter/matrix_io.hpp | 4 ++-- include/particle_filter/pca.hpp | 2 +- src/gaussian.cpp | 5 ++-- src/gaussian_mixture_model.cpp | 13 ++++------- src/gmm_classifier.cpp | 23 ++++--------------- src/gmm_regressor.cpp | 11 ++++----- src/k_means.cpp | 1 - src/matrix_io.cpp | 4 ++-- src/pca.cpp | 3 +-- 17 files changed, 48 insertions(+), 66 deletions(-) diff --git a/include/particle_filter/ImportanceResampling.hpp b/include/particle_filter/ImportanceResampling.hpp index 5452c03..e07418b 100644 --- a/include/particle_filter/ImportanceResampling.hpp +++ b/include/particle_filter/ImportanceResampling.hpp @@ -31,7 +31,7 @@ class ImportanceResampling : public ResamplingStrategy { /** * The constructor of this base class inits some members. */ - ImportanceResampling(bool reset_weights = false, double particle_reset_weight = 0); + explicit ImportanceResampling(bool reset_weights = false, double particle_reset_weight = 0); /** * The destructor is empty. diff --git a/include/particle_filter/ParticleFilter.hpp b/include/particle_filter/ParticleFilter.hpp index d50c8a5..4143432 100644 --- a/include/particle_filter/ParticleFilter.hpp +++ b/include/particle_filter/ParticleFilter.hpp @@ -504,6 +504,6 @@ class ParticleFilter { } // namespace particle_filter // include template implementation -#include +#include #endif // PARTICLEFILTER_HPP diff --git a/include/particle_filter/ParticleFilter.hxx b/include/particle_filter/ParticleFilter.hxx index 2728903..aca5ac5 100644 --- a/include/particle_filter/ParticleFilter.hxx +++ b/include/particle_filter/ParticleFilter.hxx @@ -362,7 +362,7 @@ std::vector ParticleFilter::getCovariance(float percentage) c // std::cout << "percentage: " << percentage; // std::cout << "nums to consider: " << numToConsider; unsigned int i = 0; - for (i; i < numToConsider; i++) { + for (; i < numToConsider; i++) { xI += std::round(m_CurrentList[i]->getState().getXPos() * 10000.0) / 10000.0; yI += std::round(m_CurrentList[i]->getState().getYPos() * 10000.0) / 10000.0; thetaSinI += std::round(m_CurrentList[i]->getState().getSinTheta() * 10000.0) / 10000.0; diff --git a/include/particle_filter/gaussian.hpp b/include/particle_filter/gaussian.hpp index 7fdd184..54a976b 100644 --- a/include/particle_filter/gaussian.hpp +++ b/include/particle_filter/gaussian.hpp @@ -23,7 +23,10 @@ namespace gmms { class Gaussian { public: // constructors - Gaussian() { dimensionality_ = 0; }; + Gaussian() { + dimensionality_ = 0; + cov_abs_determinant_ = 0.0; + } Gaussian(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance) { setMeanCovariance(mean, covariance); } // useful functions diff --git a/include/particle_filter/gaussian_mixture_model.hpp b/include/particle_filter/gaussian_mixture_model.hpp index 9c2f899..b4e4653 100644 --- a/include/particle_filter/gaussian_mixture_model.hpp +++ b/include/particle_filter/gaussian_mixture_model.hpp @@ -71,14 +71,14 @@ class GaussianMixtureModel { GaussianMixtureModel copy() const; Eigen::MatrixXd save(); - inline void load(const std::string filename) { + inline void load(const std::string& filename) { MatrixIO mio; Eigen::MatrixXd model; mio.readFromFile(filename, model); load(model); } - inline void save(const std::string filename) { + inline void save(const std::string& filename) { MatrixIO mio; mio.writeToFile(filename, save()); } @@ -135,16 +135,15 @@ class GaussianMixtureModel { std::vector prior_vec_; std::vector gaussian_vec_; - GaussianMixtureModel(int num_components, double delta, int num_iterations, std::vector prior_vec_, - std::vector gaussian_vec_, Eigen::MatrixXd expectations_) { - num_components_ = num_components; - prior_vec_ = std::vector(prior_vec_); - gaussian_vec_ = std::vector(gaussian_vec_); - expectations_ = expectations_.replicate(1, 1); - delta_ = delta; - num_iterations_ = num_iterations; - initialized_ = false; - } + GaussianMixtureModel(int num_components, double delta, int num_iterations, const std::vector& prior_vec, + const std::vector& gaussian_vec, const Eigen::MatrixXd& expectations) + : initialized_(false), + delta_(delta), + num_iterations_(num_iterations), + num_components_(num_components), + expectations_(expectations.replicate(1, 1)), + prior_vec_(prior_vec), + gaussian_vec_(gaussian_vec) {} inline double probability_density_function(const Eigen::VectorXd point) { double probability = 0; diff --git a/include/particle_filter/gmm_classifier.hpp b/include/particle_filter/gmm_classifier.hpp index 2e89449..8e793eb 100644 --- a/include/particle_filter/gmm_classifier.hpp +++ b/include/particle_filter/gmm_classifier.hpp @@ -20,19 +20,21 @@ class GMMClassifier { delta_ = 0.01; max_iterations_ = 100; trained_ = false; + input_size_ = 0; } GMMClassifier(const double delta, const int max_iterations) { delta_ = delta; max_iterations_ = max_iterations; trained_ = false; + input_size_ = 0; } // dataset with each datapoint per row and each dimension per column // label must be in the last column void train(const Eigen::MatrixXd& dataset, bool evaluate_bic = true, int gmm_components = 10); std::vector predict(const Eigen::MatrixXd& dataset) const; - void load(const std::string filename); - void save(const std::string filename); + void load(const std::string& filename); + void save(const std::string& filename); // setter & getter inline double delta() const { return delta_; } diff --git a/include/particle_filter/gmm_regressor.hpp b/include/particle_filter/gmm_regressor.hpp index 61b75fd..24839c5 100644 --- a/include/particle_filter/gmm_regressor.hpp +++ b/include/particle_filter/gmm_regressor.hpp @@ -21,11 +21,13 @@ class GMMRegressor { delta_ = 0.01; max_iterations_ = 100; trained_ = false; + input_size_ = 0; } GMMRegressor(const double delta, const int max_iterations) { delta_ = delta; max_iterations_ = max_iterations; trained_ = false; + input_size_ = 0; } // dataset with each datapoint per row and each dimension per column @@ -37,8 +39,8 @@ class GMMRegressor { return predict(dataset, indices); } Eigen::MatrixXd predict(const Eigen::MatrixXd& dataset, const Eigen::VectorXi& output_indices) const; - void load(const std::string filename); - void save(const std::string filename); + void load(const std::string& filename); + void save(const std::string& filename); // setter & getter inline double delta() const { return delta_; } diff --git a/include/particle_filter/k_means.hpp b/include/particle_filter/k_means.hpp index dd4edd2..56e2407 100644 --- a/include/particle_filter/k_means.hpp +++ b/include/particle_filter/k_means.hpp @@ -19,7 +19,7 @@ class KMeans { num_clusters_ = 0; termination_flag_ = false; } - KMeans(const int num_clusters) { + explicit KMeans(const int num_clusters) { setNumClusters(num_clusters); termination_flag_ = false; } diff --git a/include/particle_filter/matrix_io.hpp b/include/particle_filter/matrix_io.hpp index 0025fcb..a376f8d 100644 --- a/include/particle_filter/matrix_io.hpp +++ b/include/particle_filter/matrix_io.hpp @@ -16,8 +16,8 @@ class MatrixIO { MatrixIO() {} ~MatrixIO() {} - void readFromFile(const std::string filename, Eigen::MatrixXd& matrix); - void writeToFile(const std::string filename, const Eigen::MatrixXd& matrix); + void readFromFile(const std::string& filename, Eigen::MatrixXd& matrix); + void writeToFile(const std::string& filename, const Eigen::MatrixXd& matrix); }; #endif // _MATRIX_IO_HPP_ diff --git a/include/particle_filter/pca.hpp b/include/particle_filter/pca.hpp index 84dac50..7671ac5 100644 --- a/include/particle_filter/pca.hpp +++ b/include/particle_filter/pca.hpp @@ -16,7 +16,7 @@ class PCA { public: // constructors PCA() { num_components_ = 0; } - PCA(const int num_components) { num_components_ = num_components; } + explicit PCA(const int num_components) { num_components_ = num_components; } // setters & getters inline void setNumComponents(const int num_components) { num_components_ = num_components; } diff --git a/src/gaussian.cpp b/src/gaussian.cpp index 0e30d53..1a5095d 100644 --- a/src/gaussian.cpp +++ b/src/gaussian.cpp @@ -77,15 +77,14 @@ void Gaussian::addToEigenMatrix(Eigen::MatrixXd& matrix, float x0, float y0, flo assert(y0 < y1); float x_delta = std::abs(x1 - x0); float y_delta = std::abs(y1 - y0); - float x, y; float x_mean = mean_(0, 0); float y_mean = mean_(1, 0); // TODO: save stepsize for (int y_step = 0; y_step < stepcount; y_step++) { - y = y0 + (y_delta / stepcount * y_step); + float y = y0 + (y_delta / stepcount * y_step); for (int x_step = 0; x_step < stepcount; x_step++) { - x = x0 + (x_delta / stepcount * x_step); + float x = x0 + (x_delta / stepcount * x_step); // Function taken from // https://en.wikipedia.org/wiki/Gaussian_function#Two-dimensional_Gaussian_function // matrix(y_step, x_step) += 1 * std::exp(-(std::pow(x - x_mean, 2) diff --git a/src/gaussian_mixture_model.cpp b/src/gaussian_mixture_model.cpp index 27bf582..c370a8f 100644 --- a/src/gaussian_mixture_model.cpp +++ b/src/gaussian_mixture_model.cpp @@ -62,7 +62,6 @@ void GaussianMixtureModel::expectationMaximization(const Eigen::MatrixXd& datase throw std::runtime_error(notinitialized_()); } - double old_log_likelihood; double log_likelihood = 0.0; double delta = 0.0; int iteration = 0; @@ -70,7 +69,7 @@ void GaussianMixtureModel::expectationMaximization(const Eigen::MatrixXd& datase do { expectation(dataset); maximization(dataset); - old_log_likelihood = log_likelihood; + double old_log_likelihood = log_likelihood; log_likelihood = logLikelihood(dataset); delta = log_likelihood - old_log_likelihood; ++iteration; @@ -84,8 +83,6 @@ void GaussianMixtureModel::expectation(const Eigen::MatrixXd& dataset) { #pragma omp parallel for for (int i = 0; i < dataset_size; ++i) { - double normalization_factor = 0.f; - for (int j = 0; j < num_components_; ++j) { expectations_(i, j) = prior_vec_[j] * gaussian_vec_[j].evaluate_point(dataset.row(i)); @@ -97,7 +94,7 @@ void GaussianMixtureModel::expectation(const Eigen::MatrixXd& dataset) { double expectations_sum = expectations_.row(i).sum(); if (expectations_sum != 0) { - normalization_factor = std::min(1 / expectations_sum, 1e15); + double normalization_factor = std::min(1 / expectations_sum, 1e15); expectations_.row(i) = normalization_factor * expectations_.row(i); } } @@ -105,7 +102,6 @@ void GaussianMixtureModel::expectation(const Eigen::MatrixXd& dataset) { void GaussianMixtureModel::maximization(const Eigen::MatrixXd& dataset) { int dataset_size = dataset.rows(); - double expectation_sum = 0.f; for (int i = 0; i < num_components_; ++i) { prior_vec_[i] = expectations_.col(i).sum(); @@ -247,7 +243,6 @@ visualization_msgs::msg::Marker GaussianMixtureModel::renderMarker(float x0, flo matrix = matrix / matrix.maxCoeff(); float x_delta = std::abs(x1 - x0); float y_delta = std::abs(y1 - y0); - float x, y; float x_stepsize = x_delta / stepcount; float y_stepsize = y_delta / stepcount; visualization_msgs::msg::Marker marker; @@ -267,9 +262,9 @@ visualization_msgs::msg::Marker GaussianMixtureModel::renderMarker(float x0, flo } for (int y_step = 0; y_step < stepcount; y_step++) { - y = y0 + (y_stepsize * y_step); + float y = y0 + (y_stepsize * y_step); for (int x_step = 0; x_step < stepcount; x_step++) { - x = x0 + (x_stepsize * x_step); + float x = x0 + (x_stepsize * x_step); geometry_msgs::msg::Point point; point.x = x; point.y = y; diff --git a/src/gmm_classifier.cpp b/src/gmm_classifier.cpp index c3148cb..166fbea 100644 --- a/src/gmm_classifier.cpp +++ b/src/gmm_classifier.cpp @@ -18,8 +18,6 @@ namespace gmms { void GMMClassifier::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int gmm_components) { - int dataset_size = dataset.rows(); - Eigen::VectorXi labels = dataset.rightCols(1).cast(); Eigen::MatrixXd training_data = dataset.leftCols(dataset.cols() - 1); std::set classes(labels.data(), labels.data() + labels.size()); @@ -77,7 +75,7 @@ void GMMClassifier::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int try { model->expectationMaximization(class_data); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { std::cout << "\t\t\t\t" << c << " components are not usable" << std::endl; break; } @@ -88,10 +86,7 @@ void GMMClassifier::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int if (first_trial || trial_bic < bic) { bic = trial_bic; gmm_vec_[class_idx] = model; - - if (first_trial) { - first_trial = false; - } + first_trial = false; } } @@ -115,11 +110,6 @@ std::vector GMMClassifier::predict(const Eigen::MatrixXd& dataset) const { int dataset_size = dataset.rows(); std::vector assignments(dataset_size); - int num_threads = omp_get_max_threads(); - int thread_dataset_size = floor(double(dataset_size) / double(num_threads)); - - int thread_log_likelihood[num_threads]; - #pragma omp parallel for for (int i = 0; i < dataset_size; ++i) { bool first = true; @@ -135,10 +125,7 @@ std::vector GMMClassifier::predict(const Eigen::MatrixXd& dataset) const { if (first || log_likelihood < class_log_likelihood) { assignments[i] = classes_[k]; log_likelihood = class_log_likelihood; - - if (first) { - first = false; - } + first = false; } } } @@ -146,7 +133,7 @@ std::vector GMMClassifier::predict(const Eigen::MatrixXd& dataset) const { return assignments; } -void GMMClassifier::load(const std::string filename) { +void GMMClassifier::load(const std::string& filename) { MatrixIO mio; Eigen::MatrixXd model; @@ -171,7 +158,7 @@ void GMMClassifier::load(const std::string filename) { } } -void GMMClassifier::save(const std::string filename) { +void GMMClassifier::save(const std::string& filename) { MatrixIO mio; int rows = 0; int cols = 0; diff --git a/src/gmm_regressor.cpp b/src/gmm_regressor.cpp index af5f10e..fc4e989 100644 --- a/src/gmm_regressor.cpp +++ b/src/gmm_regressor.cpp @@ -44,7 +44,7 @@ void GMMRegressor::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int try { model->expectationMaximization(dataset); - } catch (std::runtime_error& e) { + } catch (const std::runtime_error& e) { std::cout << "\t\t\t\t" << c << " components are not usable" << std::endl; break; } @@ -56,10 +56,7 @@ void GMMRegressor::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int if (first_trial || trial_bic < bic) { bic = trial_bic; gmm_ = model; - - if (first_trial) { - first_trial = false; - } + first_trial = false; } } @@ -147,7 +144,7 @@ Eigen::MatrixXd GMMRegressor::predict(const Eigen::MatrixXd& dataset, const Eige return regressions; } -void GMMRegressor::load(const std::string filename) { +void GMMRegressor::load(const std::string& filename) { MatrixIO mio; Eigen::MatrixXd model; @@ -161,7 +158,7 @@ void GMMRegressor::load(const std::string filename) { gmm_->load(model.block(4, 0, model.rows() - 4, model.cols())); } -void GMMRegressor::save(const std::string filename) { +void GMMRegressor::save(const std::string& filename) { MatrixIO mio; Eigen::MatrixXd gmm_model = gmm_->save(); Eigen::MatrixXd model = Eigen::MatrixXd::Zero(gmm_model.rows() + 4, gmm_model.cols()); diff --git a/src/k_means.cpp b/src/k_means.cpp index 9e95208..6731c5e 100644 --- a/src/k_means.cpp +++ b/src/k_means.cpp @@ -21,7 +21,6 @@ void KMeans::cluster(const Eigen::MatrixXd& dataset) { } forgyInitialization(dataset); - int iterations = 0; do { assign(dataset); diff --git a/src/matrix_io.cpp b/src/matrix_io.cpp index ea8b407..7d0a8eb 100644 --- a/src/matrix_io.cpp +++ b/src/matrix_io.cpp @@ -11,7 +11,7 @@ #include #include -void MatrixIO::readFromFile(const std::string filename, Eigen::MatrixXd& matrix) { +void MatrixIO::readFromFile(const std::string& filename, Eigen::MatrixXd& matrix) { std::ifstream input_file; std::string line; int rows = 0; @@ -61,7 +61,7 @@ void MatrixIO::readFromFile(const std::string filename, Eigen::MatrixXd& matrix) input_file.close(); } -void MatrixIO::writeToFile(const std::string filename, const Eigen::MatrixXd& matrix) { +void MatrixIO::writeToFile(const std::string& filename, const Eigen::MatrixXd& matrix) { std::ofstream output_file; output_file.open(filename); diff --git a/src/pca.cpp b/src/pca.cpp index 47f028a..f5e8b17 100644 --- a/src/pca.cpp +++ b/src/pca.cpp @@ -5,10 +5,9 @@ // # #// // #########################################################// -#include pp - #include #include +#include #include namespace gmms { From b0e9165c8e7396b6d6908a135044241da996eb5a Mon Sep 17 00:00:00 2001 From: Jan Gutsche Date: Sat, 4 Jan 2025 22:37:32 +0000 Subject: [PATCH 7/7] Upgrade CI Ubuntu version and pre-commit versions --- .github/workflows/pre-commit.yml | 4 ++-- .pre-commit-config.yaml | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index bb68480..bb80c07 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -7,10 +7,10 @@ on: jobs: pre-commit: - runs-on: ubuntu-latest + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 - uses: actions/setup-python@v4 - name: Install cppcheck run: sudo apt install cppcheck -y - - uses: pre-commit/action@v3.0.0 + - uses: pre-commit/action@v3.0.1 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 06f738c..5aa99cc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -27,7 +27,7 @@ repos: - id: cmake-format - id: cmake-lint - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.5.0 + rev: v5.0.0 hooks: - id: check-merge-conflict - id: check-toml