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/.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 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..bb80c07 --- /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-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.1 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..5aa99cc --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +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" + - 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: v5.0.0 + hooks: + - id: check-merge-conflict + - id: check-toml + - id: check-xml + - id: check-yaml + - id: detect-private-key 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/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() 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 diff --git a/include/particle_filter/CRandomNumberGenerator.h b/include/particle_filter/CRandomNumberGenerator.h deleted file mode 100644 index 111ce29..0000000 --- a/include/particle_filter/CRandomNumberGenerator.h +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef RANDOMNUMBERGENERATOR_H -#define RANDOMNUMBERGENERATOR_H - -#include "particle_filter/RandomNumberGenerationStrategy.h" - -namespace particle_filter { - -/** - * @class CRandomNumberGenerator - * - * @brief Class for the generation of random numbers. - * - * This class can generate randomly generated numbers from uniform and - * gaussian distributions. - * Note: this is a very simple PRNG, using the C-function rand(). - * - * @author Stephan Wirth - */ -class CRandomNumberGenerator : public RandomNumberGenerationStrategy { -public: - /** - * The constructor calls init(). - */ - 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; - - /** - * 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(); - -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; -}; - -} // namespace particle_filter - -#endif // RANDOMNUMBERGENERATOR_H diff --git a/include/particle_filter/CRandomNumberGenerator.hpp b/include/particle_filter/CRandomNumberGenerator.hpp new file mode 100644 index 0000000..585204a --- /dev/null +++ b/include/particle_filter/CRandomNumberGenerator.hpp @@ -0,0 +1,65 @@ +#ifndef RANDOMNUMBERGENERATOR_HPP +#define RANDOMNUMBERGENERATOR_HPP + +#include + +namespace particle_filter { + +/** + * @class CRandomNumberGenerator + * + * @brief Class for the generation of random numbers. + * + * This class can generate randomly generated numbers from uniform and + * gaussian distributions. + * Note: this is a very simple PRNG, using the C-function rand(). + * + * @author Stephan Wirth + */ +class CRandomNumberGenerator : public RandomNumberGenerationStrategy { + public: + /** + * The constructor calls init(). + */ + 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; + + /** + * 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(); + + 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; +}; + +} // namespace particle_filter + +#endif // RANDOMNUMBERGENERATOR_HPP diff --git a/include/particle_filter/CompareParticleWeights.h b/include/particle_filter/CompareParticleWeights.hpp similarity index 55% rename from include/particle_filter/CompareParticleWeights.h rename to include/particle_filter/CompareParticleWeights.hpp index aa6a1a0..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 { @@ -21,17 +21,17 @@ 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 -#endif +#endif // COMPAREPARTICLEWEIGHTS_HPP diff --git a/include/particle_filter/ImportanceResampling.h b/include/particle_filter/ImportanceResampling.h deleted file mode 100644 index fd13d37..0000000 --- a/include/particle_filter/ImportanceResampling.h +++ /dev/null @@ -1,105 +0,0 @@ -#ifndef IMPORTANCERESAMPLING_H -#define IMPORTANCERESAMPLING_H - -#include -#include "particle_filter/CRandomNumberGenerator.h" -#include "particle_filter/Particle.h" - -namespace particle_filter { - -/** - * @class ImportanceResampling - * - * @brief A resampling strategy that performs importance resampling - * - * The resampling strategy defines how the resampling is performed in the - * resample step of a particle filter. - * - * @author Stephan Wirth - * - * @see ResamplingStrategy - */ - -template -class ImportanceResampling : public ResamplingStrategy { - /** - * 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); - - /** - * 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; - - /** - * Sets the Random Number Generator to use in resample() to generate - * uniformly distributed random numbers. - */ - -protected: - // The default random number generator - CRandomNumberGenerator m_RNG; - -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) {} - -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_); - } - } -} -} // namespace particle_filter -#endif // IMPORTANCERESAMPLING_H diff --git a/include/particle_filter/ImportanceResampling.hpp b/include/particle_filter/ImportanceResampling.hpp new file mode 100644 index 0000000..e07418b --- /dev/null +++ b/include/particle_filter/ImportanceResampling.hpp @@ -0,0 +1,98 @@ +#ifndef IMPORTANCERESAMPLING_HPP +#define IMPORTANCERESAMPLING_HPP + +#include +#include +#include + +namespace particle_filter { + +/** + * @class ImportanceResampling + * + * @brief A resampling strategy that performs importance resampling + * + * The resampling strategy defines how the resampling is performed in the + * resample step of a particle filter. + * + * @author Stephan Wirth + * + * @see ResamplingStrategy + */ + +template +class ImportanceResampling : public ResamplingStrategy { + /** + * A ParticleList is an array of pointers to Particles. + */ + typedef std::vector*> ParticleList; + + public: + /** + * The constructor of this base class inits some members. + */ + explicit ImportanceResampling(bool reset_weights = false, double particle_reset_weight = 0); + + /** + * 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; + + /** + * Sets the Random Number Generator to use in resample() to generate + * uniformly distributed random numbers. + */ + + protected: + // The default random number generator + CRandomNumberGenerator m_RNG; + + 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) {} + +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_); + } + } +} +} // namespace particle_filter +#endif // IMPORTANCERESAMPLING_HPP diff --git a/include/particle_filter/MovementModel.h b/include/particle_filter/MovementModel.h deleted file mode 100644 index ccdd698..0000000 --- a/include/particle_filter/MovementModel.h +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef MOVEMENTSTRATEGY_H -#define MOVEMENTSTRATEGY_H - -#include -#include -#include - -namespace particle_filter { - -/** - * @class MovementModel - * - * @brief Templated interface for movement models for particle filters. - * - * The movement model in a particle filter defines how a particle's state - * changes over time. It is used in the drift and diffuse step of - * particle_filter::ParticleFilter (strategy pattern). To define a movement - * model, create a sub-class of this class and implement the drift() method. A - * particle filter with this movement model applies the drift method for each - * particle in each filter step. Also you have to implement the function - * diffuse() to define a jitter that is added to a state after drift() (which - * may be empty of course). You can use the function randomGauss() to obtain - * Gaussian-distributed random variables. - * - * @author Stephan Wirth - * - * @see ParticleFilter - * @see Particle - */ - -template -class 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 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: -}; - -template -MovementModel::~MovementModel() {} - -} // namespace particle_filter -#endif diff --git a/include/particle_filter/MovementModel.hpp b/include/particle_filter/MovementModel.hpp new file mode 100644 index 0000000..92d7e19 --- /dev/null +++ b/include/particle_filter/MovementModel.hpp @@ -0,0 +1,66 @@ +#ifndef MOVEMENTSTRATEGY_HPP +#define MOVEMENTSTRATEGY_HPP + +#include +#include +#include + +namespace particle_filter { + +/** + * @class MovementModel + * + * @brief Templated interface for movement models for particle filters. + * + * The movement model in a particle filter defines how a particle's state + * changes over time. It is used in the drift and diffuse step of + * particle_filter::ParticleFilter (strategy pattern). To define a movement + * model, create a sub-class of this class and implement the drift() method. A + * particle filter with this movement model applies the drift method for each + * particle in each filter step. Also you have to implement the function + * diffuse() to define a jitter that is added to a state after drift() (which + * may be empty of course). You can use the function randomGauss() to obtain + * Gaussian-distributed random variables. + * + * @author Stephan Wirth + * + * @see ParticleFilter + * @see Particle + */ + +template +class 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 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: +}; + +template +MovementModel::~MovementModel() {} + +} // namespace particle_filter +#endif // MOVEMENTSTRATEGY_HPP diff --git a/include/particle_filter/ObservationModel.h b/include/particle_filter/ObservationModel.hpp similarity index 57% rename from include/particle_filter/ObservationModel.h rename to include/particle_filter/ObservationModel.hpp index 970c159..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 @@ -28,33 +28,33 @@ 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 ObservationModel::~ObservationModel() {} } // namespace particle_filter -#endif +#endif // OBSERVATIONSTRATEGY_HPP diff --git a/include/particle_filter/Particle.h b/include/particle_filter/Particle.h deleted file mode 100644 index b552a78..0000000 --- a/include/particle_filter/Particle.h +++ /dev/null @@ -1,129 +0,0 @@ -#ifndef PARTICLE_H -#define PARTICLE_H - -#include "particle_filter/ParticleFilter.h" - -namespace particle_filter { -/** - * @class Particle - * @brief Class that represents a particle for a particle filter. - * - * A particle as it is used in particle filters is a set of one state - * (m_State) and one importance factor (m_Weight). - * A set of Particles is a discrete representation of a probability - * distribution. Normally the user of the class ParticleFilter has - * not to care about this class, as it is used only internally by - * ParticleFilter. - * @author Niklas Fiedler - * @author Stephan Wirth - * @see 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; -}; - -template -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; -} - -template -void Particle::setState(const StateType& newState) { - m_State = newState; -} - -template -double Particle::getWeight() const { - return m_Weight * m_normalization; -} - -template -double Particle::getWeightUnnormalized() const { - return m_Weight; -} - -template -void Particle::setWeight(double newWeight) { - m_Weight = newWeight; -} - -template -void Particle::setNormalization(double normalization) { - m_normalization = normalization; -} -} // namespace particle_filter - -#endif diff --git a/include/particle_filter/Particle.hpp b/include/particle_filter/Particle.hpp new file mode 100644 index 0000000..9444c5e --- /dev/null +++ b/include/particle_filter/Particle.hpp @@ -0,0 +1,127 @@ +#ifndef PARTICLE_HPP +#define PARTICLE_HPP + +#include + +namespace particle_filter { +/** + * @class Particle + * @brief Class that represents a particle for a particle filter. + * + * A particle as it is used in particle filters is a set of one state + * (m_State) and one importance factor (m_Weight). + * A set of Particles is a discrete representation of a probability + * distribution. Normally the user of the class ParticleFilter has + * not to care about this class, as it is used only internally by + * ParticleFilter. + * @author Niklas Fiedler + * @author Stephan Wirth + * @see 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; +}; + +template +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; +} + +template +void Particle::setState(const StateType& newState) { + m_State = newState; +} + +template +double Particle::getWeight() const { + return m_Weight * m_normalization; +} + +template +double Particle::getWeightUnnormalized() const { + return m_Weight; +} + +template +void Particle::setWeight(double newWeight) { + m_Weight = newWeight; +} + +template +void Particle::setNormalization(double normalization) { + m_normalization = normalization; +} +} // namespace particle_filter + +#endif // PARTICLE_HPP diff --git a/include/particle_filter/ParticleFilter.h b/include/particle_filter/ParticleFilter.h deleted file mode 100644 index 732f8e8..0000000 --- a/include/particle_filter/ParticleFilter.h +++ /dev/null @@ -1,531 +0,0 @@ -#ifndef PARTICLEFILTER_H -#define PARTICLEFILTER_H - -#include - -#include -#include // for time measurement -#include -#include -#include -#include - - -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -#include - - -#include -#include -#include -#include - -namespace particle_filter { - -/** - * @class ParticleFilter - * - * @author Niklas Fiedler - * @author Stephan Wirth - * - * @brief (Templated) class that defines a particle filter. - * - * A particle filter is a discrete method to describe and compute with a - * probability distribution. In other words, it is an implementation of a - * recursive Bayesian filter by Monte Carlo Methods. The sequential importance - * sampling (SIS) used is also known as bootstrap filtering, the condensation - * algorithm, particle filtering, interacting particle approximations and - * survival of the fittest. This template class implements the basic methods for - * a particle filter. The changeable parts of the particle filter are - * implemented using the strategy pattern. If you don't know what it is, you - * should read something about it, as it is used in many methods here. - * - * The following strategies are used by ParticleFilter, all of them can be - * switched at runtime. - * @li ObservationModel defines how a state can be evaluated (weighted) - * @li MovementModel defines how a state will be propagated during time - * @li ResamplingStrategy defines how resampling occurs (see - * ImportanceResampling for the default implementation) - * - * - * You must do the following to use the particle filter: - * @li Create a class for the state that you want to track with the - * ParticleFilter. Let's name your state MyState. The state has to implement - * the operator - * @code - * operator=(const MyState& other); - * @endcode - * because a particle filter copies the "fittest" states in a resampling - * step. If you want to use the methods getBestXPercentEstimate() or - * getMmseEstimate(), your state has to implement two more operators: - * @code - * MyState operator*(float factor) const; - * @endcode - * and - * @code - * MyState& operator+=(const MyState& other); - * @endcode - * These operators allow the ParticleFilter to compute the weighted average - * of a set of states. - * Instead of creating your own state, you may use a basic datatype as - * state, for example to create a ParticleFilter is perfectly - * possible. - * @li The next step is to create an observation strategy MyObservationModel for - * your state, deriving it from ObservationModel and implementing the - * method ObservationModel::measure(). - * @li Create a movement strategy MyMovementModel for your state, deriving it - * from MovementModel and implement the methods - * MovementModel::diffuse() and MovementModel::drift(). - * - * After that you can use the particle filter this way: - * @code - * int numOfParticles = 500; - * MyMovementModel mm; // create movement strategy - * MyObservationModel om; // create observation strategy - * ParticleFilter pf(numOfParticles, &om, &mm); // create filter - * - * // run the filter loop - * bool doFilter = true; - * while (doFilter) { - * // update your observation model here - * // ... - * // update your movement model here (if necessary) - * // ... - * - * // run one filter step, the filter uses the - * // observation model and the movement model - * // that were given in the constructor - * pf->filter(); - * - * // retrieve the best state and - * // do something with the result - * std::cout << pf->getBestState().getVariable1() << std::endl; - * - * } - * @endcode - * - * The ParticleFilter has the following resampling modes: - * @li RESAMPLE_NEVER skip resampling, - * @li RESAMPLE_ALWAYS does a resampling in every filter step whenever - * filter() is called, - * @li RESAMPLE_NEFF does a resampling in filter() only if the number of - * effective particles falls below the half of the total number of - * particles (see getNumEffectiveParticles() for details). - * - * The default is RESAMPLE_NEFF. - * You can switch the mode via setResamplingMode(). - * - * You have two options to influence the states that are used internally by - * the particle filter. The first one is to set a prior state: - * @code - * // initialize the filter's states by setting a prior state - * MyState priorState; - * priorState.setPosition(20, 30); - * priorState.setVelocity(1.2, 0); - * pf.setPriorState(priorState); - * @endcode - * The second option is to use a state distribution that is derived from - * StateDistribution: - * @code - * // create a distribution - * MyStateDistribution distribution; - * // draw all states from this distribution - * pf.drawAllFromDistribution(distribution); - * @endcode - * - * To traverse the particle list, you may use particleListBegin() and - * particleListEnd() which return iterators to the beginning and to the end of - * the list respectively. - * - * @see Particle - * @see ObservationModel - * @see MovementModel - * @see ResamplingStrategy - */ - -/** - * Resampling modes. - */ -enum ResamplingMode { - /// 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; -}; - -} // namespace particle_filter - -// include template implementation -#include "ParticleFilter.hxx" - -#endif diff --git a/include/particle_filter/ParticleFilter.hpp b/include/particle_filter/ParticleFilter.hpp new file mode 100644 index 0000000..4143432 --- /dev/null +++ b/include/particle_filter/ParticleFilter.hpp @@ -0,0 +1,509 @@ +#ifndef PARTICLEFILTER_HPP +#define PARTICLEFILTER_HPP + +#include + +#include +#include +#include // for time measurement +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace particle_filter { + +/** + * @class ParticleFilter + * + * @author Niklas Fiedler + * @author Stephan Wirth + * + * @brief (Templated) class that defines a particle filter. + * + * A particle filter is a discrete method to describe and compute with a + * probability distribution. In other words, it is an implementation of a + * recursive Bayesian filter by Monte Carlo Methods. The sequential importance + * sampling (SIS) used is also known as bootstrap filtering, the condensation + * algorithm, particle filtering, interacting particle approximations and + * survival of the fittest. This template class implements the basic methods for + * a particle filter. The changeable parts of the particle filter are + * implemented using the strategy pattern. If you don't know what it is, you + * should read something about it, as it is used in many methods here. + * + * The following strategies are used by ParticleFilter, all of them can be + * switched at runtime. + * @li ObservationModel defines how a state can be evaluated (weighted) + * @li MovementModel defines how a state will be propagated during time + * @li ResamplingStrategy defines how resampling occurs (see + * ImportanceResampling for the default implementation) + * + * + * You must do the following to use the particle filter: + * @li Create a class for the state that you want to track with the + * ParticleFilter. Let's name your state MyState. The state has to implement + * the operator + * @code + * operator=(const MyState& other); + * @endcode + * because a particle filter copies the "fittest" states in a resampling + * step. If you want to use the methods getBestXPercentEstimate() or + * getMmseEstimate(), your state has to implement two more operators: + * @code + * MyState operator*(float factor) const; + * @endcode + * and + * @code + * MyState& operator+=(const MyState& other); + * @endcode + * These operators allow the ParticleFilter to compute the weighted average + * of a set of states. + * Instead of creating your own state, you may use a basic datatype as + * state, for example to create a ParticleFilter is perfectly + * possible. + * @li The next step is to create an observation strategy MyObservationModel for + * your state, deriving it from ObservationModel and implementing the + * method ObservationModel::measure(). + * @li Create a movement strategy MyMovementModel for your state, deriving it + * from MovementModel and implement the methods + * MovementModel::diffuse() and MovementModel::drift(). + * + * After that you can use the particle filter this way: + * @code + * int numOfParticles = 500; + * MyMovementModel mm; // create movement strategy + * MyObservationModel om; // create observation strategy + * ParticleFilter pf(numOfParticles, &om, &mm); // create filter + * + * // run the filter loop + * bool doFilter = true; + * while (doFilter) { + * // update your observation model here + * // ... + * // update your movement model here (if necessary) + * // ... + * + * // run one filter step, the filter uses the + * // observation model and the movement model + * // that were given in the constructor + * pf->filter(); + * + * // retrieve the best state and + * // do something with the result + * std::cout << pf->getBestState().getVariable1() << std::endl; + * + * } + * @endcode + * + * The ParticleFilter has the following resampling modes: + * @li RESAMPLE_NEVER skip resampling, + * @li RESAMPLE_ALWAYS does a resampling in every filter step whenever + * filter() is called, + * @li RESAMPLE_NEFF does a resampling in filter() only if the number of + * effective particles falls below the half of the total number of + * particles (see getNumEffectiveParticles() for details). + * + * The default is RESAMPLE_NEFF. + * You can switch the mode via setResamplingMode(). + * + * You have two options to influence the states that are used internally by + * the particle filter. The first one is to set a prior state: + * @code + * // initialize the filter's states by setting a prior state + * MyState priorState; + * priorState.setPosition(20, 30); + * priorState.setVelocity(1.2, 0); + * pf.setPriorState(priorState); + * @endcode + * The second option is to use a state distribution that is derived from + * StateDistribution: + * @code + * // create a distribution + * MyStateDistribution distribution; + * // draw all states from this distribution + * pf.drawAllFromDistribution(distribution); + * @endcode + * + * To traverse the particle list, you may use particleListBegin() and + * particleListEnd() which return iterators to the beginning and to the end of + * the list respectively. + * + * @see Particle + * @see ObservationModel + * @see MovementModel + * @see ResamplingStrategy + */ + +/** + * Resampling modes. + */ +enum ResamplingMode { + /// 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; +}; + +} // namespace particle_filter + +// include template implementation +#include + +#endif // PARTICLEFILTER_HPP diff --git a/include/particle_filter/ParticleFilter.hxx b/include/particle_filter/ParticleFilter.hxx index f19b6a8..aca5ac5 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 < 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 deleted file mode 100644 index 2d8389a..0000000 --- a/include/particle_filter/RandomNumberGenerationStrategy.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef RANDOMNUMBERGENERATIONSTRATEGY_H -#define RANDOMNUMBERGENERATIONSTRATEGY_H - -namespace particle_filter { - -/** - * @class RandomNumberGenerationStrategy - * - * @brief Interface for a strategy to create random numbers - * - * This class defines the interface for random number generators. To create your - * own random number generation strategy, sub-class from this class. - * - * @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: -}; - -} // namespace particle_filter - -#endif // RANDOMNUMBERGENERATIONSTRATEGY_H diff --git a/include/particle_filter/RandomNumberGenerationStrategy.hpp b/include/particle_filter/RandomNumberGenerationStrategy.hpp new file mode 100644 index 0000000..4460fda --- /dev/null +++ b/include/particle_filter/RandomNumberGenerationStrategy.hpp @@ -0,0 +1,51 @@ +#ifndef RANDOMNUMBERGENERATIONSTRATEGY_HPP +#define RANDOMNUMBERGENERATIONSTRATEGY_HPP + +namespace particle_filter { + +/** + * @class RandomNumberGenerationStrategy + * + * @brief Interface for a strategy to create random numbers + * + * This class defines the interface for random number generators. To create your + * own random number generation strategy, sub-class from this class. + * + * @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: +}; + +} // namespace particle_filter + +#endif // RANDOMNUMBERGENERATIONSTRATEGY_HPP diff --git a/include/particle_filter/ResamplingStrategy.h b/include/particle_filter/ResamplingStrategy.h deleted file mode 100644 index b47926c..0000000 --- a/include/particle_filter/ResamplingStrategy.h +++ /dev/null @@ -1,58 +0,0 @@ -#ifndef RESAMPLINGSTRATEGY_H -#define RESAMPLINGSTRATEGY_H - -#include - -#include - -namespace particle_filter { - -/** - * @class ResamplingStrategy - * - * @brief Templated interface for resampling strategies - * - * The resampling strategy defines how the resampling is performed in the - * resample step of a particle filter. One implementation of this strategy is - * @see ImportanceResampling. - * - * @author Stephan Wirth - * - * @see ParticleFilter - */ - -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: -}; - -template -ResamplingStrategy::~ResamplingStrategy() {} - -} // namespace particle_filter -#endif // RESAMPLINGSTRATEGY_H diff --git a/include/particle_filter/ResamplingStrategy.hpp b/include/particle_filter/ResamplingStrategy.hpp new file mode 100644 index 0000000..7aba8a2 --- /dev/null +++ b/include/particle_filter/ResamplingStrategy.hpp @@ -0,0 +1,56 @@ +#ifndef RESAMPLINGSTRATEGY_HPP +#define RESAMPLINGSTRATEGY_HPP + +#include +#include + +namespace particle_filter { + +/** + * @class ResamplingStrategy + * + * @brief Templated interface for resampling strategies + * + * The resampling strategy defines how the resampling is performed in the + * resample step of a particle filter. One implementation of this strategy is + * @see ImportanceResampling. + * + * @author Stephan Wirth + * + * @see ParticleFilter + */ + +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: +}; + +template +ResamplingStrategy::~ResamplingStrategy() {} + +} // namespace particle_filter +#endif // RESAMPLINGSTRATEGY_HPP diff --git a/include/particle_filter/StateDistribution.h b/include/particle_filter/StateDistribution.hpp similarity index 56% rename from include/particle_filter/StateDistribution.h rename to include/particle_filter/StateDistribution.hpp index cb2d95d..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 { /** @@ -22,30 +23,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() {} @@ -54,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.h deleted file mode 100644 index ffa088e..0000000 --- a/include/particle_filter/defs.h +++ /dev/null @@ -1,13 +0,0 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models defs.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#ifndef _DEFS_H_ -#define _DEFS_H_ - -#define M_EPS (1E-7) - -#endif // _DEFS_H_ diff --git a/include/particle_filter/defs.hpp b/include/particle_filter/defs.hpp new file mode 100644 index 0000000..d09465b --- /dev/null +++ b/include/particle_filter/defs.hpp @@ -0,0 +1,13 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models defs.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _DEFS_HPP_ +#define _DEFS_HPP_ + +#define M_EPS (1E-7) + +#endif // _DEFS_HPP_ diff --git a/include/particle_filter/gaussian.h b/include/particle_filter/gaussian.h deleted file mode 100644 index 0fc5ee2..0000000 --- a/include/particle_filter/gaussian.h +++ /dev/null @@ -1,84 +0,0 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gaussian.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#ifndef _GAUSSIAN_H_ -#define _GAUSSIAN_H_ - -#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); - } - - // 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()); - } - - 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; - -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 covariance_not_invertible() { - return "Covariance matrix is not invertible"; - } -}; -} // namespace gmms - -#endif // _GAUSSIAN_H_ diff --git a/include/particle_filter/gaussian.hpp b/include/particle_filter/gaussian.hpp new file mode 100644 index 0000000..54a976b --- /dev/null +++ b/include/particle_filter/gaussian.hpp @@ -0,0 +1,66 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models gaussian.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _GAUSSIAN_HPP_ +#define _GAUSSIAN_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gmms { +class Gaussian { + public: + // constructors + Gaussian() { + dimensionality_ = 0; + cov_abs_determinant_ = 0.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()); + } + + 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; + + 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 covariance_not_invertible() { return "Covariance matrix is not invertible"; } +}; +} // namespace gmms + +#endif // _GAUSSIAN_HPP_ diff --git a/include/particle_filter/gaussian_mixture_model.h b/include/particle_filter/gaussian_mixture_model.h deleted file mode 100644 index eb99fa6..0000000 --- a/include/particle_filter/gaussian_mixture_model.h +++ /dev/null @@ -1,212 +0,0 @@ -//#########################################################// -//# #// -//# 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 - -namespace gmms { - /** - * @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"; - } -}; -} // namespace gmms - -#endif // _GAUSSIAN_MIXTURE_MODEL_H_ diff --git a/include/particle_filter/gaussian_mixture_model.hpp b/include/particle_filter/gaussian_mixture_model.hpp new file mode 100644 index 0000000..b4e4653 --- /dev/null +++ b/include/particle_filter/gaussian_mixture_model.hpp @@ -0,0 +1,171 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models gaussian_mixture_model.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _GAUSSIAN_MIXTURE_MODEL_HPP_ +#define _GAUSSIAN_MIXTURE_MODEL_HPP_ + +#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 { + 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, 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; + + 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 + +#endif // _GAUSSIAN_MIXTURE_MODEL_HPP_ diff --git a/include/particle_filter/gmm_classifier.h b/include/particle_filter/gmm_classifier.h deleted file mode 100644 index e51dc11..0000000 --- a/include/particle_filter/gmm_classifier.h +++ /dev/null @@ -1,76 +0,0 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_classifier.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#ifndef _GMM_CLASSIFIER_H_ -#define _GMM_CLASSIFIER_H_ - -#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"; - } -}; -} // namespace gmms - -#endif // _GMM_CLASSIFIER_H_ diff --git a/include/particle_filter/gmm_classifier.hpp b/include/particle_filter/gmm_classifier.hpp new file mode 100644 index 0000000..8e793eb --- /dev/null +++ b/include/particle_filter/gmm_classifier.hpp @@ -0,0 +1,61 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_classifier.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _GMM_CLASSIFIER_HPP_ +#define _GMM_CLASSIFIER_HPP_ + +#include +#include +#include +#include + +namespace gmms { +class GMMClassifier { + public: + 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); + + // 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 + +#endif // _GMM_CLASSIFIER_HPP_ diff --git a/include/particle_filter/gmm_regressor.h b/include/particle_filter/gmm_regressor.h deleted file mode 100644 index 5c6c836..0000000 --- a/include/particle_filter/gmm_regressor.h +++ /dev/null @@ -1,89 +0,0 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_regressor.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#ifndef _GMM_REGRESSOR_H_ -#define _GMM_REGRESSOR_H_ - -#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; - } - - // 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); - - // 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_; - - 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 notvalid_() { - return "The requested output size is 0"; - } -}; -} // namespace gmms - -#endif // _GMM_REGRESSOR_H_ diff --git a/include/particle_filter/gmm_regressor.hpp b/include/particle_filter/gmm_regressor.hpp new file mode 100644 index 0000000..24839c5 --- /dev/null +++ b/include/particle_filter/gmm_regressor.hpp @@ -0,0 +1,70 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_regressor.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _GMM_REGRESSOR_HPP_ +#define _GMM_REGRESSOR_HPP_ + +#include +#include +#include +#include +#include + +namespace gmms { +class GMMRegressor { + public: + 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 + 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); + + // 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_; + + 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 notvalid_() { return "The requested output size is 0"; } +}; +} // namespace gmms + +#endif // _GMM_REGRESSOR_HPP_ diff --git a/include/particle_filter/k_means.h b/include/particle_filter/k_means.h deleted file mode 100644 index 3e31784..0000000 --- a/include/particle_filter/k_means.h +++ /dev/null @@ -1,68 +0,0 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models k_means.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#ifndef _K_MEANS_H_ -#define _K_MEANS_H_ - -#include -#include - -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); -}; -} // namespace gmms - -#endif // _K_MEANS_H_ diff --git a/include/particle_filter/k_means.hpp b/include/particle_filter/k_means.hpp new file mode 100644 index 0000000..56e2407 --- /dev/null +++ b/include/particle_filter/k_means.hpp @@ -0,0 +1,58 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models k_means.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _K_MEANS_HPP_ +#define _K_MEANS_HPP_ + +#include +#include + +namespace gmms { +class KMeans { + public: + // constructors + KMeans() { + num_clusters_ = 0; + termination_flag_ = false; + } + explicit 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 + +#endif // _K_MEANS_HPP_ diff --git a/include/particle_filter/matrix_io.h b/include/particle_filter/matrix_io.h deleted file mode 100644 index 473c6b4..0000000 --- a/include/particle_filter/matrix_io.h +++ /dev/null @@ -1,23 +0,0 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models matrix_io.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#ifndef _MATRIX_IO_H_ -#define _MATRIX_IO_H_ - -#include -#include - -class MatrixIO { -public: - MatrixIO() {} - ~MatrixIO() {} - - 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/matrix_io.hpp b/include/particle_filter/matrix_io.hpp new file mode 100644 index 0000000..a376f8d --- /dev/null +++ b/include/particle_filter/matrix_io.hpp @@ -0,0 +1,23 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models matrix_io.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _MATRIX_IO_HPP_ +#define _MATRIX_IO_HPP_ + +#include +#include + +class MatrixIO { + public: + MatrixIO() {} + ~MatrixIO() {} + + 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.h b/include/particle_filter/pca.h deleted file mode 100644 index cb47d7c..0000000 --- a/include/particle_filter/pca.h +++ /dev/null @@ -1,47 +0,0 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models pca.h #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#ifndef _PCA_H_ -#define _PCA_H_ - -#include -#include - -namespace gmms { -class PCA { -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_; - } - - // 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_; -}; -} // namespace gmms - -#endif // _PCA_H_ diff --git a/include/particle_filter/pca.hpp b/include/particle_filter/pca.hpp new file mode 100644 index 0000000..7671ac5 --- /dev/null +++ b/include/particle_filter/pca.hpp @@ -0,0 +1,38 @@ +// #########################################################// +// # #// +// # gaussian_mixture_models pca.h #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#ifndef _PCA_HPP_ +#define _PCA_HPP_ + +#include +#include + +namespace gmms { +class PCA { + public: + // constructors + PCA() { num_components_ = 0; } + explicit 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_; } + + // 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_; +}; +} // namespace gmms + +#endif // _PCA_HPP_ diff --git a/src/CRandomNumberGenerator.cpp b/src/CRandomNumberGenerator.cpp index 1180f8b..5bcbc17 100644 --- a/src/CRandomNumberGenerator.cpp +++ b/src/CRandomNumberGenerator.cpp @@ -1,9 +1,7 @@ #include #include #include - - -#include +#include using namespace particle_filter; @@ -19,44 +17,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..1a5095d 100644 --- a/src/gaussian.cpp +++ b/src/gaussian.cpp @@ -1,126 +1,108 @@ -//#########################################################// -//# #// -//# 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_mean = mean_(0, 0); + float y_mean = mean_(1, 0); + // TODO: save stepsize + + for (int y_step = 0; y_step < stepcount; y_step++) { + float y = y0 + (y_delta / stepcount * y_step); + for (int x_step = 0; x_step < 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) + // / 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..c370a8f 100644 --- a/src/gaussian_mixture_model.cpp +++ b/src/gaussian_mixture_model.cpp @@ -1,320 +1,290 @@ -//#########################################################// -//# #// -//# 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 log_likelihood = 0.0; + double delta = 0.0; + int iteration = 0; + + do { + expectation(dataset); + maximization(dataset); + double 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) { + 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) { + double 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(); - 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; - } + 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); - initialized_ = true; -} + 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); -Eigen::MatrixXd GaussianMixtureModel::save() { - if (!initialized_) { - throw std::runtime_error(notinitialized_()); - } + gaussian_vec_[i].setMeanCovariance(mean, covariance); + prior_vec_[i] = prior; - 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); + idx += size; + } - 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; - } + 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_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++) { + float y = y0 + (y_stepsize * y_step); + for (int x_step = 0; x_step < stepcount; x_step++) { + float 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..166fbea 100644 --- a/src/gmm_classifier.cpp +++ b/src/gmm_classifier.cpp @@ -1,232 +1,203 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_classifier.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_classifier.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#include #include #include #include +#include +#include #include #include #include -#include +namespace gmms { +void GMMClassifier::train(const Eigen::MatrixXd& dataset, bool evaluate_bic, int gmm_components) { + 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()); -#include -#include + gmm_vec_.clear(); + gmm_vec_.resize(classes_.size()); -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; - } - - 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); - } - } - - int num_components; - double bic = 0.0; - bool first_trial = true; - - 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(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; - } - } - } - - std::cout << "\t\tFinal class model with " - << gmm_vec_[class_idx]->numComponents() << "; "; - std::cout << "BIC " << bic << std::endl; - ++class_idx; + 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; } - trained_ = true; -} + int class_data_idx = 0; -std::vector GMMClassifier::predict(const Eigen::MatrixXd& dataset) const { - if (!trained_) { - throw std::runtime_error(nottrained_()); + 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); + } + } + + int num_components; + double bic = 0.0; + bool first_trial = true; + + 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 (const 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; + 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_()); + } - int num_threads = omp_get_max_threads(); - int thread_dataset_size = floor(double(dataset_size) / double(num_threads)); + if (dataset.cols() != input_size_) { + throw std::runtime_error(dimensionality_mismatch_()); + } - int thread_log_likelihood[num_threads]; + int dataset_size = dataset.rows(); + std::vector assignments(dataset_size); #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; + first = false; + } } + } - return assignments; + return assignments; } -void GMMClassifier::load(const std::string filename) { - MatrixIO mio; - Eigen::MatrixXd model; +void GMMClassifier::load(const std::string& filename) { + 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; +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; - } + 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..fc4e989 100644 --- a/src/gmm_regressor.cpp +++ b/src/gmm_regressor.cpp @@ -1,192 +1,173 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models gmm_regressor.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// +// #########################################################// +// # #// +// # gaussian_mixture_models gmm_regressor.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// + +#include #include #include +#include +#include #include #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 (const 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; - - 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; - } - - double trial_bic = model->bayesianInformationCriterion(dataset); + double trial_bic = model->bayesianInformationCriterion(dataset); - std::cout << "\t\t\t\tTrial BIC: " << trial_bic << std::endl; + 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 || trial_bic < bic) { + bic = trial_bic; + gmm_ = model; + 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; +void GMMRegressor::load(const std::string& filename) { + 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); +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); } } // namespace gmms diff --git a/src/k_means.cpp b/src/k_means.cpp index 4650af6..6731c5e 100644 --- a/src/k_means.cpp +++ b/src/k_means.cpp @@ -1,126 +1,119 @@ -//#########################################################// -//# #// -//# 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); - 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..7d0a8eb 100644 --- a/src/matrix_io.cpp +++ b/src/matrix_io.cpp @@ -1,83 +1,79 @@ -//#########################################################// -//# #// -//# 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..f5e8b17 100644 --- a/src/pca.cpp +++ b/src/pca.cpp @@ -1,42 +1,36 @@ -//#########################################################// -//# #// -//# gaussian_mixture_models pca.cpp #// -//# Roberto Capobianco #// -//# #// -//#########################################################// - -#include +// #########################################################// +// # #// +// # gaussian_mixture_models pca.cpp #// +// # Roberto Capobianco #// +// # #// +// #########################################################// #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