[ { "path": ".gitattributes", "content": "# Auto detect text files and perform LF normalization\n* text=auto\n\n# Custom for Visual Studio\n*.cs diff=csharp\n*.sln merge=union\n*.csproj merge=union\n*.vbproj merge=union\n*.fsproj merge=union\n*.dbproj merge=union\n\n# Standard to msysgit\n*.doc\t diff=astextplain\n*.DOC\t diff=astextplain\n*.docx diff=astextplain\n*.DOCX diff=astextplain\n*.dot diff=astextplain\n*.DOT diff=astextplain\n*.pdf diff=astextplain\n*.PDF\t diff=astextplain\n*.rtf\t diff=astextplain\n*.RTF\t diff=astextplain\n" }, { "path": ".github/ISSUE_TEMPLATE/bug_report.md", "content": "---\nname: Bug report\nabout: Create a report to help us improve\ntitle: ''\nlabels: ''\nassignees: ''\n\n---\n\n**Describe the bug**\nA clear and concise description of what the bug is.\n\n**To Reproduce**\nSteps to reproduce the behavior:\n1. Go to '...'\n2. Click on '....'\n3. Scroll down to '....'\n4. See error\n\n**Expected behavior**\nA clear and concise description of what you expected to happen.\n\n**Screenshots**\nIf applicable, add screenshots to help explain your problem.\n\n**Desktop (please complete the following information):**\n - OS: [e.g. iOS]\n - Browser [e.g. chrome, safari]\n - Version [e.g. 22]\n\n**Smartphone (please complete the following information):**\n - Device: [e.g. iPhone6]\n - OS: [e.g. iOS8.1]\n - Browser [e.g. stock browser, safari]\n - Version [e.g. 22]\n\n**Additional context**\nAdd any other context about the problem here.\n" }, { "path": ".github/ISSUE_TEMPLATE/custom.md", "content": "---\nname: Custom issue template\nabout: Describe this issue template's purpose here.\ntitle: ''\nlabels: ''\nassignees: ''\n\n---\n\n\n" }, { "path": ".github/ISSUE_TEMPLATE/feature_request.md", "content": "---\nname: Feature request\nabout: Suggest an idea for this project\ntitle: ''\nlabels: ''\nassignees: ''\n\n---\n\n**Is your feature request related to a problem? Please describe.**\nA clear and concise description of what the problem is. Ex. I'm always frustrated when [...]\n\n**Describe the solution you'd like**\nA clear and concise description of what you want to happen.\n\n**Describe alternatives you've considered**\nA clear and concise description of any alternative solutions or features you've considered.\n\n**Additional context**\nAdd any other context or screenshots about the feature request here.\n" }, { "path": ".github/workflows/macos.yml", "content": "name: macOS\n\non:\n push:\n branches:\n - dev\n - master\n pull_request:\n\njobs:\n xcode:\n runs-on: macos-10.15\n strategy:\n matrix:\n xcode: [12.4, 12.3, 12.2, 12.1.1, 12.1, 12]\n env:\n DEVELOPER_DIR: /Applications/Xcode_${{ matrix.xcode }}.app/Contents/Developer\n\n steps:\n - uses: actions/checkout@v2\n # install dependencies\n - name: boost\n run: brew install boost \n - name: cmake\n run: cmake -S . -B build -D CMAKE_BUILD_TYPE=Debug -DJSON_BuildTests=On -DJSON_FastTests=ON\n - name: build\n run: cmake --build build --parallel 10\n - name: test\n run: cd build ; ctest -j 10 --output-on-failure\n\n" }, { "path": ".github/workflows/ubuntu.yml", "content": "name: Ubuntu\n\non:\n push:\n branches:\n - dev\n - master\n pull_request:\n\njobs:\n ci_test_clang:\n runs-on: ubuntu-latest\n steps:\n # install dependencies\n - name: boost\n run: sudo apt-get update && sudo apt-get install libboost-all-dev\n - uses: actions/checkout@v2\n - name: install_ninja\n run: |\n sudo apt update\n sudo apt install ninja-build\n shell: bash\n - name: install_clang\n run: |\n wget https://apt.llvm.org/llvm.sh\n chmod +x llvm.sh\n sudo ./llvm.sh 11\n sudo apt-get install clang-tools-11\n shell: bash\n - name: cmake\n run: cmake -S . -B build -DJSON_CI=On\n - name: build\n run: cmake --build build\n\n ci_test_gcc:\n runs-on: ubuntu-latest\n steps:\n - uses: actions/checkout@v2\n # install dependencies\n - name: boost\n run: sudo apt-get update && sudo apt-get install libboost-all-dev\n - name: cmake\n run: cmake -S . -B build -DJSON_CI=On\n - name: build\n run: cmake --build build\n" }, { "path": ".gitignore", "content": "# Windows image file caches\r\nThumbs.db\r\nehthumbs.db\r\n\r\n# Folder config file\r\nDesktop.ini\r\n\r\n# Recycle Bin used on file shares\r\n$RECYCLE.BIN/\r\n\r\n# Windows Installer files\r\n*.cab\r\n*.msi\r\n*.msm\r\n*.msp\r\n\r\n# =========================\r\n# Operating System Files\r\n# =========================\r\n\r\n# OSX\r\n# =========================\r\n\r\n.DS_Store\n.AppleDouble\n.LSOverride\n\n# Icon must end with two \\r\nIcon\r\r\n\n# Thumbnails\n._*\n\n# Files that might appear on external disk\n.Spotlight-V100\n.Trashes\n\n# Directories potentially created on remote AFP share\n.AppleDB\n.AppleDesktop\nNetwork Trash Folder\nTemporary Items\n.apdisk\n\r\n*.sdf\r\n*.pdb\r\n*.sbr\r\n*.obj\r\n*.tlog\r\n*.ilk\r\n*.log\r\n*.idb\r\n*.bsc\r\n*.opensdf\n\n# Prerequisites\n*.d\n\n# Compiled Object files\n*.slo\n*.lo\n*.o\n*.obj\n\n# Precompiled Headers\n*.gch\n*.pch\n\n# Compiled Dynamic libraries\n*.so\n*.dylib\n*.dll\n\n# Fortran module files\n*.mod\n*.smod\n\n# Compiled Static libraries\n*.lai\n*.la\n*.a\n*.lib\n\n# Executables\n*.exe\n*.out\n*.app\n\nCMakeLists.txt.user\nCMakeCache.txt\nCMakeFiles\nCMakeScripts\nTesting\nMakefile\ncmake_install.cmake\ninstall_manifest.txt\ncompile_commands.json\nCTestTestfile.cmake\n_deps\n\n[Bb][Uu][Ii][Ll][Dd]/\n.vscode/\n" }, { "path": "CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.7)\nproject(Navigation-core)\n\ninclude(FetchContent)\n\nFetchContent_Declare(json\n GIT_REPOSITORY https://github.com/ArthurSonzogni/nlohmann_json_cmake_fetchcontent.git\n GIT_TAG v3.10.3)\n\nFetchContent_GetProperties(json)\nif(NOT json_POPULATED)\n FetchContent_Populate(json)\n add_subdirectory(${json_SOURCE_DIR} ${json_BINARY_DIR} EXCLUDE_FROM_ALL)\nendif()\n\nFetchContent_Declare(\n Eigen\n GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git\n GIT_TAG master\n GIT_SHALLOW TRUE\n GIT_PROGRESS TRUE)\nset(EIGEN_BUILD_DOC OFF)\nset(BUILD_TESTING OFF)\nset(EIGEN_BUILD_PKGCONFIG OFF)\nset( OFF)\n\nFetchContent_MakeAvailable(Eigen)\n\nset(BOOST_ENABLE_CMAKE ON)\nset(Boost_USE_STATIC_LIBS ON)\n\nfind_package(Boost REQUIRED)\nIF (Boost_FOUND)\n INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})\n message(${Boost_INCLUDE_DIR})\n include_directories(${Boost_SOURCE_DIR})\nENDIF ()\n\n#tests:navigation_test\nset(\"target\" \"navigation_test\")\nlist(APPEND \"${target}__cxx_srcs\"\n \"tests/navigation_test.cpp\"\n \"tools/verification/helpers.cpp\")\nadd_executable(\"${target}\" ${${target}__cxx_srcs})\ntarget_include_directories(\"${target}\" PRIVATE\n \"tests/include/\"\n \"tests/build/\"\n \"include/\")\nset_property(TARGET \"${target}\" PROPERTY CXX_STANDARD 17)\nset_target_properties(\"${target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\nset_target_properties(\"${target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions \")\ntarget_link_libraries(\"${target}\" PRIVATE\n \"navigation-core\"\n nlohmann_json::nlohmann_json\n Eigen3::Eigen)\n\n\n#navigation-core:copy_public_include_dir\nset(\"target\" \"navigation-core_copy_public_include_dir\")\nset(\"${target}__other_srcs\" \"include\")\nset(\"${target}__output\" \"tests/build/obj/include\")\nadd_custom_command(OUTPUT ${${target}__output}\n COMMAND ${CMAKE_COMMAND} -E copy_directory \"include\" \"tests/build/obj/include\"\n DEPENDS ${${target}__other_srcs}\n WORKING_DIRECTORY \"tests/build/\"\n COMMENT \"Copy ${target}\"\n VERBATIM)\nadd_custom_target(\"${target}\" SOURCES ${${target}__other_srcs} DEPENDS ${${target}__output})\n\n\n#navigation-core:navigation-core\nset(\"target\" \"navigation-core\")\nlist(APPEND \"${target}__cxx_srcs\"\n \"src/geometry.cpp\"\n \"src/triangulation.cpp\"\n \"src/knn/knn_utils.cpp\"\n \"src/likelihood/likelihood.cpp\"\n \"src/likelihood/likelihood_radiomap.cpp\"\n \"src/measurements/measurement_preprocessor.cpp\"\n \"src/position_estimator/position_estimator_knn.cpp\"\n \"src/position_estimator/position_estimator_zone.cpp\"\n \"src/position_estimator/position_estimator_outdoor.cpp\"\n \"src/position_postprocessor/navigation_time_smoother.cpp\"\n \"src/position_postprocessor/polynomial_fit.cpp\"\n \"src/position_postprocessor/position_postprocessor.cpp\"\n \"src/position_postprocessor/position_smoother_ab.cpp\"\n \"src/position_postprocessor/position_smoother_lstsq.cpp\"\n \"src/sensor_fusion/complementary_filter.cpp\"\n \"src/sensor_fusion/quaternion.cpp\"\n \"src/sensor_fusion/pedometer.cpp\"\n \"src/sensor_fusion/sensor_fusion.cpp\"\n \"src/level_estimator/barometer.cpp\"\n \"src/level_estimator/level_estimator.cpp\"\n \"src/level_estimator/level_estimator_radiomap.cpp\"\n \"src/level_estimator/level_estimator_transmitters.cpp\"\n \"src/level_estimator/level_history.cpp\"\n \"src/navigation_client_impl.cpp\"\n \"src/barriers_geometry_builder.cpp\"\n \"src/level.cpp\"\n \"src/point.cpp\"\n \"src/level_collector.cpp\"\n \"src/level_geometry.cpp\"\n \"src/radiomap.cpp\"\n \"src/vector3d.cpp\"\n \"tools/verification/helpers.cpp\"\n \"src/trilateration.cpp\")\nadd_library(\"${target}\" STATIC ${${target}__cxx_srcs})\n# add_dependencies(\"${target}\" boost_geometry)\ntarget_include_directories(\"${target}\" PRIVATE\n \"include/\"\n \"tests/build/\")\nset_property(TARGET \"${target}\" PROPERTY CXX_STANDARD 17)\nset_target_properties(\"${target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\nset_target_properties(\"${target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions \")\ntarget_link_libraries(\"${target}\" PRIVATE nlohmann_json::nlohmann_json\n\t\t\t\t\t Eigen3::Eigen)\n" }, { "path": "CODE_OF_CONDUCT.md", "content": "#

Navigine Open Source Code of Conduct

\n\nThis code of conduct provides guidance on participation in Navigine-managed open source communities and denotes the process for reporting unacceptable behavior. We are committed to providing a welcoming and inspiring community for all. People violating this code of conduct may be banned from the community.\n\n## Our Standards\n\nExamples of behavior that contributes to a positive environment for our community include:\n- Demonstrating empathy and kindness toward other people\n- Being respectful of differing opinions, viewpoints, and experiences\n- Giving and gracefully accepting constructive feedback\n- Accepting responsibility and apologizing to those affected by our mistakes, and learning from the experience\n- Focusing on what is best not just for us as individuals, but for the overall community\n\nExamples of unacceptable behavior include:\n- The use of sexualized language or imagery, and sexual attention or advances of any kind\n- Trolling, insulting or derogatory comments, and personal or political attacks\n- Public or private harassment\n- Publishing others’ private information, such as a physical or email address, without their explicit permission\n- Other conduct which could reasonably be considered inappropriate in a professional setting\n- Advocating for or encouraging any of the above behaviors\n\n## Enforcement Responsibilities\n\nCommunity leaders are responsible for clarifying and enforcing our standards of acceptable behavior and will take appropriate and fair corrective action in response to any behavior that they deem inappropriate, threatening, offensive, or harmful.\nCommunity leaders have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, and will communicate reasons for moderation decisions when appropriate.\n\n## Scope\n\nThis Code of Conduct applies within all community spaces, and also applies when an individual is officially representing the community in public spaces. Examples of representing our community include using an official e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event.\n\n## Enforcement\n\nInstances of abusive, harassing, or otherwise unacceptable behavior may be reported to the community leaders responsible for enforcement at info@navigine.com. All complaints will be reviewed and investigated promptly and fairly.\nAll community leaders are obligated to respect the privacy and security of the reporter of any incident.\n\n## Attribution\n\nThis Code of Conduct is adapted from the Contributor Covenant, version 2.1, available at https://www.contributor-covenant.org/version/2/1/code_of_conduct.html.\nFor answers to common questions about this code of conduct, see the FAQ at https://www.contributor-covenant.org/faq. Translations are available at https://www.contributor-covenant.org/translations.\n" }, { "path": "CONTRIBUTING.md", "content": "#

Contributing to Navigine Routing Library

\n\nThe easiest way to help out is to submit bug reports and feature requests on our [issues](https://github.com/Navigine/Indoor-Routing-Library/issues) page.\n\nWhen submitting a bug report, please include:\n\n - The device and operating system version that produced the bug\n - The version or commit of Routing Library that produced the bug\n - Steps required to recreate the issue\n - What happened\n - What you expected to happen\n\nThanks!\n\n## Development\n\nTo develop for the library, it is usually easiest to build and test your changes using either the Mac OS X or Ubuntu desktop targets.\n\n## Code Style\n\nIn general, code changes should follow the style of the surrounding code.\n\nWhen in doubt, you can use the provided clang-format style file for automatic styling.\n\nInstall clang-format (available through brew or apt-get):\n```\nbrew install clang-format\n```\nor\n```\nsudo apt-get install clang-format\n```\n\nRun clang-format with specified style (use -i to modify the contents of the specified file):\n```\nclang-format -i -style=file [file]\n```\n" }, { "path": "LICENSE", "content": "MIT License\n\nCopyright (c) 2021 Navigine Corporation\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE.\n" }, { "path": "README.md", "content": "\n\nIndoor-navigation-algorithms \n[![Build](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/ubuntu.yml/badge.svg)](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/ubuntu.yml) [![Build](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/macos.yml/badge.svg)](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/macos.yml)\n============================\n\n## Useful Links\n\n- Refer to the [Navigine official website](https://navigine.com/) for complete list of downloads, useful materials, information about the company, and so on.\n- [Get started](http://locations.navigine.com/login) with Navigine to get full access to Navigation services, SDKs, and applications.\n- Refer to the Navigine [User Manual](http://docs.navigine.com/) for complete product usage guidelines.\n- Find company contact information at the official website under [Contacts](https://navigine.com/contacts/) tab.\n- Get support, contribute feedback, and connect with developers—visit [the Navigine Community](https://community.navigine.com/t/indoor-positioning-and-navigation-algorithms/173).\n\n## Contents\n\nThis repository consists of the following components\n\n* [src](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/src) - the source code of Navigine positioning algorithms\n\n* [standalone_algorithms](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/standalone_algorithms) - set of algorithms used in navigation\n\n* [tests](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/tests) - test and utilities for the evaluation of source code quality\n\n* [tools](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/tools/verification) - utilities for data extraction from map and settings\n\n## Creating Navigation Client\n\nHere are some examples to give you an idea how to use [Navigation Client](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/blob/master/include/navigine/navigation-core/navigation_client.h).\n\nFirst, create variables that will store your data.\n```c++\n // variables for storing location elements\n navigine::navigation_core::GeoLevel geoLevel;\n std::shared_ptr levelCollector;\n navigine::navigation_core::NavigationSettings navigationSettings;\n std::shared_ptr navClient;\n```\n\nAdd transmitters (Beacon, Eddystone, WIFI, WIFI-RTT, etc.) as `GeoTransmitters` from your location.\n\nThen they will be converted to `XYTransmitters` for internal evaluations\n \n Parameters:\n - id - identifier of transmitter like (major,minor,uuid) for beacon, (namespaceId,instanceId) for eddystone, mac for WIFI;\n - point - latitude and longitude as GeoPoint;\n - pathlossModel - A, B and power of transmitter as `PathlossModel` struct variable;\n - type - type of transmitter, like Beacon, Eddystone, WIFI, WIFI-RTT, etc..\n\nInside for loop add all the transmitters from your location. Here is an example of adding one element\n\n```c++\n geoLevel.transmitters.emplace_back(transmitterId,\n navigine::navigation_core::GeoPoint(latitude, longitude),\n navigine::navigation_core::PathlossModel{A, B, power},\n navigine::navigation_core::TransmitterType::BEACON);\n\n```\n\nCreate geometry of the level using method `getGeometry` from [barriers_geometry_builder.h](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/blob/master/include/navigine/navigation-core/barriers_geometry_builder.h) file. Geometry could be created using the barriers and the size of the level\n \n Parameters:\n - barrierList - list of polygons, where each polygon describes the barrier of the level;\n - allowedArea - polygon, which is created using width and height of the map;\n\nCreate the list of Polygons which will describe all the barriers\n\n```c++\n std::list barriersList;\n for (size_t i = 0; i < barriers.size(); ++i)\n {\n auto coords = barriers.at(i);\n navigine::navigation_core::Polygon barrier;\n for (const auto& coord : coords)\n boost::geometry::append(barrier, navigine::navigation_core::Point(coord.first, coord.second));\n\n barriersList.push_back(barrier);\n }\n```\n\nCreate the polygon of allowed area\n\n```c++\n navigine::navigation_core::Polygon levelArea;\n auto boundingBox = navigation_core::Box(\n navigation_core::Point(leftMin.latitude, leftMin.longitude),\n navigation_core::Point(rightMax.latitude, rightMax.longitude));\n boost::geometry::convert(boundingBox, allowedArea);\n geoLevel.geometry = navigine::navigation_core::getGeometry(barriersList, levelArea);\n```\n\nCreate `LevelCollector` using method `createLevelCollector` and add all available geo levels.\n\n```c++\n levelCollector = navigine::navigation_core::createLevelCollector();\n levelCollector->addGeoLevel(geoLevel);\n```\n\nCreate `NavigationSettings`, with two parameters - level settings and common settings. (You can find them in navigation.xml i.e.)\n\nCreate `NavigationClient` using method `createNavigationClient` which will take as arguments level collector and navigation settings.\n\n```c++\n navClient = navigine::navigation_core::createNavigationClient(levelCollector, navigationSettings);\n```\n\n## Navigation test\n\nThe example of navigation is presented in a file [Navigation Test](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/blob/master/tests/navigation_test.cpp).\nThe test application takes 3 parameters:\n- path to the file of the location in` geojson` format\n- path to the file of logs gathered in this location in `json` format\n- path to the settings file in `json` format in which parameters of the algorithm are specified\n\n### Build\n\nIn order to build the test application go to the root directory of the repository and execute the following commands\n\n``` sh\ncmake -H. -Bbuild\ncmake --build build\n```\n\nTo run the tests\n\n``` sh\ncd build/\n./navigation_test location.geojson log.json settings.json\n```\n\n## Evaluate the navigation\n\nQuality of navigation can be evaluated via calculation of different navigational errors.\nThe most important among them are presented in the following table\n\n| Error | Description |\n| :--- | :---- |\n| `nav_err` | Average navigation error in meters |\n| `q_75_err` | 75% quantile of all the measurements have navigation error less than the value of `nav_75_err` |\n| `avr_fi` | Average angle error in degrees |\n| `level_err` | Percentage of wrongly determined levels |\n| `exec_time` | Execution time in seconds |\n\n## Navigation Algorithms explanation\n\nHere are lectures on navigational algorithms used in this repository\n- [Navigine's demo of WIFI-RTT](https://www.youtube.com/watch?v=VAzMhld3bH0&t)\n- [Pedometer Algorithm Explained](https://www.youtube.com/watch?v=cpwrwPTqMac&t)\n- [Complementary Filter](https://www.youtube.com/watch?v=49mJcFc2xXw)\n- [Particle Filter](https://www.youtube.com/watch?v=JFZ-HARdZA8)\n" }, { "path": "include/navigine/navigation-core/CHANGELOG.md", "content": "# Backlog of changes in navigation protocol\n\n## Version 1.0\n\n* Added first version of the navigation protocol\n\n## Version 1.1\n\n* Deleted accuracy\n* Deleted battery\n* Deleted height from LOCATION\n" }, { "path": "include/navigine/navigation-core/README.md", "content": "Message format\n============================\n\nEach message consists of one line of json format and describes an individual measurement.\nThe line types are shown below.\n\n### Type 101 - ACCELEROMETER\n\n```{“timestamp”: , “type”: 101, “values”: [value_x, value_y, value_z]}```\n\n### Type 102 - GYROSCOPE\n\n```{“timestamp”: , “type”: 102, “values”: [value_x, value_y, value_z]}```\n\n### Type 103 - MAGNETOMETER\n\n```{“timestamp”: , “type”: 103, “values”: [value_x, value_y, value_z]}```\n\n### Type 104 - BAROMETER\n\n```{“timestamp”: , “type”: 104, “values”: [pressure, pressure, pressure]}```\n\n### Type 105 - ORIENTATION\n\n```{“timestamp”: , “type”: 105, “values”: [value_x, value_y, value_z]}```\n\n### Type 106 - LOCATION\n\n```{“timestamp”: , “type”: 106, “values”: [lat, lon, accuracy]}```\n\n### Type 201 - WIFI\n\n```{“timestamp”: , “type”: 201, “bssid”: , “rssi”: }```\n\n### Type 202 - BLE\n\n```{“timestamp”: , “type”: 202, “bssid”: , “rssi”: }```\n\n### Type 203 - BEACON\n\n```{“timestamp”: , “type”: 203, “bssid”: , “rssi”: , “power”: }```\n\n### Type 204 - EDDYSTONE\n\n```{“timestamp”: , “type”: 204, “bssid”: , “rssi”: , “power”: }```\n\n### Type 205 - WIFI RTT\n\n```{“timestamp”: , “type”: 205, “bssid”: , “distance”: , “stddev”: }```\n\n### Type 300 - NMEA\n\n```{“timestamp”: , “type”: 300, “sentence_number”: , “num_sats”: }```\n\n### Type 900 - Comment\n\n```{“timestamp”: , “type”: 900, “comment”: }```\n" }, { "path": "include/navigine/navigation-core/barriers_geometry_builder.h", "content": "/** barriers_geometry_builder.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H\n#define NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H\n\n#include \"level_geometry.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nLevelGeometry getGeometry(const std::list& allowedArea);\n\n} } // namespace geometry::navigine::navigation_core\n\n#endif // NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H\n" }, { "path": "include/navigine/navigation-core/boost_geometry_adaptation.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n\n#include \n#include \"xy_particle.h\"\n\nBOOST_GEOMETRY_REGISTER_POINT_2D(navigine::navigation_core::XYPoint, double, boost::geometry::cs::cartesian, x, y);\nBOOST_GEOMETRY_REGISTER_POINT_2D(navigine::navigation_core::XYParticle, double, boost::geometry::cs::cartesian, x, y);\n\nnamespace navigine {\nnamespace navigation_core {\n\nusing Point = boost::geometry::model::d2::point_xy;\nusing Box = boost::geometry::model::box;\nusing Ring = boost::geometry::model::ring;\nusing Polygon = boost::geometry::model::polygon;\nusing Multipolygon = boost::geometry::model::multi_polygon;\nusing Linestring = boost::geometry::model::linestring;\nusing Multilinestring = boost::geometry::model::multi_linestring;\nusing BoxIdx = std::pair;\nusing RTree = boost::geometry::index::rtree>;\n\ninline std::pair getWidthHeightPair(const Box& box)\n{\n double width = box.max_corner().x() - box.min_corner().x();\n double height = box.max_corner().y() - box.min_corner().y();\n return std::make_pair(width, height);\n}\n\n} } //namespace navigine::navigation_core\n" }, { "path": "include/navigine/navigation-core/declare_identifier.h", "content": "#pragma once\n\n#include \n\n#define DECLARE_IDENTIFIER(IdentifierType) \\\nstruct IdentifierType { \\\n std::string value; \\\n inline IdentifierType() = default; \\\n inline explicit IdentifierType(const std::string& value): value(value) {} \\\n inline bool isValid() const { return !value.empty(); } \\\n inline bool operator< (const IdentifierType& _id)const { return value < _id.value; } \\\n inline bool operator!= (const IdentifierType& _id)const { return value != _id.value; } \\\n inline bool operator== (const IdentifierType& _id)const { return value == _id.value; } \\\n}; \\\n\\\nstruct Hasher##IdentifierType \\\n{ \\\n std::size_t operator()(const IdentifierType& k) const \\\n { \\\n return std::hash()(k.value); \\\n } \\\n};\n" }, { "path": "include/navigine/navigation-core/geolevel.h", "content": "/** geolevel.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_GEOLEVEL_H\n#define NAVIGINE_GEOLEVEL_H\n\n#include \n#include \n#include \n#include \n\n#include \"level.h\"\n#include \"level_geometry.h\"\n#include \"boost_geometry_adaptation.h\"\n#include \"graph.h\"\n#include \"reference_point.h\"\n#include \"transmitter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct GeoLevel\n{\n LevelId id;\n Geo3DTransmitters transmitters;\n GeoReferencePoints referencePoints;\n LevelGeometry geometry;\n Graph graph;\n double altitude;\n};\n\ntypedef std::vector > GeoLevels;\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_GEOLEVEL_H\n" }, { "path": "include/navigine/navigation-core/graph.h", "content": "/** graph.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_GRAPH_H\n#define NAVIGINE_GRAPH_H\n\n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\ntemplate \nclass Graph\n{\npublic:\n struct Vertex\n {\n int id;\n Point point;\n };\n\npublic:\n typedef typename std::vector::const_iterator VertexIterator;\n typedef std::set >::const_iterator EdgeIterator;\n\npublic:\n Graph() {}\n\n bool hasVertex(int id) const\n {\n return id >= 0 && id < static_cast(mVertices.size());\n }\n\n const Vertex& vertex(int id) const\n {\n return mVertices.at(id);\n }\n\n const Vertex getVertex(int id) const\n {\n return mVertices.at(id);\n }\n\n const VertexIterator vertexBegin() const\n {\n return mVertices.begin();\n }\n\n const VertexIterator vertexEnd() const\n {\n return mVertices.end();\n }\n\n const EdgeIterator edgesBegin() const\n {\n return mEdges.begin();\n }\n\n const EdgeIterator edgesEnd() const\n {\n return mEdges.end();\n }\n\n Vertex addVertex(const Point& p)\n {\n Vertex v = {static_cast(mVertices.size()), p};\n mVertices.push_back(v);\n return v;\n }\n\n void addEdge(int v1, int v2)\n {\n if (!hasVertex(v1) || !hasVertex(v2)) {\n throw std::logic_error(\"vertex does not exist!\");\n }\n\n std::pair p = v1 < v2 ? std::make_pair(v1, v2) : std::make_pair(v2, v1);\n mEdges.insert(std::move(p));\n }\n\nprivate:\n std::vector mVertices;\n std::set> mEdges; // contains indices of elements in vertices vector\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_GRAPH_H\n" }, { "path": "include/navigine/navigation-core/graph_particle.h", "content": "/** graph_particle.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_GRAPH_PARTICLE_H\n#define NAVIGINE_GRAPH_PARTICLE_H\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct GraphParticle : XYParticle\n{\n GraphParticle()\n : edgeId(0)\n , fromVertex(0)\n , toVertex(0)\n , percent(0)\n { }\n\n GraphParticle(int _edge, int _fromVertex, int _toVertex,\n double _percent, double _x, double _y, double _angle)\n : XYParticle(_x, _y, _angle)\n , edgeId(_edge)\n , fromVertex(_fromVertex)\n , toVertex(_toVertex)\n , percent(_percent)\n { }\n\n int edgeId;\n int fromVertex;\n int toVertex;\n double percent;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_GRAPH_PARTICLE_H\n" }, { "path": "include/navigine/navigation-core/level.h", "content": "/** level.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_LEVEL_H\n#define NAVIGINE_LEVEL_H\n\n#include \n#include \n#include \n#include \n\n#include \"boost_geometry_adaptation.h\"\n#include \"level_geometry.h\"\n#include \"graph.h\"\n#include \"radiomap.h\"\n#include \"reference_point.h\"\n#include \"transmitter.h\"\n#include \"navigation_input.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nDECLARE_IDENTIFIER(LevelId)\n\nclass Level\n{\npublic:\n Level(const LevelId& id, const GeoPoint& bindingPoint)\n : mId (id)\n , mBindingPoint (bindingPoint)\n {}\n\n void addTransmitters(const XYZTransmitters& transmitters);\n void setReferencePoints(const XYReferencePoints& referencePoints);\n void setGraph(const Graph& graph);\n void setGeometry(const LevelGeometry &levelGeometry);\n\n const LevelId& id() const;\n const GeoPoint& bindingPoint() const;\n\n bool containsTransmitter(const TransmitterId& txId) const;\n const Transmitter& transmitter(const TransmitterId& txId) const;\n\n const Radiomap& radiomap() const;\n const Graph& graph() const;\n const LevelGeometry& geometry() const;\n\nprivate:\n const LevelId mId;\n const GeoPoint mBindingPoint;\n\n std::unordered_map, HasherTransmitterId> mTransmitters;\n Radiomap mRadiomap;\n LevelGeometry mGeometry;\n Graph mGraph;\n};\n\ntypedef std::vector >::const_iterator LevelIterator;\ntypedef std::vector > Levels;\n\nRadioMeasurementsData getLevelRadioMeasurements(\n const Level& level,\n const RadioMeasurementsData& radioMsr);\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_LEVEL_H\n" }, { "path": "include/navigine/navigation-core/level_collector.h", "content": "/** level_collector.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_LEVEL_COLLECTOR_H\n#define NAVIGINE_LEVEL_COLLECTOR_H\n\n#include \n#include \n#include \n#include \n#include \n\n#include \"boost_geometry_adaptation.h\"\n#include \"graph.h\"\n#include \n#include \"geolevel.h\"\n#include \"reference_point.h\"\n#include \"transmitter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass LevelCollector\n{\npublic:\n LevelCollector() {}\n bool hasLevel(const LevelId& levelId) const;\n const Level& level(const LevelId& levelId) const;\n void addGeoLevel(const GeoLevel& geoLevel);\n const std::vector& levels() const;\n boost::optional findLevelByTransmitterId(const TransmitterId& transmitterId) const;\n\nprivate:\n std::vector mLevels;\n std::unordered_map mLevelsIndices;\n};\n\nstd::shared_ptr createLevelCollector();\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_LEVEL_COLLECTOR_H\n" }, { "path": "include/navigine/navigation-core/level_geometry.h", "content": "/** level.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_LEVEL_GEOMETRY_H\n#define NAVIGINE_LEVEL_GEOMETRY_H\n\n#include \"boost_geometry_adaptation.h\"\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nclass LevelGeometry\n{\npublic:\n LevelGeometry() {};\n LevelGeometry(const Multipolygon& allowedArea, const Box& box);\n\n bool isPathTouchesBarriers(const Linestring& path) const;\n\n const Multipolygon& allowedArea() const;\n const Box& boundingBox() const;\n\n\nprivate:\n Multipolygon mAllowedArea;\n Box mBoundingBox;\n RTree mTree;\n std::vector mInnerBarriers;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_LEVEL_GEOMETRY_H\n" }, { "path": "include/navigine/navigation-core/location_point.h", "content": "/** location_point.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_LOCATION_POINT_H\n#define NAVIGINE_LOCATION_POINT_H\n\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct LocationPoint\n{\n LevelId level;\n double x;\n double y;\n};\n\ntypedef std::vector PolyLine;\n\n} } // namespace navigine::navigation_core\n\n#endif\n" }, { "path": "include/navigine/navigation-core/logger.h", "content": "/** logger.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_LOGGER_H\n#define NAVIGINE_LOGGER_H\n\n#include \n#include \n\n#define LOG_MESSAGE(lvl, fmt, ...) \\\n do { \\\n navigine::navigation_core::_Logger.print(lvl, __FILE__, __LINE__, fmt, ##__VA_ARGS__); \\\n } while (false)\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass Logger\n{\npublic:\n Logger() = default;\n\n // Function is used for specifying a log file.\n // If @file parameter is null, function sets a default log file.\n // Otherwise, function sets a separate log file for the corresponding module.\n void setLogFile(const std::string& logFile, const char* file = 0);\n\n // Function is used for specifying a debug level.\n // If @file parameter is null, function sets a default debug level.\n // Otherwise, function sets a separate debug level for the corresponding module.\n void setDebugLevel(int level, const char* file = 0);\n\n void printDebugLevels(FILE* fp = stdout);\n void printLogFiles(FILE* fp = stdout);\n void setOutStream(FILE* os);\n\n // Function prints a format message in printf-like way.\n // @level - message debug level. If @level is greater than the debug level\n // specified earlier, the message will be ignored.\n // @file - module name, from where Logger has been called.\n // @line - line number where Logger has been called.\n // @fmt - message format string\n void print(int level, const char* file, int line, const char* fmt, ...) const;\n\nprivate:\n FILE * mOutStream = stdout;\n std::map mDebugLevels = {};\n std::map mLogFiles = {};\n};\n\nextern Logger _Logger;\n\nextern int GetLocalTime (int unixTime,\n int* year, int* month, int* day,\n int* hour, int* minute, int* second);\n\nextern long long GetCurrentTimeMillis();\n\n} } // namespace navigine::navigation_core\n\n#endif" }, { "path": "include/navigine/navigation-core/motion_info.h", "content": "/** motion_info.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_MOTION_INFO_H\n#define NAVIGINE_MOTION_INFO_H\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct MotionInfo\n{\n MotionInfo()\n : isStepDetected ( false )\n , dtSec ( 0.0 )\n , deltaAngle ( 0.0 )\n , distance ( 0.0 )\n , stepCounter ( 0 )\n , heading ( 0.0 )\n , azimuth ( 0.0 )\n , lastMotionTs ( 0 )\n , isAzimuthValid (false)\n , gyroHeading ( 0.0 )\n {}\n\n bool isStepDetected;\n double dtSec;\n double deltaAngle;\n double distance;\n size_t stepCounter;\n double heading;\n double azimuth;\n long long lastMotionTs;\n bool isAzimuthValid;\n double gyroHeading;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_MOTION_INFO_H\n" }, { "path": "include/navigine/navigation-core/navigation_client.h", "content": "#pragma once\n\n#include \"navigation_input.h\"\n#include \"navigation_output.h\"\n#include \"navigation_state.h\"\n#include \"navigation_settings.h\"\n#include \"level_collector.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass NavigationClient\n{\n public:\n // Return navigation state of this client\n virtual std::vector getStates() const = 0;\n\n // Main navigation function. Calculates current position based on incoming measurements\n virtual std::vector navigate(const std::vector& navInput) = 0;\n\n virtual ~NavigationClient() {}\n};\n\nstd::shared_ptr createNavigationClient(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n} } // namespace navigine::navigation_core\n" }, { "path": "include/navigine/navigation-core/navigation_input.h", "content": "/** navigation_input.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_INPUT_H\n#define NAVIGINE_NAVIGATION_INPUT_H\n#define _CRTDBG_MAP_ALLOC\n\n#include \n#include \n#include \"boost/variant.hpp\"\n\n#include \"vector3d.h\"\n#include \"transmitter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\ntemplate \nstruct MeasurementDataWithTimeStamp\n{\n MeasurementData data;\n long long ts;\n};\n\nstruct RadioMeasurementData\n{\n enum class Type {\n WIFI,\n BEACON,\n BLUETOOTH,\n EDDYSTONE,\n WIFI_RTT\n };\n\n Type type;\n TransmitterId id;\n double rssi = 0;\n double power = 0;\n double distance = 0;\n double stddev = 0;\n};\n\nstruct SensorMeasurementData\n{\n enum class Type {\n ACCELEROMETER,\n MAGNETOMETER,\n GYROSCOPE,\n BAROMETER,\n LOCATION,\n ORIENTATION\n };\n\n Type type;\n Vector3d values;\n};\n\nstruct NmeaMeasurementData\n{\n int sentenceNumber;\n int satellitesNumber;\n};\n\nusing RadioMeasurementsData = std::vector;\n\nusing MeasurementData = boost::variant;\n\nusing Measurement = MeasurementDataWithTimeStamp;\n\n} } // namespace navigine::navigation_core\n\n#endif\n" }, { "path": "include/navigine/navigation-core/navigation_output.h", "content": "#ifndef NAVIGINE_NAVIGATION_OUTPUT_H\n#define NAVIGINE_NAVIGATION_OUTPUT_H\n\n#include \n#include \n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nenum NavigationStatus : int\n{\n OK = 0,\n NAVIGATION_ERROR = -1,\n NO_SOLUTION = 4,\n NO_RPS = 7,\n NO_LEVEL = 30,\n PF_MUTATION_FAILED = 32,\n PF_SAMPLING_FAILED = 33,\n};\n\nenum class Provider { GNSS, INDOOR, FUSED, NONE };\n\nstruct NavigationOutput\n{\n NavigationStatus status;\n\n LevelId posLevel; // Level identifier\n double posLatitude = 0.0;\n double posLongitude = 0.0;\n double posAltitude = 0.0;\n double posR; // Output radius (meters)\n double posOrientation; // Output orientation in radians (clockwise)\n Provider provider;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_NAVIGATION_OUTPUT_H\n" }, { "path": "include/navigine/navigation-core/navigation_settings.h", "content": "#ifndef NAVIGINE_NAVIGATION_SETTINGS_H\n#define NAVIGINE_NAVIGATION_SETTINGS_H\n\n#include \n#include \n#include \n#include \n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct CommonSettings {\n enum class UseAlgorithm { PF_PDR, PF_NOPDR, ZONES, GRAPH, KNN }; //PF_PDR, PF_NOPDR, ZONES, GRAPH, KNN\n enum class MeasurementType { RSSI, DISTANCE_V1, DISTANCE_V2 }; // rssi, distance_21, distance_v2\n enum class SignalsToUse { BLE, WIFI, BOTH }; //ble, wifi, both\n\n UseAlgorithm useAlgorithm = UseAlgorithm::PF_PDR; // \"Use_algorithm\"\n double lklCorrectionNoise = 10.0; // \"Correction_noise\"\n bool useEnuAzimuth = false; // \"Use_enu_azimuth\"\n bool useDifferentionMode = false; // \"Differention_mode\"\n bool useTxPower = false; // \"Use_beacon_power\"\n bool useRadiomap = false; // \"Use_radiomap\"\n bool useStops = false; // \"Use_stops\"\n bool useTracking = false; // \"Use_tracking\"\n double stopDetectionTime = 3.0; // \"Stop_detection_time\"\n double minDeviationM = 1.0; // \"Min_deviation_m\"\n double averageMovSpeed = 1.0; // \"Average_speed\"\n MeasurementType measurementType = MeasurementType::RSSI; // \"Measurement_type\"\n\n bool useBestRefPointLevel = false; // \"Use_best_ref_point_level\"\n bool useBarometer = false; // \"Use_barometer\"\n\n bool useUnknownTransmitters = false; // \"Use_unknown_beacons\"\n double sigCutOffRss = -100; // \"Cutoff_rss\"\n bool useClosestAps = false; // \"Use_closest_aps\"\n unsigned int numClosestAps = 1; // \"N_closest_aps\"\n double sigAveragingTime = 1.0; // \"Sig_averaging_time\"\n double sigWindowShift = 0.0; // \"Sig_window_shift\"\n SignalsToUse signalsToUse = SignalsToUse::BLE; // \"Use_Signals\"\n\n unsigned int numParticles = 1000; // \"N_particles\"\n unsigned int minMsrNumForMutation = 3; // \"Min_msr_num_for_mutation\"\n double resamplingThreshold = 600.0; // \"Resampling_threshold_num\", Level\n unsigned int minNumParticlesAlive = 100; // \"Min_num_of_particles\", Level\n double gyroNoiseDegree = 4.0; // \"Gyro_noise_degree\"\n double stepNoiseDeviationMeters = 0.4; // \"Step_noise_deviation_meters\"\n bool useProjectionToWhiteArea = false; // \"Use_projection_to_white_area\"\n\n unsigned int numParticlesAroundClosestAp = 10; // \"N_particles_around_closest_AP\"\n unsigned int numParticlesToMutate = 50; // \"N_particles_to_mutate\"\n unsigned int numSamplingAttempts = 60; // \"Num_sampling_attempts\"\n unsigned int numInitializationAttempts = 500; // \"Num_initialization_attempts\"\n bool useUniformSampling = false; // \"Use_uniform_sampling\"\n\n unsigned int kNeighbors = 8; // \"K_neighbors\"\n bool useTriangles = true; // \"Use_triangles\"\n bool useDiffMode = true; // \"Use_diff_mode\"\n unsigned int minNumOfMeasurementsForPositionCalculation = 4; // \"Min_num_of_measurements_for_position_calculation\"\n\n double smoothing = 0.5; // \"Smoothing\"\n double deadReckoningTime = 5.0; // \"Dead_reckon_time\"\n double stopUpdateTime = 4.0; // \"Stop_update_time\"\n double graphProjDist = 5.0; // \"Graph_prog_distance\"\n double useStopsDistanceThresholdM = 10.0; // \"Stopped_distance_threshold_m\"\n // if distance between stopped position and current position\n // is more than the threshold - update stopped position\n\n bool useSmoothing = true; // \"Use_smoothing\"\n bool useGraphProjection = false; // \"Use_graph_projection\"\n bool useAccuracyForSmoothing = false; // \"Use_accuracy_for_smoothing\"\n bool useSpeedConstraint = true; // \"Use_speed_constraint\"\n bool useBarriers = false; // \"Use_barriers\"\n bool useTimeSmoothing = false; // \"Use_time_smoothing\"\n bool useAbFilter = true; // \"Use_ab_filter\"\n\n bool useAltitude = false; // \"Use_altitude\"\n bool useGps = false; // \"Use_gps\"\n bool useInstantGpsPosition = false; // \"Use_instant_gps_position\"\n bool preferIndoorSolution = false; // \"Prefer_indoor_solution\"\n bool fuseGps = false; // \"Fuse_gps\"\n bool useGpsOutsideMap = false; // \"Use_gps_outside_map\"\n bool useGpsSigmaFilter = false; // \"Use_gps_sigma_filter\"\n unsigned int sigmaFilterWindow = 1; // \"Sigma_filter_window\"\n double sigmaFilterDelta = 0.0; // \"Sigma_filter_delta\"\n unsigned int minNumSats = 20; // \"Min_num_sats\"\n\n double priorDeviation = 50.0; // \"Prior_deviation\"\n double minGpsDeviation = 5.0; // \"Gps_deviation\"\n double maxGpsDeviation = std::numeric_limits::infinity(); // Maximum_to_accept_gps_measurement\"\n double fuseGpsBorderM = 10; // \"Fuse_gps_border\"\n long long noSignalTimeThreshold = 35; // \"No_signal_time_threshold\"\n long long noActionTimeThreshold = 20; // \"No_action_time_threshold\"\n double positionIsTooOldSec = 20.0; // \"Position_old_for_fusing_sec\"\n double gpsValidTimeWindowSec = 1.0; // \"Gps_valid_time_window_sec\"\n\n bool useCalculatedAzimuth = false; // \"Use_calculated_azimuth\"\n double deviceAzimuthLifetimeSeconds = 10.0; // \"Device_azimuth_lifetime_seconds\"\n double stepMultiplier = 1.0; // \"Step_multiplier\"\n\n bool hasAccelerometer = true; // \"Has_accelerometer\"\n bool hasGyroscope = true; // \"Has_gyroscope\"\n\n double wiFiRttOffset = -1.5; // \"WiFi_RTT_Offset\"\n};\n\nstruct LevelSettings {\n double normalModeA = -82.0; // \"GP_Normal_Mode\", \"A\", Level\n double normalModeB = 3.0; // \"GP_Normal_Mode\", \"B\", Level\n};\n\nstruct NavigationSettings\n{\n CommonSettings commonSettings;\n std::map levelsSettings;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_NAVIGATION_SETTINGS_H\n" }, { "path": "include/navigine/navigation-core/navigation_state.h", "content": "/** navigation_state.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_STATE_H\n#define NAVIGINE_NAVIGATION_STATE_H\n\n#include \n#include \"navigation_input.h\"\n#include \n#include \"xy_particle.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass NavigationState\n{\n public:\n NavigationState() { }\n \n NavigationState(const std::vector& particles,\n const std::vector& weights,\n const RadioMeasurementsData& radioMsr,\n const std::vector& likelihoodPoints,\n const std::vector& likelihoodWeights,\n double heading = 0,\n double stepLen = 0,\n int stepCounter = 0)\n : mParticles ( particles )\n , mParticleWeights ( weights )\n , mMeasurements ( radioMsr )\n , mLikelihoodMapPoints ( likelihoodPoints )\n , mLikelihoodMapWeights ( likelihoodWeights )\n , mHeading ( heading )\n , mStepLen ( stepLen )\n , mStepCounter ( stepCounter )\n { }\n \n void setStepCounter (double stepCounter) { mStepCounter = stepCounter; }\n void setStepLen (double stepLen) { mStepLen = stepLen; }\n void setIndoorPosition (const XYPoint& pos) { mIndoorPosition = pos; }\n void setOutdoorPosition (const XYPoint& pos) { mOutdoorPosition = pos; }\n\n XYPoint getIndoorPosition ( void ) const { return mIndoorPosition; }\n XYPoint getOutdoorPosition ( void ) const { return mOutdoorPosition; }\n int getStepCounter ( void ) const { return mStepCounter; }\n double getHeading ( void ) const { return mHeading; }\n double getStepLen ( void ) const { return mStepLen; }\n std::vector getParticles ( void ) const { return mParticles; }\n std::vector getParticleWeights ( void ) const { return mParticleWeights; }\n RadioMeasurementsData getLevelRadioMeasurements ( void ) const { return mMeasurements; }\n std::vector getLikelihoodMapPoints ( void ) const { return mLikelihoodMapPoints; }\n std::vector getLikelihoodMapWeights ( void ) const { return mLikelihoodMapWeights; }\n \n private:\n std::vector mParticles;\n std::vector mParticleWeights;\n RadioMeasurementsData mMeasurements;\n std::vector mLikelihoodMapPoints;\n std::vector mLikelihoodMapWeights;\n double mHeading;\n double mStepLen;\n int mStepCounter;\n XYPoint mIndoorPosition;\n XYPoint mOutdoorPosition;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_NAVIGATION_STATE_H\n" }, { "path": "include/navigine/navigation-core/point.h", "content": "/** point.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_POINT_H\n#define NAVIGINE_POINT_H\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct GeoPoint\n{\n GeoPoint()\n : latitude ( 0.0 )\n , longitude( 0.0 )\n { }\n\n GeoPoint(double _latitude, double _longitude)\n : latitude ( _latitude )\n , longitude( _longitude )\n { }\n\n double latitude;\n double longitude;\n};\n\nstruct GeoPoint3D\n{\n GeoPoint3D()\n : latitude ( 0.0 )\n , longitude( 0.0 )\n , altitude ( 0.0 )\n { }\n\n GeoPoint3D(double _latitude, double _longitude, double _altitude)\n : latitude ( _latitude )\n , longitude( _longitude )\n , altitude ( _altitude )\n { }\n\n double latitude;\n double longitude;\n double altitude;\n};\n\nstruct XYPoint\n{\n XYPoint()\n : x( 0.0 )\n , y( 0.0 )\n { }\n\n XYPoint(double _x, double _y)\n : x( _x )\n , y( _y )\n { }\n\n double x;\n double y;\n};\n\nstruct XYZPoint\n{\n XYZPoint()\n : x( 0.0 )\n , y( 0.0 )\n , z( 0.0 )\n { }\n\n XYZPoint(double _x, double _y, double _z)\n : x( _x )\n , y( _y )\n , z( _z )\n { }\n\n double x;\n double y;\n double z;\n};\n\nXYPoint gpsToLocal(const GeoPoint& point, const GeoPoint& bindPoint);\nGeoPoint localToGps(const XYPoint& localPoint, const GeoPoint& bindPoint);\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_POINT\n" }, { "path": "include/navigine/navigation-core/radiomap.h", "content": "/** radiomap.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_RADIOMAP_H\n#define NAVIGINE_RADIOMAP_H\n\n#include \n\n#include \"reference_point.h\"\n#include \"transmitter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass Radiomap\n{\npublic:\n Radiomap(const XYReferencePoints& referencePoints = {});\n\n const XYReferencePoints& referencePoints() const;\n\n ReferencePoint getReferencePoint(const ReferencePointId& refPointId) const;\n bool hasReferencePoint(const ReferencePointId& refPointId) const;\n\nprivate:\n XYReferencePoints mReferencePoints;\n std::map mReferencePointsIndex; // {id -> index}\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_RADIOMAP_H\n" }, { "path": "include/navigine/navigation-core/reference_point.h", "content": "/** reference_point.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_REFERENCE_POINT_H\n#define NAVIGINE_REFERENCE_POINT_H\n\n#include \n#include \n#include \n#include \"transmitter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct SignalStatistics\n{\n TransmitterType type;\n double mean;\n double variance;\n size_t nMeasurements;\n\n void addMeasurement(double meas);\n};\n\nDECLARE_IDENTIFIER(ReferencePointId)\n\ntemplate \nstruct ReferencePoint\n{\n ReferencePoint(\n ReferencePointId _id,\n Point _point,\n const std::map& _fingerprints)\n : id(_id)\n , point(_point)\n , fingerprints(_fingerprints)\n {}\n\n ReferencePointId id;\n Point point;\n std::map fingerprints;\n};\n\ntypedef std::vector> XYReferencePoints;\ntypedef std::vector> GeoReferencePoints;\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_REFERENCE_POINT_H\n" }, { "path": "include/navigine/navigation-core/transmitter.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \"declare_identifier.h\"\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct PathlossModel\n{\n double A;\n double B;\n double power;\n\n inline double operator()(double r) const {\n static constexpr double MIN_DISTANCE = 0.01;\n return A - B * std::log(std::max(r, MIN_DISTANCE));\n }\n};\n\nenum class TransmitterType { WIFI, BEACON, BLUETOOTH, UNKNOWN, EDDYSTONE, LOCATOR };\n\nDECLARE_IDENTIFIER(TransmitterId)\n\ntemplate \nstruct Transmitter \n{\n Transmitter() = default;\n\n Transmitter(TransmitterId _id, Point _p, PathlossModel _model, TransmitterType _type)\n : id ( _id )\n , point ( _p )\n , pathlossModel ( _model )\n , type ( _type )\n { }\n\n TransmitterId id;\n Point point;\n PathlossModel pathlossModel;\n TransmitterType type;\n\n bool operator== (const Transmitter& tx)const { return id == tx.id; }\n bool operator< (const Transmitter& tx)const { return id < tx.id; }\n};\n\ntypedef std::vector> XYZTransmitters;\ntypedef std::vector> Geo3DTransmitters;\n\n} } // namespace navigine::navigation_core\n" }, { "path": "include/navigine/navigation-core/vector3d.h", "content": "/** vector3d.h\n*\n** Copyv2 (c) 2018 Navigine. All v2s reserved.\n*\n*/\n\n#ifndef NAVIGINE_VECTOR3D_H\n#define NAVIGINE_VECTOR3D_H\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Vector3d\n{\n double x;\n double y;\n double z;\n\n Vector3d(): x(0.0), y(0.0), z(0.0) { }\n Vector3d(double _x, double _y, double _z);\n\n double magnitude() const;\n Vector3d normalized() const;\n Vector3d& normalize();\n bool isNull() const;\n\n static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2);\n static double dotProduct (const Vector3d& v1, const Vector3d& v2);\n\n bool operator==(const Vector3d& v2);\n bool operator!=(const Vector3d& v2);\n\n Vector3d& operator+=(const Vector3d& v2);\n Vector3d& operator-=(const Vector3d& v2);\n\n Vector3d& operator*=(double multiplier);\n Vector3d& operator/=(double divisor);\n};\n\ninline Vector3d operator*(double multiplier, const Vector3d& v)\n{\n return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);\n}\n\ninline Vector3d operator*(const Vector3d& v, double multiplier)\n{\n return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);\n}\n\ninline Vector3d operator+(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);\n}\n\ninline Vector3d operator-(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);\n}\n\ninline Vector3d operator-(const Vector3d& v)\n{\n return Vector3d(-v.x, -v.y, -v.z);\n}\n\ninline Vector3d operator/(const Vector3d& v1, double divisor)\n{\n return Vector3d(v1.x / divisor, v1.y / divisor, v1.z / divisor);\n}\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_VECTOR3D_H\n" }, { "path": "include/navigine/navigation-core/version.h", "content": "#ifndef NAVIGINE_VERSION_H\n#define NAVIGINE_VERSION_H\n\n#define NAVIGATION_LIBRARY_VERSION \"2.0\" // 09 July 2019\n\n#endif\n" }, { "path": "include/navigine/navigation-core/xy_particle.h", "content": "/** xy_particle.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_PARTICLE_H\n#define NAVIGINE_PARTICLE_H\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct XYParticle : XYPoint\n{\n XYParticle()\n : XYPoint ( 0.0, 0.0 )\n , angle ( 0.0 )\n { }\n\n XYParticle(double _x, double _y, double _angle)\n : XYPoint( _x, _y )\n , angle ( _angle )\n { }\n \n double angle;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_PARTICLE\n" }, { "path": "src/README", "content": "the implementations of navigation algorithms developed by Navigine" }, { "path": "src/barriers_geometry_builder.cpp", "content": "/** barriers_geometry_builder.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\n//TODO test for empty barrier list\nLevelGeometry getGeometry(const std::list& allowedArea)\n{\n Multipolygon allowedFullArea;\n for (const auto& area: allowedArea)\n {\n Polygon correctedArea = area;\n boost::geometry::correct(correctedArea);\n allowedFullArea.push_back(correctedArea);\n }\n\n return {allowedFullArea, boost::geometry::return_envelope(allowedFullArea)};\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/device_properties.h", "content": "#ifndef NAVIGINE_DEVICE_PROPERTIES_H\n#define NAVIGINE_DEVICE_PROPERTIES_H\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nclass DeviceProperties\n{\n public:\n DeviceProperties() { }\n \n DeviceProperties(const std::string& _id, const std::string& _model,\n bool _hasAcc = false,\n bool _hasGyro = false,\n bool _hasMagn = false,\n bool _hasBaro = false)\n : mId ( _id )\n , mModel ( _model )\n , mHasAccel ( _hasAcc )\n , mHasGyro ( _hasGyro )\n , mHasMagn ( _hasMagn )\n , mHasBaro ( _hasBaro )\n { }\n \n public:\n std::string getId ( void ) const { return mId; }\n std::string getModel ( void ) const { return mModel; }\n bool hasAccel ( void ) const { return mHasAccel; }\n bool hasGyro ( void ) const { return mHasGyro; }\n bool hasMagn ( void ) const { return mHasMagn; }\n bool hasBaro ( void ) const { return mHasBaro; }\n \n private:\n std::string mId = \"\"; // Client ID\n std::string mModel = \"\"; // Telephone model\n bool mHasAccel = false;\n bool mHasGyro = false;\n bool mHasMagn = false;\n bool mHasBaro = false;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_DEVICE_PROPERTIES_H\n" }, { "path": "src/geometry.cpp", "content": "#include \n#include \"geometry.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\n// Get distance from A to B\ndouble GetDist(double ax, double ay, double bx, double by)\n{\n return std::sqrt((ax - bx) * (ax - bx) + (ay - by) * (ay - by));\n}\n\n// Get distance from A to B\ndouble GetDist(const LocationPoint& A, const LocationPoint& B)\n{\n return A.level == B.level ? GetDist(A.x, A.y, B.x, B.y) : NAN;\n}\n\n// Get distance from segment to point\ndouble SegmentPointDist(const XYPoint& p1, const XYPoint& p2, const XYPoint& p)\n{\n double v1x = p.x - p1.x;\n double v1y = p.y - p1.y;\n\n double v2x = p2.x - p1.x;\n double v2y = p2.y - p1.y;\n\n double len = v2x * v2x + v2y * v2y;\n\n if (len < GEOMETRY_DOUBLE_EPSILON)\n return GetDist(p1.x, p1.y, p.x, p.y);\n\n double t = (v1x * v2x + v1y * v2y) / len;\n t = std::min(std::max(t, 0.0), 1.0);\n\n double sx = p1.x + t * (p2.x - p1.x);\n double sy = p1.y + t * (p2.y - p1.y);\n\n double dist = std::sqrt((sx - p.x) * (sx - p.x) + (sy - p.y) * (sy - p.y));\n return dist;\n}\n\n// Get distance from C to line (AB)\ndouble GetDist(const LocationPoint& A, const LocationPoint& B, const LocationPoint& C)\n{\n if (A.level != B.level || A.level != C.level)\n return NAN;\n \n double d = GetDist(A, B);\n return d >= GEOMETRY_DOUBLE_EPSILON ? std::fabs((B.x - A.x) * (C.y - A.y) - (C.x - A.x) * (B.y - A.y)) / d : NAN;\n}\n\n// Calculate the oriented area of triangle (ABC).\nextern double GetArea(const LocationPoint& A,\n const LocationPoint& B,\n const LocationPoint& C)\n{\n if (A.level != B.level || A.level != C.level)\n return NAN;\n return GetArea(A.x, A.y, B.x, B.y, C.x, C.y);\n}\n\n// Calculate the oriented area of triangle (ABC).\nextern double GetArea(double ax, double ay,\n double bx, double by,\n double cx, double cy)\n{\n return Determinant(ax - bx, ay - by, ax - cx, ay - cy) / 2;\n}\n\n// Calculate determinant of the 2x2 matrix:\n// a11 a12\n// a21 a22\ndouble Determinant(double a11, double a12,\n double a21, double a22)\n{\n return a11 * a22 - a12 * a21;\n}\n\n// Calculate determinant of the 3x3 matrix:\n// a11 a12 a13\n// a21 a22 a23\n// a31 a32 a33\ndouble Determinant(double a11, double a12, double a13,\n double a21, double a22, double a23,\n double a31, double a32, double a33)\n{\n return (a11 * a22 * a33 + a12 * a23 * a31 + a21 * a32 * a13) -\n (a21 * a12 * a33 + a11 * a23 * a32 + a13 * a22 * a31);\n}\n\n// Get projection from (ox,oy) to segment [(ax,ay), (bx,by)].\ndouble GetProjection(double ax, double ay,\n double bx, double by,\n double ox, double oy,\n double* px, double* py)\n{\n // Find such point P, that vector OP is orthogonal to the vector AB\n // Decompose vector OP for basis (OA, OB):\n // OP = (1-k) * OA + k * OB\n // Return value: k\n double ab = ((bx - ax) * (bx - ax) + (by - ay) * (by - ay));\n double ao = ((ox - ax) * (bx - ax) + (oy - ay) * (by - ay));\n double k = ab > GEOMETRY_DOUBLE_EPSILON ? ao / ab : 0.0;\n if (px) { *px = ax + k * (bx - ax); }\n if (py) { *py = ay + k * (by - ay); }\n return k;\n}\n\ndouble GetIntersection(double px, double py,\n double qx, double qy,\n double ax, double ay,\n double bx, double by,\n double* ox, double* oy)\n{\n double pqx = qx - px;\n double pqy = qy - py;\n double abx = bx - ax;\n double aby = by - ay;\n double pax = ax - px;\n double pay = ay - py;\n \n double det = Determinant(pqx, abx, pqy, aby);\n if (std::fabs(det) < GEOMETRY_DOUBLE_EPSILON)\n return NAN;\n \n double k = Determinant(pax, abx, pay, aby) / det;\n double l = -Determinant(pqx, pax, pqy, pay) / det;\n \n if (k < 0 || k > 1.0 || l < 0 || l > 1.0)\n return NAN;\n \n if (ox) { *ox = px + k * pqx; }\n if (oy) { *oy = py + k * pqy; }\n return k;\n}\n\n// Check if segments [AB] and [CD] has intersection\nbool CheckIntersection(double ax, double ay,\n double bx, double by,\n double cx, double cy,\n double dx, double dy)\n{\n auto boundBoxCheck = [](double a, double b, double c, double d) -> bool\n {\n if (a > b) std::swap(a, b);\n if (c > d) std::swap(c, d);\n return std::max(a,c) < std::min(b,d);\n };\n return boundBoxCheck(ax, bx, cx, dx) &&\n boundBoxCheck(ay, by, cy, dy) &&\n GetArea(ax, ay, bx, by, cx, cy) * GetArea(ax, ay, bx, by, dx, dy) < 0 &&\n GetArea(cx, cy, dx, dy, ax, ay) * GetArea(cx, cy, dx, dy, bx, by) < 0;\n}\n\nbool PointOnLine(double x, double y, double x1, double y1, double x2, double y2)\n{\n const double Ax = x1 - x;\n const double Ay = y1 - y;\n const double Bx = x2 - x;\n const double By = y2 - y;\n \n return (std::fabs(Ax * By - Ay * Bx) < GEOMETRY_DOUBLE_EPSILON &&\n Ax * Bx + Ay * By < GEOMETRY_DOUBLE_EPSILON);\n}\n\nbool XRayIntersectsLine(double x, double y, double x1, double y1, double x2, double y2)\n{\n static const double DELTA = std::sqrt(GEOMETRY_DOUBLE_EPSILON);\n \n if (std::fabs(y - y1) < GEOMETRY_DOUBLE_EPSILON)\n y1 += DELTA;\n \n if (std::fabs(y - y2) < GEOMETRY_DOUBLE_EPSILON)\n y2 += DELTA;\n \n if (y1 < y && y < y2)\n return Determinant(x2, y2, 1, x, y, 1, x1, y1, 1) + GEOMETRY_DOUBLE_EPSILON > 0;\n \n if (y2 < y && y < y1)\n return Determinant(x1, y1, 1, x, y, 1, x2, y2, 1) + GEOMETRY_DOUBLE_EPSILON > 0;\n \n return false;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/geometry.h", "content": "/** geometry.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_GEOMETRY_H\n#define NAVIGINE_GEOMETRY_H\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double GEOMETRY_DOUBLE_EPSILON = 1e-8;\n\n// Get distance from segment to point\nextern double SegmentPointDist(const XYPoint &p1,\n const XYPoint &p2,\n const XYPoint &p);\n\n// Get distance from A to B\nextern double GetDist(const LocationPoint& A, const LocationPoint& B);\nextern double GetDist(double ax, double ay, double bx, double by);\n\n// Get distance from C to line (AB)\nextern double GetDist(const LocationPoint& A,\n const LocationPoint& B,\n const LocationPoint& C);\n\n// Calculate the oriented area of triangle (ABC).\nextern double GetArea(const LocationPoint& A,\n const LocationPoint& B,\n const LocationPoint& C);\n\n// Calculate the oriented area of triangle (ABC).\nextern double GetArea(double ax, double ay,\n double bx, double by,\n double cx, double cy);\n\n// Calculate determinant of the 2x2 matrix:\n// a11 a12\n// a21 a22\nextern double Determinant(double a11, double a12,\n double a21, double a22);\n\n// Calculate determinant of the 3x3 matrix:\n// a11 a12 a13\n// a21 a22 a23\n// a31 a32 a33\nextern double Determinant(double a11, double a12, double a13,\n double a21, double a22, double a23,\n double a31, double a32, double a33);\n\n// Get distance from O to segment [AB].\n// Px - is the closest point on [AB].\nextern double GetProjection(double ax, double ay,\n double bx, double by,\n double ox, double oy,\n double* px, double* py);\n\n// Get intersection of vector PQ and segment [AB].\nextern double GetIntersection(double px, double py,\n double qx, double qy,\n double ax, double ay,\n double bx, double by,\n double* x, double* y);\n\n// Check if segments [AB] and [CD] has intersection\nextern bool CheckIntersection(double ax, double ay,\n double bx, double by,\n double cx, double cy,\n double dx, double dy);\n\nextern bool PointOnLine ( double x, double y, double x1, double y1, double x2, double y2 );\nextern bool XRayIntersectsLine ( double x, double y, double x1, double y1, double x2, double y2 );\n\n} } // namespace navigine::navigation_core\n\n#endif\n" }, { "path": "src/knn/knn_utils.cpp", "content": "/** knn_utils.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include \n#include \n#include \n\n#include \"knn_utils.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace {\nstatic constexpr double MIN_WEIGHT_EPSILON = 1e-8;\nstatic constexpr bool USE_AP_WEIGHTS = true;\nstatic constexpr bool USE_TRIANGULATION = true;\nstatic constexpr int MIN_NUM_VISIBLE_BS_IN_RP = 3; // should be more than 0!!!\nstatic constexpr double RSSI_ON_METER = -10.0;\nstatic constexpr double MIN_RP_WEIGHT = 1e-6; // we skip RPs with 0 weights because they are most improbable,\n // but if all signals coincide to map, we should add small additive\nstatic constexpr double GAMMA = 3.0;\nstatic constexpr double G_WEIGHT_COEF = 1.8; // Coef to find weight of AP (See Accuracy of RSS-Based Centroid Localization\n // Algorithms in an Indoor Environment\n} //namespace\n\nstd::map> triangulate(const std::shared_ptr& levelCollector)\n{\n std::map> levelTriangles;\n#ifdef DEBUG_OUTPUT_TRIANGULATION\n std::ofstream debugOs;\n std::string fileName = DEBUG_OUTPUT_TRIANGULATION;\n debugOs.open(fileName);\n debugOs << \"level ax ay bx by cx cy \" << std::endl;\n debugOs.close();\n#endif\n\n for (const Level& level : levelCollector->levels())\n {\n const XYReferencePoints& referencePoints = level.radiomap().referencePoints();\n if (referencePoints.size() > 3)\n {\n std::vector vertices;\n for(const ReferencePoint& rp: referencePoints)\n {\n TriangleVertex v = TriangleVertex(rp.point.x, rp.point.y, rp.id);\n vertices.push_back(v);\n }\n\n auto widthHeightPair = getWidthHeightPair(level.geometry().boundingBox());\n double maxedge = std::max(widthHeightPair.first, widthHeightPair.second);\n std::vector triangles = TriangulateVertices(vertices, maxedge);\n if (triangles.empty())\n {\n // triangulation failed!\n return levelTriangles;\n }\n#ifdef DEBUG_OUTPUT_TRIANGULATION\n std::ofstream debugOs;\n std::string fileName = DEBUG_OUTPUT_TRIANGULATION;\n debugOs.open(fileName, std::ios_base::app);\n#endif\n std::vector refPointsTriangles;\n for (auto it = triangles.begin(); it != triangles.end(); it++)\n {\n#ifdef DEBUG_OUTPUT_TRIANGULATION\n Triangle& t = *it;\n debugOs << level.id << \" \"\n << t.getA().x << \" \" << t.getA().y << \" \"\n << t.getB().x << \" \" << t.getB().y << \" \"\n << t.getC().x << \" \" << t.getC().y << std::endl;\n#endif\n RefPointsTriangle rpTriangle = {it->getA().id, it->getB().id, it->getC().id};\n refPointsTriangles.push_back(rpTriangle);\n }\n#ifdef DEBUG_OUTPUT_TRIANGULATION\n debugOs.close();\n#endif\n\n levelTriangles.insert(std::make_pair(level.id(), refPointsTriangles));\n }\n }\n return levelTriangles;\n}\n\nstd::map>>\n getLevelsRadiomaps(const std::shared_ptr &levelCollector)\n{\n std::map>>\n levelsRadiomaps;\n\n for (const Level& level : levelCollector->levels())\n {\n std::multimap> bssidRpToSigstatMap;\n\n const auto& levelId = level.id();\n for (const ReferencePoint& rp : level.radiomap().referencePoints())\n {\n const std::map& rpFingerprint = rp.fingerprints;\n for (const auto& bssidSigStatPair: rpFingerprint)\n {\n bssidRpToSigstatMap.insert({bssidSigStatPair.first,\n std::make_pair(rp.id, bssidSigStatPair.second)});\n }\n }\n levelsRadiomaps[levelId] = bssidRpToSigstatMap;\n }\n return levelsRadiomaps;\n}\n\nstd::map> getLevelReferenceTransmittersMap(const std::shared_ptr &levelCollector)\n{\n std::map> levelTransmitters;\n for (const auto& level : levelCollector->levels())\n {\n const LevelId levelId = level.id();\n for (const ReferencePoint& rp : level.radiomap().referencePoints())\n {\n const std::map& rpFingerprint = rp.fingerprints;\n std::set rpTransmittersIds;\n std::transform(rpFingerprint.begin(), rpFingerprint.end(),\n std::inserter(rpTransmittersIds, rpTransmittersIds.end()),\n [](const auto& p){ return p.first; });\n if (rpTransmittersIds.empty())\n continue;\n if (levelTransmitters.find(levelId) == levelTransmitters.end())\n levelTransmitters.insert({levelId, rpTransmittersIds});\n else\n levelTransmitters[levelId].insert(rpTransmittersIds.begin(), rpTransmittersIds.end());\n }\n }\n return levelTransmitters;\n}\n\n/**\n * @brief calculate weight of the RadioSignal\n *\n * The function weight the RadioSignal from AP based on this RSSI value.\n * The stronger the signal the bigger weight it will get.\n * The RSSI at some distance d is: RSSI = A + B * gamma * lg (d / d0)\n * RSSI1 - RSSI2 = B * gamma * lg (d1 / d2) and d1 can be found when d2 = 1 meter.\n * The rssi at 1 meter distance from the transmitter assumed to be defined in parameter.\n * Gamma is the pass loss exponent, or attenuation coefficient.\n * For G_WEIGHT_COEF see Accuracy of RSS-Based Centroid Localization Algorithms in an Indoor Environment\n *\n * @param msr RadioMeasurement either from the access point\n *\n * @return @warning unnormalized weight of the measurement.\n */\n\nstatic double calculateTransmitterTrustworthiness(const RadioMeasurementData& msr)\n{\n double B = 10 * GAMMA;\n double txPower = RSSI_ON_METER;\n double gCoef = G_WEIGHT_COEF;\n\n txPower = msr.power < 0.00001 ? RSSI_ON_METER : msr.power;\n double distance = pow(10, (txPower - msr.rssi) / B); //double apWeight = exp((aps[i].rssi - txPower) / B);\n double apWeight = 1 / pow(distance, gCoef);\n\n return apWeight;\n}\n\nstatic std::map calcTransmittersWeights(\n const RadioMeasurementsData& msr)\n{\n // Calculate AP weights in the basis of signal strength\n // We should also take into account number of incoming measurements during weights calculation!!!!!!!!!!!!\n // We call this function after averageDuplicateMeasurements\n std::map trWeightMap;\n double trWeightSum = 0.0;\n for (const auto& m : msr)\n {\n const TransmitterId& trId = m.id;\n double trWeight = USE_AP_WEIGHTS ? calculateTransmitterTrustworthiness(m) : 1.0;\n trWeightMap[trId] = trWeight;\n trWeightSum += trWeight;\n }\n \n for (auto& tp : trWeightMap)\n {\n tp.second /= trWeightSum;\n }\n\n return trWeightMap;\n}\n\n/**\n * This function calculates weights of RPs on the basis of RSS measurements and\n * radio map of the building.\n * The radiomap is multimap of transmitter as keys\n * and pairs of reference points ids and corresponding signal statistics as values\n*/\nstd::map calcRpWeightsKnn(\n const std::multimap>& radiomap,\n const RadioMeasurementsData& msr,\n bool useDiffMode)\n{\n struct SigStat { TransmitterId txId; double rssiMeasured; double rssiDatabase;};\n\n std::map> rpsStatistics;\n for (const auto& curMsr : msr)\n {\n auto range = radiomap.equal_range(curMsr.id);\n\n for (auto it = range.first; it != range.second; it++)\n {\n const std::pair& refPointsSsPair = it->second;\n const ReferencePointId& rpId = refPointsSsPair.first;\n if (rpsStatistics.find(rpId) == rpsStatistics.end())\n rpsStatistics[rpId] = std::vector();\n SigStat sigStat = {curMsr.id, curMsr.rssi, refPointsSsPair.second.mean};\n rpsStatistics[rpId].push_back(sigStat);\n }\n }\n\n std::map rpWeights;\n std::map rpNormalize;\n std::map trWeights = calcTransmittersWeights(msr);\n\n for (const auto& pair : rpsStatistics)\n {\n const auto& rpId = pair.first;\n const auto& vecSigStat = pair.second;\n if (vecSigStat.size() < MIN_NUM_VISIBLE_BS_IN_RP)\n continue;\n\n for (auto it1 = vecSigStat.begin(); it1 != vecSigStat.end(); it1 ++)\n {\n if (useDiffMode)\n {\n for (auto it2 = vecSigStat.begin(); it2 != it1; it2++)\n {\n double measDiff = it1->rssiMeasured - it2->rssiMeasured;\n double mapDiff = it1->rssiDatabase - it2->rssiDatabase;\n \n // We sum with rp Penalties, to make sum of apWeight = 1 after normalization)\n double apW = trWeights[it1->txId] * trWeights[it2->txId];\n rpNormalize[rpId] += apW;\n\n double weight = std::max(fabs(mapDiff - measDiff) * apW, MIN_RP_WEIGHT);\n rpWeights[rpId] += weight;\n }\n }\n else\n {\n double apW = trWeights[it1->txId];\n rpNormalize[rpId] += apW;\n double weight = std::max(fabs(it1->rssiDatabase + it1->rssiMeasured) * apW, MIN_RP_WEIGHT);\n rpWeights[rpId] += weight;\n }\n }\n }\n\n for (auto& pair : rpWeights)\n {\n const ReferencePointId& rpId = pair.first;\n double& rpWeight = pair.second;\n \n if (rpWeight > MIN_WEIGHT_EPSILON)\n {\n // normalize in order to make sum of transmitter weights == 1,\n // because we summed transmitter weights on all APs, not only visible\n if (USE_AP_WEIGHTS)\n {\n auto rpDivider = rpNormalize[rpId];\n if (rpDivider > MIN_WEIGHT_EPSILON)\n rpWeight /= rpDivider;\n }\n\n // do sqrt because this norm is better (it was shown empirically) \n // this norm also takes into account\n // the number of visible APS, due to its asymptotic is 1 / sqrt(N)\n rpWeight = std::sqrt(rpWeight);\n\n // Divide by number of visible APs because it differs from point to point\n // rely more on RP where there are more visible tx from measurements vector\n // for KNN the more penalty the more probability\n auto visibleApNum = rpsStatistics[rpId].size();\n if (visibleApNum > 0)\n rpWeight /= visibleApNum;\n\n // Revert weights in KNN (to transform weights to probabilities)\n rpWeight = 1.0 / rpWeight;\n }\n }\n\n return rpWeights;\n}\n\nstatic double getTriangleWeight(const RefPointsTriangle& triangle,\n const std::map& rpWeights)\n{\n double w1, w2, w3;\n std::tie(w1, w2, w3) = getValueForEachKey(rpWeights, 0.0, \n triangle.rp1, \n triangle.rp2, \n triangle.rp3);\n return w1 + w2 + w3;\n}\n\nXYPoint calcPositionInTriangle(const std::vector& triangles,\n const Radiomap& radiomap,\n const std::map& rpWeights)\n{\n auto maxWeightTriangle = std::max_element(triangles.begin(), triangles.end(),\n [&rpWeights] \n (const RefPointsTriangle& lhs, const RefPointsTriangle& rhs) \n {\n return getTriangleWeight(lhs, rpWeights) < getTriangleWeight(rhs, rpWeights);\n });\n\n double w1, w2, w3;\n std::tie(w1, w2, w3) = getValueForEachKey(rpWeights, 0.0, \n maxWeightTriangle->rp1, \n maxWeightTriangle->rp2, \n maxWeightTriangle->rp3);\n double norm = w1 + w2 + w3;\n\n if (norm < std::numeric_limits::epsilon())\n {\n //TODO send error code\n return XYPoint(0.0, 0.0);\n }\n\n if (!radiomap.hasReferencePoint(maxWeightTriangle->rp1) ||\n !radiomap.hasReferencePoint(maxWeightTriangle->rp2) ||\n !radiomap.hasReferencePoint(maxWeightTriangle->rp3))\n {\n //TODO send error code\n return XYPoint(0.0, 0.0);\n }\n\n ReferencePoint rp1 = radiomap.getReferencePoint(maxWeightTriangle->rp1);\n ReferencePoint rp2 = radiomap.getReferencePoint(maxWeightTriangle->rp2);\n ReferencePoint rp3 = radiomap.getReferencePoint(maxWeightTriangle->rp3);\n\n double x = (w1 * rp1.point.x + w2 * rp2.point.x + w3 * rp3.point.x) / norm;\n double y = (w1 * rp1.point.y + w2 * rp2.point.y + w3 * rp3.point.y) / norm;\n\n return XYPoint(x, y);\n}\n\nXYPoint calcKHeaviestRefPointsAverage(const Radiomap& radiomap,\n const std::map& rpToWeight,\n size_t k)\n{\n k = std::min(k, rpToWeight.size());\n std::vector> kHeaviestPoints;\n kHeaviestPoints.reserve(k);\n std::partial_sort_copy(\n rpToWeight.begin(), rpToWeight.end(),\n kHeaviestPoints.begin(), kHeaviestPoints.end(),\n [](std::pair const& l,\n std::pair const& r)\n {\n return l.second > r.second;\n });\n\n double x = 0.0;\n double y = 0.0;\n double norm = 0.0;\n for (auto it = kHeaviestPoints.cbegin(); it != kHeaviestPoints.cend(); it++)\n {\n // valid, due to heaviest point build structure\n ReferencePoint rp = radiomap.getReferencePoint(it->first);\n double w = it->second;\n x += w * rp.point.x;\n y += w * rp.point.y;\n norm += w;\n }\n\n if (norm < std::numeric_limits::epsilon())\n {\n //TODO check if norm can be zero and process error\n return XYPoint(0.0, 0.0);\n }\n\n x /= norm;\n y /= norm;\n\n return XYPoint(x, y);\n}\n\nRadioMeasurementsData getWhitelistedMeasurements(\n const std::set& wl,\n const RadioMeasurementsData msrs)\n{\n RadioMeasurementsData filtered;\n\n std::copy_if(msrs.begin(), msrs.end(), std::back_inserter(filtered),\n [&wl](const RadioMeasurementData& msr) {return wl.find(msr.id) != wl.end(); });\n\n return filtered;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/knn/knn_utils.h", "content": "/** knn_utils.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef KNN_UTILS_H\n#define KNN_UTILS_H\n\n#include \n#include \n#include \n#include \n\n#include \n\n#include \"../triangulation.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct RefPointsTriangle\n{\n ReferencePointId rp1;\n ReferencePointId rp2;\n ReferencePointId rp3;\n};\n\nstruct TriangleVerticesWeights\n{\n double w1;\n double w2;\n double w3;\n};\n\nstd::map> triangulate(const std::shared_ptr& levelCollector);\n\nstd::map>>\n getLevelsRadiomaps(const std::shared_ptr& levelCollector);\n\nstd::map> getLevelReferenceTransmittersMap(\n const std::shared_ptr& levelCollector);\n\nstd::map calcRpWeightsKnn(\n const std::multimap >& radiomap,\n const RadioMeasurementsData& msr,\n bool useDiffMode = true);\n\nXYPoint calcPositionInTriangle(const std::vector& triangles,\n const Radiomap& rps,\n const std::map& rpToWeight);\n\nXYPoint calcKHeaviestRefPointsAverage(const Radiomap& radiomap,\n const std::map& rpToWeight,\n size_t k);\n\nRadioMeasurementsData getWhitelistedMeasurements(\n const std::set& wl,\n const RadioMeasurementsData msrs);\n\ntemplate \n ValType func_helper(const std::map& map, KeyType key, ValType defVal) {\n if (map.find(key) == map.end())\n return defVal;\n else\n return map.find(key)->second; \n}\n\ntemplate\nauto getValueForEachKey(const std::map& map, ValType defVal,\n Keys... keys) -> decltype(std::make_tuple(func_helper(map, keys, defVal)...)) {\n return std::make_tuple(func_helper(map, keys, defVal)...);\n}\n\n} } // namespace navigine::navigation_core\n\n#endif // KNN_UTILS_H\n" }, { "path": "src/level.cpp", "content": "/** level.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nconst GeoPoint& Level::bindingPoint() const\n{\n return mBindingPoint;\n}\n\nconst LevelId& Level::id() const\n{\n return mId;\n}\n\nconst Radiomap &Level::radiomap() const\n{\n return mRadiomap;\n}\n\nconst Graph& Level::graph() const\n{\n return mGraph;\n}\n\nconst LevelGeometry& Level::geometry() const\n{\n return mGeometry;\n}\n\nbool Level::containsTransmitter(const TransmitterId& txId) const\n{\n return mTransmitters.find(txId) != mTransmitters.end();\n}\n\nconst Transmitter& Level::transmitter(const TransmitterId& txId) const\n{\n return mTransmitters.at(txId);\n}\n\nvoid Level::addTransmitters(const XYZTransmitters& transmitters)\n{\n for (const Transmitter& tx: transmitters)\n mTransmitters.insert({tx.id, tx});\n}\n\n//TODO after second call should reference points be added to existed or replaced?\nvoid Level::setReferencePoints(const XYReferencePoints& referencePoints)\n{\n mRadiomap = Radiomap(referencePoints);\n}\n\nvoid Level::setGraph(const Graph& graph)\n{\n mGraph = graph;\n}\n\nvoid Level::setGeometry(const LevelGeometry& levelGeometry)\n{\n mGeometry = levelGeometry;\n}\n\nRadioMeasurementsData getLevelRadioMeasurements(\n const Level& level,\n const RadioMeasurementsData& radioMsr)\n{\n RadioMeasurementsData levelRadioMeasurements;\n for (const RadioMeasurementData& msr: radioMsr)\n {\n if (level.containsTransmitter(msr.id))\n levelRadioMeasurements.push_back(msr);\n }\n return levelRadioMeasurements;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/level_collector.cpp", "content": "/** level_collector.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace {\n\nXYZTransmitters getTransmittersInLocalCoordinates(\n const GeoPoint& binding,\n const std::vector>& transmitters,\n const double& altitude)\n{\n XYZTransmitters localTransmitters;\n for (const Transmitter& t: transmitters)\n {\n XYPoint point2D = gpsToLocal(GeoPoint(t.point.latitude, t.point.longitude), binding);\n localTransmitters.emplace_back(\n TransmitterId(t.id.value),\n XYZPoint(point2D.x, point2D.y, altitude + t.point.altitude),\n t.pathlossModel,\n t.type);\n }\n return localTransmitters;\n}\n\nXYReferencePoints getRefPointsInLocalCoordinates(\n const GeoPoint& binding,\n const std::vector>& referencePoints)\n{\n XYReferencePoints localReferencePoints;\n for (const ReferencePoint& rp: referencePoints)\n {\n localReferencePoints.emplace_back(\n ReferencePointId(rp.id.value),\n gpsToLocal(rp.point, binding),\n rp.fingerprints);\n }\n return localReferencePoints;\n}\n\nPolygon getPolygonInLocalCoordinates(\n const GeoPoint& binding,\n const Polygon& geoPolygon)\n{\n Polygon result;\n for (const auto& p : geoPolygon.outer())\n {\n XYPoint pxy = gpsToLocal(GeoPoint(p.x(), p.y()), binding);\n boost::geometry::append(result.outer(), Point(pxy.x, pxy.y));\n }\n for (const auto& innerRing : geoPolygon.inners())\n {\n result.inners().push_back({});\n for (const auto& p : innerRing)\n {\n XYPoint pxy = gpsToLocal(GeoPoint(p.x(), p.y()), binding);\n boost::geometry::append(result.inners().back(), Point(pxy.x, pxy.y));\n }\n }\n boost::geometry::correct(result);\n return result;\n}\n\nBox getBBoxInLocalCoordinates(const GeoPoint& binding, const Box& geoBox)\n{\n XYPoint pmin = gpsToLocal(GeoPoint(geoBox.min_corner().x(), geoBox.min_corner().y()), binding);\n XYPoint pmax = gpsToLocal(GeoPoint(geoBox.max_corner().x(), geoBox.max_corner().y()), binding);\n return Box(Point(pmin.x, pmin.y), Point(pmax.x, pmax.y));\n}\n\nLevelGeometry getGeometryInLocalCoordinates(\n const GeoPoint& binding,\n const LevelGeometry& geolevelGeometry)\n{\n Multipolygon localArea;\n for (const auto& poly : geolevelGeometry.allowedArea())\n {\n localArea.push_back(getPolygonInLocalCoordinates(binding, poly));\n }\n boost::geometry::correct(localArea);\n\n Box localBox = getBBoxInLocalCoordinates(binding, geolevelGeometry.boundingBox());\n boost::geometry::correct(localBox);\n\n return LevelGeometry{localArea, localBox};\n}\n\nGraph getGraphInLocalCoordinates(\n const GeoPoint& binding,\n const Graph& geoGraph)\n{\n Graph xyGraph;\n std::map geoToXyVertexIds;\n for (Graph::VertexIterator it = geoGraph.vertexBegin(); it != geoGraph.vertexEnd(); it++)\n {\n Graph::Vertex geoVertex = *it;\n XYPoint p = gpsToLocal(geoVertex.point, binding);\n Graph::Vertex xyVertex = xyGraph.addVertex(p);\n geoToXyVertexIds[geoVertex.id] = xyVertex.id;\n }\n\n for (Graph::EdgeIterator it = geoGraph.edgesBegin(); it != geoGraph.edgesEnd(); it++)\n {\n Graph::Vertex geoV1 = geoGraph.getVertex(it->first);\n Graph::Vertex geoV2 = geoGraph.getVertex(it->second);\n int xyV1 = geoToXyVertexIds[geoV1.id];\n int xyV2 = geoToXyVertexIds[geoV2.id];\n xyGraph.addEdge(xyV1, xyV2);\n }\n return xyGraph;\n}\n} //namespace\n\n\nstd::shared_ptr createLevelCollector()\n{\n return std::make_shared();\n}\n\nboost::optional LevelCollector::findLevelByTransmitterId(const TransmitterId &transmitterId) const\n{\n for (const Level& level: mLevels)\n {\n if (level.containsTransmitter(transmitterId))\n {\n return level.id();\n }\n }\n\n return boost::none;\n}\n\nconst std::vector& LevelCollector::levels() const\n{\n return mLevels;\n}\n\nbool LevelCollector::hasLevel(const LevelId& levelId) const\n{\n return mLevelsIndices.find(levelId) != mLevelsIndices.end();\n}\n\nconst Level& LevelCollector::level(const LevelId& levelId) const\n{\n return mLevels.at(mLevelsIndices.at(levelId));\n}\n\nGeoPoint getBinding(const GeoLevel& geoLevel)\n{\n auto geoBox = geoLevel.geometry.boundingBox();\n boost::geometry::correct(geoBox);\n double minLatitude = geoBox.min_corner().x();\n double minLongitude = geoBox.min_corner().y();\n return GeoPoint(minLatitude, minLongitude);\n}\n\nvoid LevelCollector::addGeoLevel(const GeoLevel& geoLevel)\n{\n LevelId levelId = LevelId(geoLevel.id);\n GeoPoint binding = getBinding(geoLevel);\n Level level(levelId, binding);\n\n level.addTransmitters(getTransmittersInLocalCoordinates(binding, geoLevel.transmitters, geoLevel.altitude));\n level.setGeometry(getGeometryInLocalCoordinates(binding, geoLevel.geometry));\n\n level.setReferencePoints(getRefPointsInLocalCoordinates(binding, geoLevel.referencePoints));\n level.setGraph(getGraphInLocalCoordinates(binding, geoLevel.graph));\n\n mLevels.push_back(std::move(level));\n mLevelsIndices.insert({levelId, mLevels.size() - 1});\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/level_estimator/barometer.cpp", "content": "#include \n\n#include \n#include \n#include \n\n#include \"barometer.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\n#ifdef DEBUG_OUTPUT_BAROMETER\n#include \n#include \n#endif\n\nstatic const double BAROMETER_R = 8.31; // [J / (mol * K)]\nstatic const double BAROMETER_G = 9.8; // [m / s^2]\nstatic const double BAROMETER_T = 288.0; // [K]\nstatic const double BAROMETER_M = 0.0289; // [kg / mol]\nstatic const double BAROMETER_PASCAL_TO_METERS = -BAROMETER_R * BAROMETER_T / (BAROMETER_G * BAROMETER_M);\nstatic const double BAROMETER_MBAR_TO_PASCAL = 100.0;\nstatic const double BAROMETER_MIN_FLOOR_HEIGHT_M = 2.0; // [m]\n\nstatic const size_t SIZE_OF_CHUNK = 100;\n\nstatic double calcMean(const std::deque& measurements)\n{\n if (measurements.empty())\n {\n //TODO process error\n return 0;\n }\n\n return std::accumulate(measurements.begin(), measurements.end(), 0.0) / measurements.size();\n}\n\n//TODO test if there is no barometer in the phone\n//TODO try to use ring buffer with fixed size\n//TODO take into account vertical speed in Pa/sec\n// - the floor is changed only if the vertical displacement is more than distance between floors (i.e. 3m)\nLevelId Barometer::checkLevelChange(\n const LevelId& levelId,\n const SensorMeasurement& sensorMsr)\n{\n if (!mIsInitialized)\n {\n mIsInitialized = true;\n return levelId;\n }\n\n updateBuffers(sensorMsr);\n updateLevel(levelId);\n return mLastKnownLevel;\n}\n\nvoid Barometer::updateBuffers(const SensorMeasurement& sensorMsr)\n{\n#ifdef DEBUG_OUTPUT_BAROMETER\n std::ofstream debugOutputFile;\n debugOutputFile.open(DEBUG_OUTPUT_BAROMETER, std::ios_base::app);\n if (sensorMsr.data.type == SENSOR_ENTRY_BAROMETER)\n debugOutputFile << m.ts << \" \" << m.values.x << std::endl;\n debugOutputFile.close();\n#endif\n\n if (sensorMsr.data.type == SensorMeasurementData::Type::BAROMETER)\n {\n if (mStoredMeasurements.size() > SIZE_OF_CHUNK)\n mStoredMeasurements.pop_front();\n\n if (mNewMeasurements.size() > SIZE_OF_CHUNK)\n {\n mStoredMeasurements.push_back(mNewMeasurements.front());\n mNewMeasurements.pop_front();\n }\n\n mNewMeasurements.push_back(sensorMsr.data.values.x);\n }\n}\n\n//TODO take into account pressure change speed - i.e. a person can move slowly upstairs\nvoid Barometer::updateLevel(LevelId levelId)\n{\n if ((mNewMeasurements.size() > SIZE_OF_CHUNK)\n && (mStoredMeasurements.size() >= SIZE_OF_CHUNK))\n {\n if (mLastKnownLevel != levelId)\n {\n double curMean = calcMean(mNewMeasurements);\n double lastMean = calcMean(mStoredMeasurements);\n\n if (curMean <= std::numeric_limits::epsilon()\n || lastMean <= std::numeric_limits::epsilon())\n {\n //TODO: process error: incorrect average pressure!\n return;\n }\n\n double p2Pa = BAROMETER_MBAR_TO_PASCAL * curMean;\n double p1Pa = BAROMETER_MBAR_TO_PASCAL * lastMean;\n\n double deltaHeightM = BAROMETER_PASCAL_TO_METERS * std::log(p2Pa / p1Pa);\n\n bool levelHasChanged = std::abs(deltaHeightM) > BAROMETER_MIN_FLOOR_HEIGHT_M;\n\n if (levelHasChanged)\n mLastKnownLevel = levelId;\n }\n }\n else\n mLastKnownLevel = levelId;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/level_estimator/barometer.h", "content": "/** barometer.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef BAROMETER_H\n#define BAROMETER_H\n\n#include \n\n#include \n#include \n\n#include \"../measurement_types.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass Barometer\n{\n public:\n Barometer() : mIsInitialized ( false ), mLastKnownLevel ( LevelId(\"\")) {}\n LevelId checkLevelChange(\n const LevelId &levelId,\n const SensorMeasurement& sensorMsr);\n\n private:\n void updateBuffers(const SensorMeasurement& sensorMsr);\n void updateLevel(LevelId levelId);\n\n std::deque mNewMeasurements = {};\n std::deque mStoredMeasurements = {};\n\n bool mIsInitialized;\n LevelId mLastKnownLevel;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // BAROMETER_H\n" }, { "path": "src/level_estimator/level_estimator.cpp", "content": "/** level_estimator.cpp\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#include \n#include \"level_estimator.h\"\n\n\nnamespace navigine {\nnamespace navigation_core {\n\nLevelEstimator::LevelEstimator(const NavigationSettings& navProps)\n : mUseBarometer(navProps.commonSettings.useBarometer)\n{\n}\n\n/**\n * function detects id of current level according to highest average RSSI for\n * placed transmitters. In a case of any error returns -1\n */\nLevelId LevelEstimator::calculateLevel(\n const RadioMeasurementsData& radioMsr,\n const SensorMeasurement& sensorMsr)\n{\n LevelId detectedLevelId = detectCurrentLevel(radioMsr);\n LevelId levelIdWithHistory = mHistory.detectLevelUsingHistory(detectedLevelId);\n LevelId levelId = mUseBarometer ? mBarometer.checkLevelChange(levelIdWithHistory, sensorMsr)\n : levelIdWithHistory;\n\n return levelId;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/level_estimator/level_estimator.h", "content": "/** level_estimator.h\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#ifndef LEVEL_ESTIMATOR_H\n#define LEVEL_ESTIMATOR_H\n\n#include \n#include \n#include \n\n#include \"level_history.h\"\n#include \"barometer.h\"\n\n#include \"../measurement_types.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass LevelEstimator\n{\n public:\n LevelEstimator(const NavigationSettings& navProps);\n virtual ~LevelEstimator() {}\n LevelId calculateLevel(\n const RadioMeasurementsData& radioMsr,\n const SensorMeasurement& sensorMsr);\n\n protected:\n virtual LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) = 0;\n\n private:\n LevelHistory mHistory;\n Barometer mBarometer;\n const bool mUseBarometer;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // LEVEL_ESTIMATOR_H\n" }, { "path": "src/level_estimator/level_estimator_radiomap.cpp", "content": "/** level_estimator_radiomap.cpp\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#include \n#include \n\n#include \"level_estimator_radiomap.h\"\n#include \"../knn/knn_utils.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nLevelEstimatorRadiomap::LevelEstimatorRadiomap(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n : LevelEstimator(navProps)\n , mLevelIndex(levelCollector)\n , mUseBestRp(navProps.commonSettings.useBestRefPointLevel)\n{\n mLevelsRadiomaps = getLevelsRadiomaps(levelCollector);\n}\n\nLevelId LevelEstimatorRadiomap::detectCurrentLevel(const RadioMeasurementsData& radioMsr)\n{\n LevelId bestLevelId = LevelId(\"\");\n double bestLevelWeight = 0.0;\n\n for (const Level& level : mLevelIndex->levels())\n { \n const std::multimap& radiomap = mLevelsRadiomaps.at(level.id());\n\n double levelWeight = mUseBestRp ? calcWeightBestRp(radioMsr, radiomap)\n : calcWeight(radioMsr, radiomap);\n if (levelWeight > bestLevelWeight)\n {\n bestLevelWeight = levelWeight;\n bestLevelId = level.id();\n }\n }\n\n return bestLevelId;\n}\n\n/** level weight is calculated based of difference between measured and reference signals */\ndouble LevelEstimatorRadiomap::calcWeight(\n const RadioMeasurementsData& radioMsr,\n const std::multimap &fingerprints)\n{\n int nUniqueTransmitters = 0;\n double levelWeight = 0.0;\n\n for (const RadioMeasurementData& meas : radioMsr)\n {\n //TODO try to use lower weight for wifi signal since it is not reliable for level identification\n double signalTypeWeight = 1.0;//(meas.type == SIGNAL_ENTRY_WIFI) ? 0.01 : 1.0;\n double measWeight = 0.0;\n int numOfOccurrences = 0;\n auto range = fingerprints.equal_range(meas.id);\n\n for (auto it = range.first; it != range.second; ++it)\n {\n const fingerprint& refPointsSsPair = it->second;\n const SignalStatistics& sigStat = refPointsSsPair.second;\n // It is possible that positive rssi values are used in measure.xml\n // and negative rssi values are used in measurements - so both abs are necessary here:\n double difference = std::fabs(std::fabs(meas.rssi) - std::fabs(sigStat.mean));\n measWeight += signalTypeWeight * 1.0 / (difference + 0.1);\n numOfOccurrences ++;\n }\n\n if (numOfOccurrences > 0)\n levelWeight += measWeight / numOfOccurrences;\n\n if (std::distance(range.first, range.second) > 0)\n nUniqueTransmitters++;\n }\n\n levelWeight *= static_cast(nUniqueTransmitters);\n return levelWeight;\n}\n\n/** level weight is given equal to weight of best reference point */\ndouble LevelEstimatorRadiomap::calcWeightBestRp(\n const RadioMeasurementsData& radioMsr,\n const std::multimap& radiomap)\n{\n std::map rpToWeight = calcRpWeightsKnn(radiomap, radioMsr, true);\n\n double bestWeight = 0.0;\n if (!rpToWeight.empty())\n {\n ReferencePointId bestRp = rpToWeight.begin()->first;\n for (auto const& rpWeightPair : rpToWeight)\n {\n if (rpWeightPair.second > bestWeight)\n {\n bestRp = rpWeightPair.first;\n bestWeight = rpWeightPair.second;\n }\n }\n }\n\n return bestWeight;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/level_estimator/level_estimator_radiomap.h", "content": "/** level_estimator_radiomap.h\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#ifndef LEVEL_ESTIMATOR_RADIOMAP_H\n#define LEVEL_ESTIMATOR_RADIOMAP_H\n\n#include \"level_estimator.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass LevelEstimatorRadiomap : public LevelEstimator\n{\npublic:\n LevelEstimatorRadiomap(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\nprotected:\n LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) override;\n\nprivate:\n using fingerprint = std::pair;\n\n double calcWeight(\n const RadioMeasurementsData& radioMsr,\n const std::multimap &fingerprints);\n\n double calcWeightBestRp(\n const RadioMeasurementsData& radioMsr,\n const std::multimap &radiomap);\n\n std::shared_ptr mLevelIndex;\n std::map> mLevelsRadiomaps;\n\n const bool mUseBestRp;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // LEVEL_ESTIMATOR_RADIOMAP_H\n" }, { "path": "src/level_estimator/level_estimator_transmitters.cpp", "content": "/** level_estimator_transmitters.cpp\n*\n** Copyright (c) 2019 Navigine.\n*\n*/\n\n#include \"level_estimator_transmitters.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nLevelEstimatorTransmitters::LevelEstimatorTransmitters(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n : LevelEstimator(navProps)\n , mLevelIndex(levelCollector)\n , mUseTxPower(navProps.commonSettings.useTxPower)\n{\n}\n\nstatic double getAverageRssi(const RadioMeasurementsData& radioMsr)\n{\n double averageRssi = 0.0;\n for(int i = 0; i < (int)radioMsr.size(); ++i)\n averageRssi += radioMsr[i].rssi;\n averageRssi /= std::max((int)radioMsr.size(), 1);\n return averageRssi;\n}\n\nstatic double getAverageA(const Level& level, const RadioMeasurementsData& radioMsr)\n{\n double averageA = 0.0;\n for(int i = 0; i < (int)radioMsr.size(); ++i)\n {\n const Transmitter transmitter = level.transmitter(radioMsr[i].id);\n const PathlossModel pathLossModel = transmitter.pathlossModel;\n averageA += pathLossModel.A;\n }\n averageA /= std::max((int)radioMsr.size(), 1);\n return averageA;\n}\n\nLevelId LevelEstimatorTransmitters::detectCurrentLevel(const RadioMeasurementsData& radioMsr)\n{\n LevelId bestLevelId(\"\");\n double bestAverCost = -1000.0;\n\n for (const Level& level : mLevelIndex->levels())\n {\n RadioMeasurementsData levelMsr = getLevelRadioMeasurements(level, radioMsr);\n if (!levelMsr.empty())\n {\n double averageRssi = getAverageRssi(levelMsr);\n double averageA;\n if (mUseTxPower)\n averageA = getAverageA(level, levelMsr);\n else\n averageA = 0;\n double averageCost = averageRssi + averageA;\n // Using weighted average. Weight depends on number of visible Transmitters\n double weight = 1.0 + 0.1 * levelMsr.size() / radioMsr.size();\n averageCost /= weight;\n\n if (averageCost > bestAverCost)\n {\n bestAverCost = averageCost;\n bestLevelId = level.id();\n }\n }\n }\n\n return bestLevelId;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/level_estimator/level_estimator_transmitters.h", "content": "/** level_estimator_transmitters.h\n*\n** Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef LEVEL_ESTIMATOR_TRANSMITTERS_H\n#define LEVEL_ESTIMATOR_TRANSMITTERS_H\n\n#include \"level_estimator.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass LevelEstimatorTransmitters : public LevelEstimator\n{\npublic:\n LevelEstimatorTransmitters(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\nprotected:\n LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) override;\n\nprivate:\n std::shared_ptr mLevelIndex;\n const bool mUseTxPower;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // LEVEL_ESTIMATOR_TRANSMITTERS_H\n" }, { "path": "src/level_estimator/level_history.cpp", "content": "/** level_history.cpp\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#include \"level_history.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const int MAX_LEVEL_ID_OCCUR = 4;\n\nLevelHistory::LevelHistory()\n : mMaxLevelIdOccurrence(MAX_LEVEL_ID_OCCUR)\n , mIsInitialized(false)\n , mCurrentLevelId(LevelId(\"\"))\n{\n}\n\nLevelId LevelHistory::detectLevelUsingHistory(const LevelId& detectedLevelId)\n{\n //TODO change to boost optional!!!\n if (detectedLevelId.value == \"\")\n return mCurrentLevelId;\n\n if (!mIsInitialized)\n {\n mIsInitialized = true;\n mCurrentLevelId = detectedLevelId;\n }\n\n if (++mFloorCnt[detectedLevelId] > MAX_LEVEL_ID_OCCUR)\n {\n mFloorCnt.clear();\n mFloorCnt[detectedLevelId] = MAX_LEVEL_ID_OCCUR;\n mCurrentLevelId = detectedLevelId;\n }\n\n return mCurrentLevelId;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/level_estimator/level_history.h", "content": "/** level_history.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef LEVEL_HISTORY_H\n#define LEVEL_HISTORY_H\n\n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nclass LevelHistory\n{\n public:\n LevelHistory();\n LevelId detectLevelUsingHistory(const LevelId &detectedLevelId);\n \n private:\n const int mMaxLevelIdOccurrence;\n LevelId mCurrentLevelId;\n bool mIsInitialized;\n std::map mFloorCnt;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // LEVEL_HISTORY_H\n" }, { "path": "src/level_geometry.cpp", "content": "#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace { }\n\nLevelGeometry::LevelGeometry(const Multipolygon& allowedArea, const Box& box)\n : mAllowedArea(allowedArea)\n , mBoundingBox(box)\n{\n for (const auto& polygon : mAllowedArea)\n for (const auto& inner : polygon.inners())\n mInnerBarriers.push_back(inner);\n\n std::vector boxIndices;\n for (std::size_t i = 0; i < mInnerBarriers.size(); i++)\n {\n boost::geometry::correct(mInnerBarriers[i]);\n boxIndices.push_back({boost::geometry::return_envelope(mInnerBarriers[i]), i});\n }\n mTree = RTree(boxIndices);\n}\n\nbool LevelGeometry::isPathTouchesBarriers(const Linestring& path) const\n{\n bool isInsideAllowedArea = false;\n for (const auto& allowedZone : mAllowedArea)\n if (boost::geometry::covered_by(path, allowedZone.outer()))\n isInsideAllowedArea = true;\n if (!isInsideAllowedArea)\n return true;\n\n std::vector intersectedGeometryBoxes;\n mTree.query(boost::geometry::index::intersects(path), std::back_inserter(intersectedGeometryBoxes));\n\n for (const auto& boxIdxPair : intersectedGeometryBoxes)\n {\n if (boost::geometry::intersects(path, mInnerBarriers[boxIdxPair.second]))\n return true;\n }\n\n return false;\n}\n\nconst Multipolygon& LevelGeometry::allowedArea() const\n{\n return mAllowedArea;\n}\n\nconst Box& LevelGeometry::boundingBox() const\n{\n return mBoundingBox;\n}\n\n} } // namespace navigine::navigation_core" }, { "path": "src/likelihood/likelihood.cpp", "content": "#include \n#include \n#include \n#include \n#include \n\n#include \"likelihood.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nbool isParticleInIntersectionArea(\n const Level& level,\n const RadioMeasurementsData& radioMsr,\n double x, double y)\n{\n for (const auto& msr : radioMsr)\n {\n const Transmitter tx = level.transmitter(msr.id);\n const double circleEps = -1e-7;\n const double dx = tx.point.x - x;\n const double dy = tx.point.y - y;\n const double modelValue = std::sqrt(dx * dx + dy * dy);\n const double delta = msr.distance - modelValue;\n if (delta < circleEps)\n return false;\n }\n return true;\n};\n}\n}" }, { "path": "src/likelihood/likelihood.h", "content": "#ifndef LIKELIHOOD_H\n#define LIKELIHOOD_H\n\n#include \n#include \n#include \n#include \n\n#include \"../position.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nconst constexpr double MIN_WEIGHT_SUM_DOUBLE_EPSILON = 1e-8;\n\nenum class MeasurementLikelihoodModel {DISTANCE_V1, DISTANCE_V2, RSSI, RADIOMAP};\n\n\nclass Likelihood\n{\n public:\n virtual ~Likelihood() {}\n virtual std::vector calculateWeights(\n const Level& levelCollector,\n const RadioMeasurementsData& msr, \n const std::vector& particles) = 0;\n\n /**\n * optimization for mutation and sampling - first we perform all calculations necessary for likelihood\n * and then use this likelihood to get particle weights in method getWeight()\n */\n virtual void doPreliminaryCalculations(\n const Level& level,\n const RadioMeasurementsData& radioMsr) = 0;\n\n /**\n * return position weight base on information calculated during Likelihood::compute() method\n */\n virtual double getWeight(\n const Level& level,\n const RadioMeasurementsData& radioMsr,\n double x, double y) const = 0;\n\n virtual bool hasSufficientMeasurementsForUpdate(\n const Level& level,\n const RadioMeasurementsData& radioMsr) const = 0;\n\n virtual bool hasSufficientMeasurementsForMutation(\n const Level& level,\n const RadioMeasurementsData& radioMsr) const = 0;\n\n /**\n * Heuristic to improve particle filter robustness.\n * The method returns points for sampling particles around them.\n * Depending on implementation it can be transmitter positions with best signals,\n * or reference points with best weighs\n */\n virtual std::vector calculateSamplingCenters(\n const Level& level,\n const RadioMeasurementsData& radioMsr) = 0;\n\n virtual MeasurementLikelihoodModel getLikelihoodModel(void) = 0;\n\n virtual RadioMeasurementsData getOnlyCorrectlyIntersectedMeasurements(\n const Level& level,\n const RadioMeasurementsData& radioMsr) const = 0;\n};\n\nbool isParticleInIntersectionArea(\n const Level& level,\n const RadioMeasurementsData& radioMsr,\n double x, double y);\n\nstd::shared_ptr createLikelihoodLogModel(const std::shared_ptr& levelCollector, const NavigationSettings& navProps, MeasurementLikelihoodModel likelihoodModel);\n\n} } // namespace navigine::navigation_core\n\n#endif // LIKELIHOOD_H" }, { "path": "src/likelihood/likelihood_radiomap.cpp", "content": "#include \"likelihood_radiomap.h\"\n#include \"../knn/knn_utils.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\n#ifdef NAVIGATION_VISUALIZATION\nXYPoint LikelihoodRadiomap::debugTrianglePoint = XYPoint(0, 0);\n\nstd::map\nLikelihoodRadiomap::debugRefPointWeights = std::map();\n#endif\n\nLikelihoodRadiomap::LikelihoodRadiomap(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n : mK(navProps.commonSettings.kNeighbors)\n , mMinMsrNumForMutation(navProps.commonSettings.minMsrNumForMutation)\n , mMinMsrNumForPositionCalculation(navProps.commonSettings.minNumOfMeasurementsForPositionCalculation)\n , mUseTriangles(navProps.commonSettings.useTriangles)\n , mUseDiffMode(navProps.commonSettings.useDiffMode)\n , mLikelihoodPoint(boost::none)\n{\n if (mUseTriangles)\n mLevelTriangles = triangulate(levelCollector);\n\n mLevelReferenceTransmittersMap = getLevelReferenceTransmittersMap(levelCollector);\n mLevelsRadiomaps = getLevelsRadiomaps(levelCollector);\n}\n\nbool LikelihoodRadiomap::hasSufficientMeasurementsForUpdate(const Level& level,\n const RadioMeasurementsData& radioMsr) const\n{\n const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id());\n auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);\n return validMsr.size() >= mMinMsrNumForPositionCalculation;\n}\n\nbool LikelihoodRadiomap::hasSufficientMeasurementsForMutation(\n const Level& level,\n const RadioMeasurementsData& radioMsr) const\n{\n const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id());\n auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);\n return validMsr.size() >= mMinMsrNumForMutation;\n}\n\ndouble calcParticleWeight(\n XYParticle particle,\n XYPoint trianglePosition)\n{\n double sqrDist = (particle.x - trianglePosition.x) * (particle.x - trianglePosition.x)\n + (particle.y - trianglePosition.y) * (particle.y - trianglePosition.y);\n return 1.0 / (0.1 + sqrt(sqrDist));\n}\n\nstd::vector LikelihoodRadiomap::calculateWeights(const Level& level,\n const RadioMeasurementsData& radioMsr,\n const std::vector& particles)\n{\n const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id());\n auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);\n std::vector weightsMult(particles.size(), 1.0 / particles.size());\n\n if (validMsr.size() < mMinMsrNumForPositionCalculation)\n {\n //TODO check if error is possible and process error\n return weightsMult;\n }\n\n const std::multimap>&\n radiomap = mLevelsRadiomaps.at(level.id());\n std::map rpToWeight = calcRpWeightsKnn(radiomap, validMsr, mUseDiffMode);\n\n boost::optional p = calculateRadiomapPoint(level.id(), level.radiomap(), rpToWeight);\n\n#ifdef NAVIGATION_VISUALIZATION\n std::map weights;\n for (const auto &rpPair: rpToWeight)\n weights.insert(rpPair);\n\n debugRefPointWeights = weights;\n if (p.is_initialized())\n debugTrianglePoint = p.get();\n#endif\n\n double sum = 0.0;\n for (size_t i = 0; i < particles.size(); i++)\n {\n double w = p.is_initialized() ? calcParticleWeight(particles[i], p.get()) : 0.0;\n weightsMult[i] = w;\n sum += w;\n }\n\n if (sum > MIN_WEIGHT_SUM_DOUBLE_EPSILON)\n {\n for (size_t i = 0; i < particles.size(); i++)\n {\n weightsMult[i] /= sum;\n }\n }\n\n return weightsMult;\n}\n\nboost::optional LikelihoodRadiomap::calculateRadiomapPoint(\n const LevelId& levelId,\n const Radiomap& radiomap,\n const std::map& rpToWeight)\n{\n if (!mUseTriangles)\n {\n return calcKHeaviestRefPointsAverage(radiomap, rpToWeight, mK);\n }\n else\n {\n auto trianglesIt = mLevelTriangles.find(levelId);\n if (trianglesIt == mLevelTriangles.end())\n {\n return boost::none;\n }\n\n const std::vector& triangles = trianglesIt->second;\n if (triangles.empty())\n {\n return boost::none;\n }\n\n return calcPositionInTriangle(triangles, radiomap, rpToWeight);\n }\n}\n\nstd::vector LikelihoodRadiomap::calculateSamplingCenters(const Level& level,\n const RadioMeasurementsData& radioMsr)\n{\n this->doPreliminaryCalculations(level, radioMsr);\n\n std::vector samplingCenters;\n if (mLikelihoodPoint.is_initialized())\n samplingCenters.push_back(mLikelihoodPoint.get());\n\n return samplingCenters;\n}\n\nvoid LikelihoodRadiomap::doPreliminaryCalculations(\n const Level& level,\n const RadioMeasurementsData& radioMsr)\n{\n const std::set& txIds = mLevelReferenceTransmittersMap.at(level.id());\n auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);\n\n if (validMsr.size() >= mMinMsrNumForPositionCalculation)\n {\n const Radiomap& refPointsMap = level.radiomap();\n const std::multimap>& signalsMap =\n mLevelsRadiomaps.at(level.id());\n std::map rpToWeight =\n calcRpWeightsKnn(signalsMap, validMsr, mUseDiffMode);\n\n mLikelihoodPoint = calculateRadiomapPoint(level.id(), refPointsMap, rpToWeight);\n }\n}\n\ndouble LikelihoodRadiomap::getWeight(const Level&,\n const RadioMeasurementsData&,\n double x,\n double y) const\n{\n return mLikelihoodPoint.is_initialized()\n ? calcParticleWeight(XYParticle{x, y, 0}, mLikelihoodPoint.get())\n : 0.0;\n}\n\nRadioMeasurementsData LikelihoodRadiomap::getOnlyCorrectlyIntersectedMeasurements(\n const Level& level,\n const RadioMeasurementsData& radioMsr) const\n{\n return radioMsr;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/likelihood/likelihood_radiomap.h", "content": "#ifndef LIKELIHOOD_TRIANGULATION_H\n#define LIKELIHOOD_TRIANGULATION_H\n\n#include \n#include \n\n#include \"likelihood.h\"\n#include \"../knn/knn_utils.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass LikelihoodRadiomap : public Likelihood\n{\n public:\n LikelihoodRadiomap(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n std::vector calculateWeights(\n const Level& level,\n const RadioMeasurementsData& radioMsr,\n const std::vector& particles\n ) override;\n\n void doPreliminaryCalculations(const Level& level,\n const RadioMeasurementsData& radioMsr\n ) override;\n\n double getWeight(\n const Level& level,\n const RadioMeasurementsData& radioMsr,\n double x, double y\n ) const override;\n\n std::vector calculateSamplingCenters(\n const Level& level,\n const RadioMeasurementsData& radioMsr\n ) override;\n\n bool hasSufficientMeasurementsForUpdate(\n const Level& level,\n const RadioMeasurementsData& radioMsr\n ) const override;\n\n bool hasSufficientMeasurementsForMutation(const Level& level,\n const RadioMeasurementsData& radioMsr\n ) const override;\n\n MeasurementLikelihoodModel getLikelihoodModel(void) override{return mLikelihoodModel;}\n\n RadioMeasurementsData getOnlyCorrectlyIntersectedMeasurements(\n const Level& level,\n const RadioMeasurementsData& radioMsr\n ) const override;\n\n #ifdef NAVIGATION_VISUALIZATION\n static XYPoint debugTrianglePoint;\n static std::map debugRefPointWeights;\n #endif\n\n private:\n boost::optional calculateRadiomapPoint(\n const LevelId& levelId,\n const Radiomap& radiomap,\n const std::map& rpToWeight);\n\n const MeasurementLikelihoodModel mLikelihoodModel = MeasurementLikelihoodModel::RADIOMAP;\n const int mK;\n const size_t mMinMsrNumForMutation;\n const size_t mMinMsrNumForPositionCalculation;\n const bool mUseTriangles;\n const bool mUseDiffMode;\n\n //TODO take into account probability of such point!\n boost::optional mLikelihoodPoint; // point for particle mutation and particle sampling\n std::map> mLevelTriangles;\n std::map> mLevelReferenceTransmittersMap;\n std::map>>\n mLevelsRadiomaps;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // LIKELIHOOD_TRIANGULATION_H\n" }, { "path": "src/measurement_types.h", "content": "#pragma once\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nusing SensorMeasurement = MeasurementDataWithTimeStamp; // TODO: m.doroshenko rename me\nusing NmeaMeasurement = MeasurementDataWithTimeStamp; // TODO: m.doroshenko rename me\nusing RadioMeasurement = MeasurementDataWithTimeStamp; // TODO: m.doroshenko rename me\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/measurements/measurement_preprocessor.cpp", "content": "/** measurement_preprocessor.cpp\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#include \n#include \n\n#include \"measurement_preprocessor.h\"\n\n#if defined (DEBUG_OUTPUT_MSR_PREPROCESSOR)\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const std::string PREPROCESSED_MSR_DEBUG_OUTPUT_FILE = DEBUG_OUTPUT_MSR_PREPROCESSOR;\nvoid clearDebugOutput();\nvoid printInputSignals(NavigationMessage* navMsg, RadioMeasurementsData& entries);\nvoid printExtractedMeasurements(RadioMeasurementsData& radioMeasurements);\n\n} } // namespace navigine::navigation_core\n#endif\n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace {\n\nconst constexpr bool DEFAULT_USE_WIFI = false;\nconst constexpr bool DEFAULT_USE_BLE = true;\n\nbool checkUseWifi(const NavigationSettings& navProps) {\n const auto signalsToUse = navProps.commonSettings.signalsToUse;\n if (signalsToUse == CommonSettings::SignalsToUse::WIFI || signalsToUse == CommonSettings::SignalsToUse::BOTH) {\n return true;\n } else if (signalsToUse == CommonSettings::SignalsToUse::BLE) {\n return false;\n }\n return DEFAULT_USE_WIFI;\n}\n\nbool checkUseBle(const NavigationSettings& navProps) {\n const auto signalsToUse = navProps.commonSettings.signalsToUse;\n if (signalsToUse == CommonSettings::SignalsToUse::BLE || signalsToUse == CommonSettings::SignalsToUse::BOTH) {\n return true;\n } else if (signalsToUse == CommonSettings::SignalsToUse::WIFI) {\n return false;\n }\n return DEFAULT_USE_BLE;\n}\n\nRadioMeasurementBuffer createRadioMeasurementBuffer(\n const NavigationSettings& navProps) {\n long long sigAverageTime = (long long)(1000 * navProps.commonSettings.sigAveragingTime);\n long long sigWindowShift = (long long)(1000 * navProps.commonSettings.sigWindowShift);\n long long stopDetectTime = (long long)(1000 * navProps.commonSettings.stopDetectionTime);\n bool useStops = navProps.commonSettings.useStops;\n\n // TO DO: why RadioMeasurementBuffer is not initialized here\n if (sigWindowShift == 0 || sigWindowShift > sigAverageTime)\n sigWindowShift = sigAverageTime;\n \n long long keepRadioTimeMs = sigAverageTime - sigWindowShift;\n \n double rssiBias = 0;\n\n return RadioMeasurementBuffer(keepRadioTimeMs, sigWindowShift, stopDetectTime, rssiBias, useStops);\n}\n\n}\n\nMeasurementsPreprocessor::MeasurementsPreprocessor(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n : mLevelIndex(levelCollector)\n , mWiFiRttOffset(navProps.commonSettings.wiFiRttOffset)\n , mCutOffRssi(navProps.commonSettings.sigCutOffRss)\n , mUseClosestAps(navProps.commonSettings.useClosestAps)\n , mNumClosestAps(navProps.commonSettings.numClosestAps)\n , mUseUnknownTxs(navProps.commonSettings.useUnknownTransmitters)\n , mMeasurementType(navProps.commonSettings.measurementType)\n , mUseWifi(checkUseWifi(navProps))\n , mUseBle(checkUseBle(navProps))\n , mIsTracking(navProps.commonSettings.useTracking)\n , mRadiosBuffer(createRadioMeasurementBuffer(navProps))\n , mCurrentTs(0)\n{ }\n\nvoid MeasurementsPreprocessor::updateMeasurements(const Measurement& navMsg)\n{\n mCurrentTs = navMsg.ts;\n boost::apply_visitor(*this, navMsg.data);\n}\n\nvoid MeasurementsPreprocessor::operator()(const RadioMeasurementData& measurement)\n{\n RadioMeasurementData signal = measurement;\n boost::optional validSignalData = getValidSignalEntry(signal);\n if (validSignalData.is_initialized())\n {\n RadioMeasurement validSignal;\n validSignal.data = validSignalData.get();\n validSignal.ts = mCurrentTs;\n mRadiosBuffer.addMeasurement(validSignal);\n mLastSignalTs = mCurrentTs;\n }\n}\n\nvoid MeasurementsPreprocessor::operator()(const SensorMeasurementData& measurement)\n{\n mSensor.data = measurement;\n mSensor.ts = mCurrentTs;\n}\n\nvoid MeasurementsPreprocessor::operator()(const NmeaMeasurementData& measurement)\n{\n mNmea.data = measurement;\n mNmea.ts = mCurrentTs;\n}\n\nRadioMeasurementsData MeasurementsPreprocessor::extractRadioMeasurements(long long lastStepTime)\n{\n RadioMeasurementsData radioMeasurements = mRadiosBuffer.extractMeasurements(lastStepTime);\n if (mUseClosestAps)\n {\n std::sort(radioMeasurements.rbegin(), radioMeasurements.rend(),\n [](const RadioMeasurementData& lhs, const RadioMeasurementData& rhs)\n {\n return lhs.rssi < rhs.rssi;\n });\n\n if (radioMeasurements.size() > (unsigned int) mNumClosestAps)\n radioMeasurements.resize(mNumClosestAps);\n }\n\n for (auto& msr : radioMeasurements)\n {\n if (msr.type != RadioMeasurementData::Type::WIFI_RTT)\n {\n boost::optional levelId = mLevelIndex->findLevelByTransmitterId(msr.id);\n if (levelId.is_initialized())\n {\n const Level& level = mLevelIndex->level(levelId.get());\n Transmitter transmitter = level.transmitter(msr.id);\n const PathlossModel pathLossModel = transmitter.pathlossModel;\n msr.distance = std::exp((pathLossModel.A - msr.rssi) / pathLossModel.B);\n }\n continue;\n }\n\n msr.distance -= mWiFiRttOffset;\n }\n\n return radioMeasurements;\n}\n\nlong long MeasurementsPreprocessor::getCurrentTs() const\n{\n return mCurrentTs;\n}\n\nlong long MeasurementsPreprocessor::getLastSignalTs() const\n{\n return mLastSignalTs;\n}\n\nboost::optional MeasurementsPreprocessor::getValidSignalEntry(\n const RadioMeasurementData& entry) const\n{\n bool isTransmitterValid = mUseUnknownTxs\n || (mLevelIndex->findLevelByTransmitterId(entry.id).is_initialized()\n && isTransmitterSupported(entry.id));\n if (mMeasurementType != CommonSettings::MeasurementType::RSSI && entry.type != RadioMeasurementData::Type::WIFI_RTT)\n isTransmitterValid = false;\n if (mMeasurementType == CommonSettings::MeasurementType::RSSI && !isRssiValid(entry.rssi))\n {\n isTransmitterValid = false;\n }\n\n if (isTransmitterValid && isSignalTypeSupported(entry.type))\n return entry;\n else\n return boost::none;\n}\n\nbool MeasurementsPreprocessor::isSignalTypeSupported(RadioMeasurementData::Type signalType) const\n{\n return (signalType == RadioMeasurementData::Type::WIFI && mUseWifi) ||\n (signalType == RadioMeasurementData::Type::BEACON && mUseBle) ||\n (signalType == RadioMeasurementData::Type::BLUETOOTH && mUseBle) ||\n (signalType == RadioMeasurementData::Type::EDDYSTONE && mUseBle) ||\n (signalType == RadioMeasurementData::Type::WIFI_RTT && mUseWifi);\n}\n\nbool MeasurementsPreprocessor::isRssiValid(double rssi) const\n{\n return rssi < 0.0 && rssi > mCutOffRssi;\n}\n\nbool MeasurementsPreprocessor::isTransmitterSupported(const TransmitterId& transmitterId) const\n{\n boost::optional levelId = mLevelIndex->findLevelByTransmitterId(transmitterId);\n if (!levelId.is_initialized())\n return false;\n\n if (!mLevelIndex->hasLevel(levelId.get()))\n return false;\n\n const Level& level = mLevelIndex->level(levelId.get());\n if (!level.containsTransmitter(transmitterId))\n return false;\n\n const Transmitter& transmitter = level.transmitter(transmitterId);\n return mIsTracking ? transmitter.type == TransmitterType::LOCATOR\n : transmitter.type != TransmitterType::LOCATOR;\n}\n\nRadioMeasurementBuffer::RadioMeasurementBuffer(\n long long radioKeepPeriodMs,\n long long sigWindowShiftMs,\n long long stopDetectionTime,\n double rssiBias,\n bool useStops)\n : mRadioKeepPeriodMs(radioKeepPeriodMs)\n , mSignalWindowShiftMs(sigWindowShiftMs)\n , mStopDetectionTimeMs(stopDetectionTime)\n , mRssiBias(rssiBias)\n , mUseStops(useStops)\n , mLastExtractionTs(0)\n , mCurrentTs(0)\n , mLastStepTs(0)\n{ }\n\nvoid RadioMeasurementBuffer::addMeasurement(const RadioMeasurement& msr)\n{\n if (mCurrentTs == 0)\n {\n mCurrentTs = msr.ts;\n mLastExtractionTs = msr.ts;\n }\n\n RadioMeasurement radioMsr = msr;\n radioMsr.data.rssi = msr.data.rssi + mRssiBias;\n\n mMeasurements.push_back(radioMsr);\n mCurrentTs = msr.ts;\n}\n\nRadioMeasurementsData RadioMeasurementBuffer::extractMeasurements(long long lastStepTs)\n{\n RadioMeasurementsData measurements;\n\n if (lastStepTs > 0)\n mLastStepTs = lastStepTs;\n\n if (mCurrentTs - mLastExtractionTs < mSignalWindowShiftMs)\n {\n // if not enough time passed\n return measurements;\n }\n\n mLastExtractionTs = mCurrentTs;\n\n // Averaging measurements from same Transmitters\n std::sort(mMeasurements.begin(), mMeasurements.end(),\n [](const RadioMeasurement& lhs, const RadioMeasurement& rhs)\n {\n return lhs.data.id < rhs.data.id;\n });\n\n auto sameIdFirst = mMeasurements.cbegin();\n while (sameIdFirst != mMeasurements.cend())\n {\n auto sameIdLast = std::upper_bound(sameIdFirst, mMeasurements.cend(), *sameIdFirst,\n [](const RadioMeasurement& lhs, const RadioMeasurement& rhs)\n {\n return lhs.data.id < rhs.data.id;\n });\n\n double nSignals = std::distance(sameIdFirst, sameIdLast);\n double sumRssi = std::accumulate(sameIdFirst, sameIdLast, 0.0,\n [](double sum, const RadioMeasurement& m)\n {\n return sum + m.data.rssi;\n });\n\n double averRssi = sumRssi / nSignals;\n\n double sumDistances = std::accumulate(sameIdFirst, sameIdLast, 0.0,\n [](double sum, const RadioMeasurement& m)\n {\n return sum + m.data.distance;\n });\n\n double averDistance = sumDistances / nSignals;\n\n RadioMeasurement radioMsr = *sameIdFirst;\n radioMsr.data.rssi = averRssi;\n radioMsr.data.distance = averDistance;\n measurements.push_back(radioMsr.data);\n sameIdFirst = sameIdLast;\n }\n\n\n // In case we are standing still we don't erase old measurements\n if (mUseStops & (mLastStepTs > 0) && (mCurrentTs - mLastStepTs > mStopDetectionTimeMs))\n return measurements;\n\n // Erasing old measurements\n if (mRadioKeepPeriodMs == 0)\n {\n mMeasurements.clear();\n }\n else\n {\n long long keepFromTs = mCurrentTs - mRadioKeepPeriodMs;\n\n std::sort(mMeasurements.begin(), mMeasurements.end(),\n [](const RadioMeasurement& lhs, const RadioMeasurement& rhs)\n {\n return lhs.ts < rhs.ts;\n });\n\n auto eraseUpperBound = std::find_if(mMeasurements.begin(),\n mMeasurements.end(),\n [keepFromTs](const RadioMeasurement& rm)\n {\n return rm.ts >= keepFromTs;\n });\n\n mMeasurements.erase(mMeasurements.begin(), eraseUpperBound);\n }\n\n return measurements;\n}\n\nSensorMeasurement MeasurementsPreprocessor::getValidSensor() const\n{\n return mSensor;\n}\n\nNmeaMeasurement MeasurementsPreprocessor::getCurrentNmea() const\n{\n return mNmea;\n}\n\n#if defined (DEBUG_OUTPUT_MSR_PREPROCESSOR)\nvoid clearDebugOutput()\n{\n cout << \"print debug output to \" << PREPROCESSED_MSR_DEBUG_OUTPUT_FILE << endl;\n std::ofstream debugOutputFile;\n debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE);\n debugOutputFile << \"type id ts rssi\" << endl;\n debugOutputFile.close();\n}\n\nvoid printInputSignals(NavigationMessage* navMsg, RadioMeasurementsData& entries)\n{\n ofstream debugOutputFile;\n debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE, ofstream::out | ofstream::app);\n for (RadioMeasurement& entry: entries)\n {\n long long wsTsSec = navMsg->tmUnixTime;\n long long wsTsMs = wsTsSec * 1000 - entry.offset;\n debugOutputFile << \"INPUT \"<< entry.bssid << \" \" << wsTsMs << \" \" << entry.data.rssi << endl;\n }\n debugOutputFile.close();\n}\n\nvoid printExtractedMeasurements(RadioMeasurementsData& radioMeasurements)\n{\n ofstream debugOutputFile;\n debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE, ofstream::out | ofstream::app);\n for (RadioMeasurement msr: radioMeasurements)\n {\n debugOutputFile << \"OUTPUT \"<< msr.data.id << \" \" << msr.ts << \" \" << msr.data.rssi << endl;\n }\n debugOutputFile.close();\n}\n\n#endif\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/measurements/measurement_preprocessor.h", "content": "#ifndef MEASUREMENT_PREPROCESSOR_H\n#define MEASUREMENT_PREPROCESSOR_H\n\n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n\n#include \"../measurement_types.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass RadioMeasurementBuffer\n{\npublic:\n RadioMeasurementBuffer(const RadioMeasurementBuffer&) = default;\n\n RadioMeasurementBuffer(long long radioKeepPeriodMs,\n long long sigWindowShiftMs,\n long long stopDetectionTime,\n double rssiBias,\n bool useStops);\n\n RadioMeasurementsData extractMeasurements(long long lastStepTime);\n\n void addMeasurement(const RadioMeasurement& msr);\n\nprivate:\n const long long mRadioKeepPeriodMs;\n const long long mSignalWindowShiftMs;\n const long long mStopDetectionTimeMs;\n const double mRssiBias;\n const bool mUseStops;\n\n long long mLastExtractionTs;\n long long mCurrentTs;\n long long mLastStepTs;\n\n std::vector mMeasurements = {};\n};\n\nclass MeasurementsPreprocessor: public boost::static_visitor<>\n{\n public:\n MeasurementsPreprocessor(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n \n void updateMeasurements(const Measurement& navMsg);\n long long getCurrentTs() const;\n long long getLastSignalTs() const;\n RadioMeasurementsData extractRadioMeasurements(long long lastStepTime);\n SensorMeasurement getValidSensor() const;\n NmeaMeasurement getCurrentNmea() const;\n\n void operator()(const RadioMeasurementData& measurement);\n void operator()(const SensorMeasurementData& measurement);\n void operator()(const NmeaMeasurementData& measurement);\n\n private:\n boost::optional getValidSignalEntry(const RadioMeasurementData& entry) const;\n\n bool isSignalTypeSupported(RadioMeasurementData::Type signalType) const;\n bool isRssiValid(double rssi) const;\n std::pair transmitterLevelId(const TransmitterId& transmitterId) const;\n bool isTransmitterSupported(const TransmitterId& transmitterId) const;\n\n private:\n const std::shared_ptr mLevelIndex;\n const double mWiFiRttOffset;\n const double mCutOffRssi;\n const bool mUseClosestAps;\n const int mNumClosestAps;\n const bool mUseUnknownTxs;\n const CommonSettings::MeasurementType mMeasurementType;\n const bool mUseWifi;\n const bool mUseBle;\n const bool mIsTracking;\n long long mLastSignalTs;\n\n RadioMeasurementBuffer mRadiosBuffer;\n long long mCurrentTs;\n\n SensorMeasurement mSensor;\n NmeaMeasurement mNmea;\n};\n\n} } // namespace navigine::navigation_core\n\n\n#endif // MEASUREMENT_PREPROCESSOR_H\n" }, { "path": "src/navigation_client_impl.cpp", "content": "#include \n#include \n#include \"navigation_client_impl.h\"\n#include \"position_estimator/position_estimator_outdoor.h\"\n#include \"position_estimator/position_estimator_zone.h\"\n#include \"position_estimator/position_estimator_knn.h\"\n#include \"level_estimator/level_estimator_radiomap.h\"\n#include \"level_estimator/level_estimator_transmitters.h\"\n#include \"trilateration.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nNavigationClientImpl::NavigationClientImpl(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps) \n : mUseEnuAzimuth(navProps.commonSettings.useEnuAzimuth)\n , mNoSignalTimeThreshold((long long)(1000 * navProps.commonSettings.noSignalTimeThreshold))\n , mNoActionTimeThreshold((long long)(1000 * navProps.commonSettings.noActionTimeThreshold))\n , mLevelIndex(levelCollector)\n , mLevelEstimator(createLevelEstimator(levelCollector, navProps))\n , mPositionEstimatorIndoor(createPostitionEstimator(levelCollector, navProps))\n , mMsrPreprocessor(std::make_unique(levelCollector, navProps))\n , mPositionEstimatorOutdoor(std::make_unique(levelCollector, navProps))\n , mPositionPostprocessor(std::make_unique(navProps))\n , mSensorFusion(std::make_unique(navProps))\n , mFlagIndoorPos(false)\n , mPrevFusedPosHeading(boost::none)\n , mPrevGyroHeading(boost::none)\n , mUseAltitude(navProps.commonSettings.useAltitude)\n{ }\n\nstd::vector NavigationClientImpl::navigate(const std::vector& navBatchInput)\n{\n std::vector navBatchOutput;\n if (navBatchInput.size() == 0)\n return navBatchOutput;\n\n mNavigationStates.clear();\n long long prevTs = navBatchInput[0].ts;\n for (const auto &navInput: navBatchInput)\n {\n if (prevTs > navInput.ts)\n {\n std::cerr << \"prevTs: \" << prevTs << \", t.ts: \" << navInput.ts << std::endl;\n continue;\n }\n else\n {\n prevTs = navInput.ts;\n }\n\n NavigationStatus retStatus = NavigationStatus::OK;\n\n mMsrPreprocessor->updateMeasurements(navInput);\n const long long curTs = mMsrPreprocessor->getCurrentTs();\n const long long lastSignalTs = mMsrPreprocessor->getLastSignalTs();\n const SensorMeasurement sensorMsr = mMsrPreprocessor->getValidSensor();\n const NmeaMeasurement nmeaMsr = mMsrPreprocessor->getCurrentNmea();\n MotionInfo motionInfo = mSensorFusion->calculateDisplacement(sensorMsr, curTs);\n const auto radioMsr = mMsrPreprocessor->extractRadioMeasurements(motionInfo.lastMotionTs);\n\n LevelId levelId = mLevelEstimator->calculateLevel(radioMsr, sensorMsr);\n const Position outdoorPos = mPositionEstimatorOutdoor->calculatePosition(curTs, sensorMsr, nmeaMsr, mFlagIndoorPos);\n mFlagIndoorPos = false; // TODO refactoring needed\n\n if (curTs - lastSignalTs >= mNoSignalTimeThreshold)\n {\n mPositionEstimatorIndoor->reInit();\n levelId = LevelId();\n }\n\n if (curTs - lastSignalTs >= mNoActionTimeThreshold && curTs - motionInfo.lastMotionTs >= mNoActionTimeThreshold && motionInfo.lastMotionTs > 0)\n { \n mPositionEstimatorIndoor->reInit();\n levelId = LevelId();\n }\n\n if (mLevelIndex->hasLevel(levelId))\n {\n const Level& level = mLevelIndex->level(levelId);\n const Position indoorPos = mPositionEstimatorIndoor->calculatePosition(level, curTs, radioMsr, motionInfo, retStatus);\n if (!indoorPos.isEmpty)\n mFlagIndoorPos = true;\n\n const Position fusedPos = mPositionPostprocessor->fusePositions(curTs, indoorPos, outdoorPos, retStatus);\n\n if (!fusedPos.isEmpty && mPrevFusedPosHeading.is_initialized() && (fusedPos.heading != mPrevFusedPosHeading.get()))\n {\n mPrevGyroHeading = motionInfo.gyroHeading;\n motionInfo.isAzimuthValid = false;\n }\n else if (mPrevGyroHeading.is_initialized())\n {\n double deltaGyroHeading = motionInfo.gyroHeading - mPrevGyroHeading.get();\n motionInfo.azimuth = - (mPrevFusedPosHeading.get() - deltaGyroHeading - M_PI_2); // NED\n motionInfo.isAzimuthValid = true;\n }\n\n if (!fusedPos.isEmpty)\n mPrevFusedPosHeading = fusedPos.heading;\n\n mPosition = mPositionPostprocessor->getProcessedPosition(fusedPos, curTs, motionInfo, level);\n if (!motionInfo.isAzimuthValid) {\n motionInfo.azimuth = - (fusedPos.heading - M_PI_2); // convert from ENU to NED\n }\n\n if (mUseEnuAzimuth)\n motionInfo.azimuth = -motionInfo.azimuth + M_PI_2; // convert from NED to ENU\n }\n else if (!outdoorPos.isEmpty)\n {\n mPosition = outdoorPos;\n }\n else\n {\n retStatus = NavigationStatus::NO_LEVEL;\n }\n\n // Filling navigationSate for Debug\n auto navigationState = mPositionEstimatorIndoor->getParticleFilterState();\n navigationState.setStepCounter(motionInfo.stepCounter);\n navigationState.setStepLen(motionInfo.distance);\n navigationState.setIndoorPosition(XYPoint(mPositionPostprocessor->getIndoorPosition().x,\n mPositionPostprocessor->getIndoorPosition().y));\n navigationState.setOutdoorPosition(XYPoint(mPositionPostprocessor->getOutdoorPosition().x,\n mPositionPostprocessor->getOutdoorPosition().y));\n mNavigationStates.push_back(navigationState);\n\n // Filling navOutput\n NavigationOutput navOutput;\n navOutput.status = retStatus;\n\n // TODO: optimize NavClient state variables (mPosition, mRetStatus, etc.)\n // https://redmine.navigine.com/issues/2554\n if (mPosition.isEmpty)\n {\n if (retStatus == NavigationStatus::OK)\n {\n navOutput.status = NavigationStatus::NAVIGATION_ERROR;\n //TODO ret rid of empty position status\n }\n navBatchOutput.emplace_back(navOutput);\n continue;\n }\n else\n {\n navOutput.status = NavigationStatus::OK;\n }\n\n navOutput.posLevel = mPosition.levelId;\n\n if (navOutput.posLevel.isValid())\n {\n const Level& level = mLevelIndex->level(navOutput.posLevel);\n const GeoPoint gpsPosition = localToGps(XYPoint(mPosition.x, mPosition.y), level.bindingPoint());\n navOutput.posLatitude = gpsPosition.latitude;\n navOutput.posLongitude = gpsPosition.longitude;\n if (mUseAltitude)\n {\n Trilateration altitudeWorker;\n boost::optional altitude = altitudeWorker.calculateAltitude(level, radioMsr);\n if (altitude.is_initialized())\n navOutput.posAltitude = altitude.get();\n }\n }\n\n navOutput.posR = mPosition.deviationM;\n navOutput.provider = mPosition.provider;\n navOutput.posOrientation = to_minus_Pi_Pi(motionInfo.azimuth);\n navBatchOutput.emplace_back(navOutput);\n }\n return navBatchOutput;\n}\n\nstd::unique_ptr NavigationClientImpl::createLevelEstimator(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n{\n const bool useRadiomap = navProps.commonSettings.useRadiomap;\n\n // the following three lines are used for navigation.xml backcompatibility\n const CommonSettings::UseAlgorithm algName = navProps.commonSettings.useAlgorithm;\n const bool useKnn = (algName == CommonSettings::UseAlgorithm::KNN);\n\n if (useRadiomap || useKnn)\n return std::make_unique(levelCollector, navProps);\n\n return std::make_unique(levelCollector, navProps);\n}\n\nstd::unique_ptr NavigationClientImpl::createPostitionEstimator(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n{\n const CommonSettings::UseAlgorithm algName = navProps.commonSettings.useAlgorithm;\n \n bool usePDR;\n if (algName == CommonSettings::UseAlgorithm::KNN) return std::make_unique(levelCollector, navProps);\n\n return std::make_unique(levelCollector, navProps);\n}\n\nstd::vector NavigationClientImpl::getStates() const\n{\n return mNavigationStates;\n}\n\nstd::shared_ptr createNavigationClient(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n{\n return std::make_shared(levelCollector, navProps);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/navigation_client_impl.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"measurements/measurement_preprocessor.h\"\n#include \"position_estimator/position_estimator.h\"\n#include \"position_estimator/position_estimator_outdoor.h\"\n#include \"position_postprocessor/position_postprocessor.h\"\n#include \"sensor_fusion/sensor_fusion.h\"\n#include \"level_estimator/level_estimator.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass NavigationClientImpl : public NavigationClient\n{\n public:\n NavigationClientImpl(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n // Return navigation state of this client\n std::vector getStates() const override;\n\n // Main navigation function. Calculates current position based on incoming measurements\n std::vector navigate(const std::vector& navBatchInput) override;\n\n private:\n NavigationClientImpl(const NavigationClientImpl&) = delete;\n void operator=(const NavigationClientImpl&) = delete;\n\n std::unique_ptr createLevelEstimator(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n std::unique_ptr createPostitionEstimator(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n private:\n const bool mUseEnuAzimuth;\n const long long mNoSignalTimeThreshold;\n const long long mNoActionTimeThreshold;\n std::shared_ptr mLevelIndex;\n std::unique_ptr mLevelEstimator;\n std::unique_ptr mPositionEstimatorIndoor;\n std::unique_ptr mMsrPreprocessor;\n std::unique_ptr mPositionEstimatorOutdoor;\n std::unique_ptr mPositionPostprocessor;\n std::unique_ptr mSensorFusion;\n \n std::vector mNavigationStates = {};\n Position mPosition = {};\n bool mFlagIndoorPos;\n boost::optional mPrevFusedPosHeading;\n boost::optional mPrevGyroHeading;\n const bool mUseAltitude;\n};\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/navigation_error_codes.h", "content": "/** navigation_error_codes.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_ERROR_CODES_H\n#define NAVIGINE_NAVIGATION_ERROR_CODES_H\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const int ERROR_NO_SOLUTION = 4;\nstatic const int MISSING_STRING_CONFIG = 5;\nstatic const int MISSING_LEVELS = 6;\nstatic const int NO_PARAMETERS_IN_JSON_FILE = 21;\nstatic const int ERROR_OPEN_FILE = 25;\nstatic const int INVALID_LOC_ID_IN_JSON_FILE = 26;\nstatic const int INVALID_PARAMETER_IN_JSON_FILE = 27;\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_NAVIGATION_ERROR_CODES_H\n" }, { "path": "src/point.cpp", "content": "/** point.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nconstexpr double EQUATOR_CIRCUMFERENCE_METERS = 40075160.0;\nconstexpr double POLE_CIRCUMFERENCE_METERS = 40008000.0;\n\n// Normalize longitude to (-180, 180]\ndouble normalizeLongitude(double lon)\n{\n if (lon > 180)\n {\n lon -= 360;\n } else if (lon <= -180)\n {\n lon += 360;\n }\n return lon;\n}\n\ndouble degToRad(double deg)\n{\n return deg * M_PI / 180.0;\n}\n\nXYPoint gpsToLocal(const GeoPoint& point, const GeoPoint& bindPoint)\n{\n const double deltaLat = point.latitude - bindPoint.latitude;\n const double deltaLon = normalizeLongitude(point.longitude - bindPoint.longitude);\n const double latitudeCircumference =\n EQUATOR_CIRCUMFERENCE_METERS * std::cos(degToRad(bindPoint.latitude));\n\n XYPoint localPoint;\n localPoint.x = deltaLon * (latitudeCircumference / 360.0);\n localPoint.y = deltaLat * (POLE_CIRCUMFERENCE_METERS / 360.0);\n return localPoint;\n}\n\nGeoPoint localToGps(const XYPoint& localPoint, const GeoPoint& bindPoint)\n{\n const double latitudeCircumference = EQUATOR_CIRCUMFERENCE_METERS * std::cos(degToRad(bindPoint.latitude));\n const double lat = (localPoint.y * (360.0 / POLE_CIRCUMFERENCE_METERS)) + bindPoint.latitude;\n const double lon = normalizeLongitude(localPoint.x * 360.0 / latitudeCircumference + bindPoint.longitude);\n return GeoPoint(lat, lon);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position.h", "content": "/** position.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_POSITION_H\n#define NAVIGINE_POSITION_H\n\n#include \n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Position : XYPoint\n{\n Position()\n : XYPoint(0.0, 0.0)\n , levelId(LevelId(\"\"))\n , deviationM(0)\n , ts(0)\n , isEmpty(true)\n , provider(Provider::NONE)\n , heading(0.0)\n { }\n \n Position(double _x, double _y, LevelId _levelId,\n double _deviationM, long long _ts, bool _isEmpty, \n const Provider& _provider,\n double _heading)\n : XYPoint(_x, _y)\n , levelId(_levelId)\n , deviationM(_deviationM)\n , ts(_ts)\n , isEmpty(_isEmpty)\n , provider(_provider)\n , heading(_heading)\n { }\n \n LevelId levelId;\n double deviationM;\n long long ts;\n bool isEmpty = true; //TODO use boost optional for empty?\n Provider provider = Provider::NONE;\n double heading;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_POSITION_H\n" }, { "path": "src/position_estimator/Readme.md", "content": "\n" }, { "path": "src/position_estimator/docs/KNN.md", "content": "# K-Nearest Neighbours Positioning Algorithm\n\n\nNote:: The algorithm requires special radiomap data measurement that can beperformed using procedure called Radiomap Measurement.\nFor detailed information of data collection for algorithm usage refer to [https://docs.navigine.com/Measuring_Radiomap](https://docs.navigine.com/Measuring_Radiomap)\n\nRadiomap Measurement:: measuring the target location's radiomap on foot via an Android device with the [Navigine Application](https://github.com/Navigine/Indoor-Navigation-Android-Mobile-SDK-2.0) installed.\n\n*Requirements*:\n* infrastructure deployed\n* maps (locations) implemented\n* [radiomap data](https://docs.navigine.com/Measuring_Radiomap) collected\n*Our notation*:\n* RSSI\n* KNN\n* transmismitter\n* beacon\n* BLE\n* radio map\n## Measurements & context\n\nAlgorithm is based on an RSS mapping technique. For the set of locations in an observation area with an apriory known positions we record RSSI measurements.\n\nAll observations from multiple positions we call a radio map. From radio map we can estimate user position by measuring similarity between observations and observations measured from a known prior location.\n\n\n\n\n\n\n\n\n\n\nThe algorithms implies averaging on a radio map data, measuring and processing incoming RSSI signals from transmiters to receiver, and a signal propagation model, used for localization.\n\n### What means **access point** is KNN positioning context\n\nWe measure incoming RSSI in several points of building for a long enough time.\nAfter several minutes of collecting the signal, we perform averaging of signals. After that we have a precise measurements for a single point. Decent radio-map consists of several access points mapped to a building map.\n\nThe \"access point\" is the landmark, point with known coordinates and a history of observations from each beacon in visible range.\n\n### KNN logfile and map contents\n\n![radio-map example](img/3-10-2021-21-50-31-PM.png)\n\nOn figure above we see GeoJSON visualization containing some access points (circles).\n\nThe structure of data used for this algorithm is as follows:\n\n```json\n{\n \"type\": \"Feature\",\n \"properties\": {\n \"name\": null,\n \"type\": \"reference_point\",\n \"level\": \"5179\",\n \"id\": \"1\",\n \"uuid\": \"eff14dbc-b33a-11e8-945d-7c7a91af7e27\",\n \"entries\": [\n {\n \"value\": [\n {\n \"rssi\": 83,\n \"count\": 6\n },\n {\n ...\n },\n ],\n \"bssid\": \"20:18:00:00:04:32\",\n \"type\": \"WIFI\"\n },\n {\n ...\n }\n ]\n },\n \"geometry\": {\n \"type\": \"Point\",\n \"coordinates\": [\n -1.6307104042,\n 47.225793733499998\n ]\n }\n}\n```\n\nFor each entity of \"type\": \"reference_point\" we have many observations for each detected beacon in area.\n\nApproximately we can say:\nThe example log will consist of 20-100 access point locations, 10000 RSSI observations from 20-50 detected beacons for a single location.\n\nWe have set of reference points, for each of them RSSI data from closest beacons in area are measured. This is the starting point of an algorithm.\n\n## KNN algorithm setup\n\nThe settings we use for program configuration:\n\n```json\n\"Use_algorithm\": \"KNN\",\n\"Use_triangles\": true,\n\"Min_num_of_measurements_for_position_calculation\": 6,\n\"Use_diff_mode\": true,\n\"K\": 3,\n```\n\nWe have two mode options for this algorithm called tringle and differential modes respectively.\n\n\n\n\n\n## The algorithm\n\n### KNN algorithm math and general concept\n\nKNN approach is based on a radio-channel propagation model:\n\n

\"RSS(d)

\n\nThe goal of an RSS-based localization algorithm is to provide estimate \"\\hat\nof position \"p\" given the vector \"[RSS_1,.\n\nThere exist several algorithms that can be used to determine\nthe position of a target through RSS measurements: Some\nof them are geometric methods, such as lateration or a\nminimum—maximum method (min—max), whereas some\nothers are based on statistical approaches, such as maximum\nlikelihood.\n\nThe algorithms for localization can be classified as either statistical of geometrical.\n\nWhen the target node communicates with all the anchors, i.e., all anchors are visible, the centroid results the center of the anchors coordinates.\n\nIf we assume that all anchors we record were in the line of sight, or equally we asseme that there is no signal reflections in the model. If we have to consider noise from reflections and reflections model, the algorithm will be much more complicated. For simplicity, we work with a classical log-linear signal propagation model.\n\nAnchors - beacons transmitting the signal.\n\n

\"\\hat

\n\nIf we have \"m\" different anchor points in our collected data - cardinality of visible subset. (under assumption that all the visible anchors are equally close to target node).\n\n### Implementation\n\n#### Initialization step\n* collect measurements\n* construct building radio map\nFirstly we organise the recorded data, write set of detected anchors to their respective sublocations.\n\nWe map radiomap data to each detected beacon(*reverse mapping* of radiomap data). Thus later during localization process, we may get closest reference points from an observed set of beacons at a zero computational cost.\n\n#### RP weights estimation step\n* get measurements\n* get reference points for the respective location\n* filter outliers\n* stop until enough measurements collected\n* calculate weights of RPs based on RSS measurements and building radio map\n* calculate position based on known RPs weights and positions (see below)\nWe select the level / sublocation for localization.\nFor sublocation we select: map, known reference points and a set of detected beacons. From beacon to radio data map we obtain positions of most similar reference points.\n\nWe assume that reference points with the most similar signal are also the closest one, because of our signal prpagation model.\n\n#### Position calculation\n\nThe pose estimation from weighted set of closest reference points can be performed in different ways: geometrically, statistically, etc.\n\nBased on our settings, we have two options:\n\n##### Weighted centroid approach\n\nThe position is the weighted mean of closest reference points with respect to their likelihood / signal similarity.\n\n

\"\\hat

\n\nWhere \"n\" is the cardinality of closest reference points set.\n\n##### Triangulation\n* Triangulate our reference points set.\n* Select triange with maximum cumulative weight: \"w_i\nFrom 3 closest reference points we select the triangle; Based on our signal model we calculate the position.\n\n

\"\\hat

\n* Pros:: direct calclation with low number of reference points required. Robust solution.\n* Cons:: we don't consider information from all other reference points, our solution can be geometrically wrong and we have no options to tune the solution.\n##### Discussion\n\nThe effective number of nearest neighbours needed for lacation estimation can be different. For some cases we want to choose from 3 optimal closest points, for some cases we prefer to have averaging. All logic comes from observations, noise, location properties and optimization procedure.\n\nIf during the measurement procedure, average visible number of beacons for each access points was high, we can possibly improve our accuracy. And if there's multiple signal reflections, we may want to localize only on 3 most strong signals and rejecting all noisy information.\n\n## Preview & examples\n\n\nDemo video for KNN in triangulation mode:\n\nhttps://user-images.githubusercontent.com/68880242/139697439-7cffb9a0-e8cf-4f52-8ba3-f40350acbd14.mov\n\n" }, { "path": "src/position_estimator/position_estimator.h", "content": "#ifndef POSITION_ESTIMATOR_H\n#define POSITION_ESTIMATOR_H\n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"../position.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionEstimator\n{\npublic:\n virtual ~PositionEstimator() = default;\n\n PositionEstimator(const std::shared_ptr& levelCollector)\n : mLevelIndex(levelCollector)\n , mPosition(Position())\n , mParticleFilterState(NavigationState())\n { }\n\n virtual Position calculatePosition(\n const Level& level,\n long long ts,\n const RadioMeasurementsData& radioMsr,\n const MotionInfo& motionInfo,\n NavigationStatus& retStatus) = 0;\n\n virtual void reInit() {};\n\n NavigationState getParticleFilterState() const { return mParticleFilterState; }\n\nprotected:\n std::shared_ptr mLevelIndex;\n Position mPosition;\n NavigationState mParticleFilterState;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // POSITION_ESTIMATOR_H\n" }, { "path": "src/position_estimator/position_estimator_knn.cpp", "content": "/** position_estimator_knn.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include \n#include \n#include \n\n#include \n#include \"position_estimator_knn.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\n#ifdef NAVIGATION_VISUALIZATION\n std::map\n PositionEstimatorKnn::debugRefPointWeights = std::map();\n#endif\n\nPositionEstimatorKnn::PositionEstimatorKnn(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n : PositionEstimator(levelCollector)\n , mUseTriangles(navProps.commonSettings.useTriangles)\n , mUseDiffMode(navProps.commonSettings.useDiffMode)\n , mK(navProps.commonSettings.kNeighbors)\n , mMinMsrNumForPositionCalculation(navProps.commonSettings.minNumOfMeasurementsForPositionCalculation)\n , mLevelTriangles(triangulate(levelCollector))\n , mLevelReferenceTransmittersMap(getLevelReferenceTransmittersMap(levelCollector))\n , mLevelsRadiomaps(getLevelsRadiomaps(levelCollector))\n{\n}\n\nPosition PositionEstimatorKnn::calculatePosition(\n const Level& level,\n long long ts,\n const RadioMeasurementsData& radioMsr,\n const MotionInfo&,\n NavigationStatus &retStatus)\n{\n if (radioMsr.empty())\n {\n retStatus = NavigationStatus::NO_SOLUTION;\n return mPosition;\n }\n\n retStatus = NavigationStatus::OK;\n\n const std::set& beaconsIds = mLevelReferenceTransmittersMap.at(level.id());\n auto validMsr = getWhitelistedMeasurements(beaconsIds, radioMsr);\n if (validMsr.size() < mMinMsrNumForPositionCalculation)\n {\n retStatus = NavigationStatus::NO_SOLUTION;\n return mPosition;\n }\n\n const Radiomap& refPointsMap = level.radiomap();\n const std::multimap>& signalMap = mLevelsRadiomaps.at(level.id());\n std::map rpToWeight = calcRpWeightsKnn(signalMap, validMsr, mUseDiffMode);\n#ifdef NAVIGATION_VISUALIZATION\n std::map weights;\n for (std::map::iterator it = rpToWeight.begin();\n it != rpToWeight.end(); it++)\n weights.insert(*it);\n\n debugRefPointWeights = weights;\n#endif\n\n XYPoint p = XYPoint(0.0, 0.0);\n if (mUseTriangles)\n {\n if (mLevelTriangles.find(level.id()) == mLevelTriangles.end())\n {\n retStatus = NavigationStatus::NAVIGATION_ERROR;\n return mPosition;\n }\n\n const std::vector& triangles = mLevelTriangles.find(level.id())->second;\n\n if (triangles.empty())\n {\n retStatus = NavigationStatus::NO_RPS;\n return mPosition;\n }\n p = calcPositionInTriangle(triangles, refPointsMap, rpToWeight);\n }\n else\n {\n p = calcKHeaviestRefPointsAverage(refPointsMap, rpToWeight, mK);\n }\n\n bool isEmpty = false;\n double precision = 1.0; //TO FIX: calculate position accuracy\n mPosition = Position(p.x, p.y, level.id(), precision, ts, isEmpty, Provider::INDOOR, 0.0);\n \n return mPosition;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position_estimator/position_estimator_knn.h", "content": "/** position_estimator_knn.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef POSITION_ESTIMATOR_KNN_H\n#define POSITION_ESTIMATOR_KNN_H\n\n#include \n\n#include \"position_estimator.h\"\n#include \"../knn/knn_utils.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionEstimatorKnn: public PositionEstimator\n{\npublic:\n PositionEstimatorKnn(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n Position calculatePosition(\n const Level& level,\n long long ts,\n const RadioMeasurementsData& radioMsr,\n const MotionInfo& motionInfo,\n NavigationStatus& retStatus) override;\n\n#ifdef NAVIGATION_VISUALIZATION\n static std::map debugRefPointWeights;\n#endif\n\nprivate:\n const bool mUseTriangles;\n const bool mUseDiffMode;\n const size_t mK;\n const size_t mMinMsrNumForPositionCalculation;\n \n const std::map> mLevelTriangles;\n const std::map> mLevelReferenceTransmittersMap;\n const std::map>> mLevelsRadiomaps;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // POSITION_ESTIMATOR_KNN_H\n" }, { "path": "src/position_estimator/position_estimator_outdoor.cpp", "content": "/** position_estimator_outdoor.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n#include \n#include \"position_estimator_outdoor.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nbool SigmaFilter::add(const XYPoint& point)\n{\n bool pntValid = false;\n if (mGNSSbuffer.size() == mSigmaWindow) {\n double sumX = 0;\n double sumY = 0;\n for (auto const& pnt : mGNSSbuffer)\n {\n sumX += pnt.x;\n sumY += pnt.y;\n }\n double meanX = sumX / mSigmaWindow;\n double meanY = sumY / mSigmaWindow;\n\n double accumX = 0.0;\n double accumY = 0.0;\n std::for_each(std::begin(mGNSSbuffer), std::end(mGNSSbuffer), [&](auto const d) {\n accumX += (d.x - meanX) * (d.x - meanX);\n accumY += (d.y - meanY) * (d.y - meanY);\n });\n\n auto deviationX = 3.0 * std::sqrt(accumX / mSigmaWindow) + mSigmaDelta;\n auto deviationY = 3.0 * std::sqrt(accumY / mSigmaWindow) + mSigmaDelta;\n\n if (std::abs(meanX - point.x) <= deviationX &&\n std::abs(meanY - point.y) <= deviationY)\n pntValid = true;\n\n mGNSSbuffer.pop_front();\n }\n mGNSSbuffer.push_back(point);\n return pntValid;\n}\n\nvoid SigmaFilter::clear()\n{\n mGNSSbuffer.clear();\n}\n\nPositionEstimatorOutdoor::PositionEstimatorOutdoor(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps)\n : mLevelIndex (levelCollector)\n , mSigmaFilter(navProps.commonSettings.sigmaFilterWindow, navProps.commonSettings.sigmaFilterDelta)\n , mGpsValidTimeWindowSec(navProps.commonSettings.gpsValidTimeWindowSec)\n , mUseGpsOutsideMap(navProps.commonSettings.useGpsOutsideMap)\n , mUseSigmaFilter(navProps.commonSettings.useGpsSigmaFilter)\n , mMinNumSats(navProps.commonSettings.minNumSats)\n , mMinGpsDeviation(navProps.commonSettings.minGpsDeviation)\n , mMaxGpsDeviation(navProps.commonSettings.maxGpsDeviation)\n{ }\n\nGpsPosition PositionEstimatorOutdoor::extractGpsPosition(long long ts, const SensorMeasurement& sensorMsr)\n{\n\n if (sensorMsr.data.type == SensorMeasurementData::Type::LOCATION)\n {\n double lat = sensorMsr.data.values.x;\n double lng = sensorMsr.data.values.y;\n double dev = std::max(sensorMsr.data.values.z, mMinGpsDeviation);\n GpsPosition pos = GpsPosition(lat, lng, dev, ts);\n return pos;\n }\n else\n {\n return GpsPosition();\n }\n}\n\nint PositionEstimatorOutdoor::extractNumberOfSatellites(const NmeaMeasurement& nmeaEntry)\n{\n int totalNumSats = 0;\n\n if ((nmeaEntry.data.sentenceNumber == 1) && (nmeaEntry.data.satellitesNumber != 0))\n totalNumSats += nmeaEntry.data.satellitesNumber;\n return totalNumSats;\n}\n\nPosition PositionEstimatorOutdoor::calculatePosition(\n long long ts,\n const SensorMeasurement& sensorEntry,\n const NmeaMeasurement& nmeaEntry,\n const bool flagIndoorPos)\n{\n GpsPosition gpsPos = extractGpsPosition(ts, sensorEntry);\n int totalNumSats = extractNumberOfSatellites(nmeaEntry);\n\n if (flagIndoorPos || (gpsPos.deviation > mMaxGpsDeviation))\n {\n mSigmaFilter.clear(); //If there is indoor solution then clear sigma filter GNSS buffer.\n }\n\n if (gpsPos.isEmpty)\n {\n double timeSinceLastGpsPosition = (ts - mLastGpsPosition.ts) / 1000.0;\n if (timeSinceLastGpsPosition > mGpsValidTimeWindowSec)\n return Position(); //TODO error code: gps is empty!\n else\n return mLastGpsPosition;\n }\n\n //TODO find map closest to previous position (at least check if inside)!!!!\n for (const Level& level: mLevelIndex->levels())\n {\n XYPoint point = gpsToLocal(GeoPoint(gpsPos.latitude, gpsPos.longitude), level.bindingPoint());\n\n if ((!std::isnan(point.x) && !std::isnan(point.y)\n && boost::geometry::covered_by(Point(point.x, point.y), level.geometry().boundingBox())) || mUseGpsOutsideMap)\n {\n Position outPos;\n\n outPos.ts = gpsPos.ts;\n outPos.x = point.x;\n outPos.y = point.y;\n outPos.levelId = level.id();\n outPos.isEmpty = false;\n outPos.deviationM = gpsPos.deviation;\n outPos.provider = Provider::GNSS;\n \n bool isValid = true;\n if (outPos.ts != mLastGpsPosition.ts)\n {\n if (mUseSigmaFilter && !mSigmaFilter.add(point))\n isValid = false;\n }\n\n if (!isValid || (totalNumSats != 0 && totalNumSats < mMinNumSats))\n return Position();\n mLastGpsPosition = outPos;\n\n return outPos;\n }\n }\n\n return Position(); //TODO error code: \"can not find map for gps position!\"\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position_estimator/position_estimator_outdoor.h", "content": "/** position_estimator_outdoor.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_POSITION_ESTIMATOR_OUTDOOR_H\n#define NAVIGINE_POSITION_ESTIMATOR_OUTDOOR_H\n\n#include \n#include \n#include \n#include \n#include \n\n#include \"../position.h\"\n#include \"../measurement_types.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct GpsPosition\n{\n GpsPosition(double _latitude, double _longitude, double _deviation, long long _ts)\n : latitude (_latitude)\n , longitude (_longitude)\n , deviation (_deviation)\n , ts (_ts)\n {\n isEmpty = false;\n }\n\n GpsPosition()\n {\n isEmpty = true;\n }\n\n double latitude = NAN;\n double longitude = NAN;\n double altitude = NAN;\n double deviation = NAN;\n long long ts = 0;\n bool isEmpty = true;\n};\n\nclass SigmaFilter\n{\n public:\n SigmaFilter(\n const unsigned int& sigmaWindow,\n const double& sigmaDelta)\n : mSigmaWindow(sigmaWindow)\n , mSigmaDelta(sigmaDelta)\n {}\n\n bool add(const XYPoint& point);\n void clear();\n\n private:\n const unsigned int mSigmaWindow;\n const double mSigmaDelta;\n std::deque mGNSSbuffer;\n};\n\nclass PositionEstimatorOutdoor\n{\npublic:\n PositionEstimatorOutdoor(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n Position calculatePosition(\n long long ts,\n const SensorMeasurement& sensorMsr,\n const NmeaMeasurement& nmeaMsr,\n const bool flagIndoorPos);\n\n private:\n GpsPosition extractGpsPosition(long long ts, const SensorMeasurement& sensorEntry);\n int extractNumberOfSatellites(const NmeaMeasurement& nmeaEntry);\n Position mLastGpsPosition;\n\n std::shared_ptr mLevelIndex;\n SigmaFilter mSigmaFilter;\n const double mGpsValidTimeWindowSec;\n const bool mUseGpsOutsideMap;\n const bool mUseSigmaFilter;\n const int mMinNumSats;\n const double mMinGpsDeviation;\n const double mMaxGpsDeviation;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // POSITION_ESTIMATOR_OUTDOOR_H\n" }, { "path": "src/position_estimator/position_estimator_zone.cpp", "content": "/** position_estimator_zone.cpp\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#include \n#include \"position_estimator_zone.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nPositionEstimatorZone::PositionEstimatorZone(\n const std::shared_ptr &levelCollector,\n const NavigationSettings& navProps)\n : PositionEstimator(levelCollector)\n{\n for (const Level& level: levelCollector->levels())\n {\n mLogModelParameterA[level.id()] = navProps.levelsSettings.at(level.id()).normalModeA;\n mLogModelParameterB[level.id()] = navProps.levelsSettings.at(level.id()).normalModeB;\n }\n}\n\n// TODO take into account transmitter power!\nPosition PositionEstimatorZone::calculatePosition(\n const Level& level,\n long long ts,\n const RadioMeasurementsData& radioMsr,\n const MotionInfo&,\n NavigationStatus &retStatus)\n{\n if (radioMsr.empty())\n {\n retStatus = NavigationStatus::NO_SOLUTION;\n return mPosition;\n }\n \n retStatus = NavigationStatus::OK;\n\n RadioMeasurementsData radioMeasurements = getLevelRadioMeasurements(level, radioMsr);\n if (radioMeasurements.empty())\n {\n retStatus = NavigationStatus::NO_SOLUTION;\n return Position();\n }\n\n auto nearestTx = std::max_element(radioMeasurements.begin(), radioMeasurements.end(),\n [](RadioMeasurementData msr1, RadioMeasurementData msr2) {return msr1.rssi < msr2.rssi; });\n\n TransmitterId nearestTxId = nearestTx->id;\n\n XYZPoint p = XYZPoint(0.0, 0.0, 0.0);\n if (level.containsTransmitter(nearestTxId))\n {\n p = level.transmitter(nearestTxId).point;\n }\n\n //TODO rename prop from A to \"GP_Normal_Mode_A\"\n double A = mLogModelParameterA[level.id()];\n double B = mLogModelParameterB[level.id()];\n\n double nearestTxRssi = nearestTx->rssi;\n\n double precision = sqrt(exp((A - nearestTxRssi) / B)) + 1.0;\n\n return Position(\n p.x, \n p.y, \n level.id(),\n precision, \n ts, \n false, \n Provider::INDOOR,\n 0.0);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position_estimator/position_estimator_zone.h", "content": "/** position_estimator_zone.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef POSITION_ESTIMATOR_ZONE_H\n#define POSITION_ESTIMATOR_ZONE_H\n\n#include \"position_estimator.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionEstimatorZone: public PositionEstimator\n{\npublic:\n PositionEstimatorZone(\n const std::shared_ptr& levelCollector,\n const NavigationSettings& navProps);\n\n Position calculatePosition(\n const Level& level,\n long long ts,\n const RadioMeasurementsData& radioMsr,\n const MotionInfo& motionInfo,\n NavigationStatus& retStatus) override;\n\nprivate:\n std::map mLogModelParameterA;\n std::map mLogModelParameterB;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // POSITION_ESTIMATOR_ZONE_H\n" }, { "path": "src/position_postprocessor/navigation_time_smoother.cpp", "content": "/** navigation_time_smoother.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include \"navigation_time_smoother.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nNavigationTimeSmoother::NavigationTimeSmoother()\n : mPrevPosition() // init empty position\n , mNextPosition() // init empty position\n{\n}\n\n/**\n * Returns position smoothed in time (due to this navigation is updated every time wsMessage is received).\n * Keeps last two positions and calculates intermidiate position between them based on how much time passed since\n * last known position was calculated\n */\nPosition NavigationTimeSmoother::getTimeSmoothedPosition(const Position& lastKnownPosition, long long currentTs)\n{\n if (currentTs < lastKnownPosition.ts)\n return lastKnownPosition;\n\n if (mPrevPosition.levelId != lastKnownPosition.levelId || mNextPosition.levelId != lastKnownPosition.levelId)\n {\n mPrevPosition = lastKnownPosition;\n mNextPosition = lastKnownPosition;\n return lastKnownPosition;\n }\n\n if (mPrevPosition.isEmpty || mNextPosition.isEmpty || lastKnownPosition.isEmpty)\n {\n mPrevPosition = mNextPosition;\n mNextPosition = lastKnownPosition;\n return lastKnownPosition;\n }\n\n if (lastKnownPosition.ts > mNextPosition.ts)\n {\n mPrevPosition = mNextPosition;\n mNextPosition = lastKnownPosition;\n }\n\n if (mNextPosition.ts == mPrevPosition.ts)\n return mNextPosition;\n\n double actualTsDiff = double(currentTs - mNextPosition.ts);\n double keptTsDiff = double(mNextPosition.ts - mPrevPosition.ts);\n\n if (actualTsDiff > keptTsDiff) // we don't use extrapolation\n return lastKnownPosition;\n\n double timeSmoothedX = (mNextPosition.x * actualTsDiff + mPrevPosition.x * (keptTsDiff - actualTsDiff)) / keptTsDiff;\n double timeSmoothedY = (mNextPosition.y * actualTsDiff + mPrevPosition.y * (keptTsDiff - actualTsDiff)) / keptTsDiff;\n\n Position timeSmoothedPos = mNextPosition;\n timeSmoothedPos.x = timeSmoothedX;\n timeSmoothedPos.y = timeSmoothedY;\n timeSmoothedPos.ts = currentTs;\n\n return timeSmoothedPos;\n}\n\n} } // namespace navigine\n" }, { "path": "src/position_postprocessor/navigation_time_smoother.h", "content": "/** navigation_time_smoother.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_TIME_SMOOTHER_H\n#define NAVIGINE_NAVIGATION_TIME_SMOOTHER_H\n\n#include \"../position.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass NavigationTimeSmoother\n{\n public:\n NavigationTimeSmoother();\n Position getTimeSmoothedPosition(const Position& lastKnownPosition, long long currentTs);\n\n private:\n Position mPrevPosition;\n Position mNextPosition;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_NAVIGATION_TIME_SMOOTHER_H\n" }, { "path": "src/position_postprocessor/polynomial_fit.cpp", "content": "/** polynomial_fit.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include \"polynomial_fit.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nPolynomialFit::PolynomialFit(int power, double timeInterval, double timeShift, double newWeight)\n : mPower(power)\n , mTimeInterval(timeInterval)\n , mTimeShift(timeShift)\n , mCoeff(mPower + 1, 0.)\n , mNewSolutionWeight(newWeight)\n , mLastOutput(0)\n{\n}\n\nvoid PolynomialFit::addSequencePoint(double x, double y)\n{\n if (mY.empty())\n mCoeff.assign(mPower + 1, 0.);\n\n if (mY.empty() || mY.back() != y)\n {\n mY.push_back(y);\n mX.push_back(x);\n removeOldPoints();\n if ((int)mY.size() > mPower)\n getLeastSquaresCoeff();\n }\n}\n\nvoid PolynomialFit::removeOldPoints()\n{\n while ((mX.back() - mX.front()) > mTimeInterval)\n {\n mX.pop_front();\n mY.pop_front();\n }\n}\n\nvoid PolynomialFit::clear()\n{\n mX.clear();\n mY.clear();\n}\n\ndouble PolynomialFit::predict(double x)\n{\n double out = 0;\n double slnCoef = mNewSolutionWeight;\n int size = (int)mY.size();\n\n if (size == 0)\n {\n return -1.0;\n }\n else if (size == 1 || size < mPower + 1)\n {\n out = getLagrangeSolution(x);\n }\n else\n {\n out = getLeastSquaresSolution(x);\n out = out * slnCoef + mLastOutput * (1.0 - slnCoef);\n }\n mLastOutput = out;\n return out;\n}\n\ndouble PolynomialFit::getLagrangeSolution(double curTime)\n{\n size_t pwr = mY.size();\n double out = 0.0;\n double mult = 1.0;\n double time = curTime - mTimeShift;\n\n for (size_t i = 0; i < pwr; i++)\n {\n mult = 1.0;\n for (size_t j = 0; j < pwr; j++)\n {\n if (i != j)\n mult *= (time - mX[j]) / (mX[i] - mX[j]);\n }\n out += mY[i] * mult;\n }\n\n return out;\n}\n\ndouble PolynomialFit::getLeastSquaresSolution (double curTime)\n{\n double out = 0.0;\n double time = curTime - mTimeShift;\n \n for (int i = 0; i <= mPower; i++)\n {\n out += mCoeff[i] * pow(time, i);\n }\n return out;\n}\n\nvoid PolynomialFit::getLeastSquaresCoeff()\n{\n std::vector A ( (mPower + 1)*(mPower + 1), 0.);\n std::vector b ( mPower + 1, 0.);\n std::vector sub(2*mPower + 1, 0.);\n double sum;\n\n // Initialize sub\n for (int i = 0; i < 2 * mPower + 1; i++)\n {\n sum = 0;\n for (size_t j = 0; j < mY.size(); j++)\n {\n sum += pow(mX[j], i);\n }\n sub[i] = sum;\n }\n // Initialize matrix 'A'\n for (int i = 0; i < mPower + 1; i++)\n for (int j = 0; j < mPower + 1; j++)\n A[i + j*(mPower + 1)] = sub[i + j];\n\n // Initialize vector 'b' \n for (int i = 0; i < mPower + 1; i++)\n {\n sum = 0;\n for (size_t j = 0; j < mY.size(); j++)\n {\n sum += pow(mX[j], i) * mY[j];\n }\n b[i] = sum;\n }\n\n solveLinearSystem(A, b, mCoeff, mPower + 1);\n\n return;\n}\n\nvoid solveLinearSystem(const std::vector & A, const std::vector & b,\n std::vector & x, const int n)\n{\n // Rotations method\n int i, j, k;\n double cosphi, sinphi, norm, temp;\n std::vector E(n*(n + 1));\n // E initialization. E is a [n x (n+1)] matrix formed as\n // E = [A,b] to simplify the calculations \n for (int i = 0; i < n; i++)\n {\n E[i*(n + 1) + n] = b[i];\n for (int j = 0; j < n; j++)\n E[i*(n + 1) + j] = A[i*n + j];\n }\n\n // Forward Gaussian\n for (k = 0; k < n; ++k)\n for (j = k + 1; j < n; ++j)\n {\n cosphi = E[k * (n + 1) + k];\n sinphi = E[j * (n + 1) + k];\n norm = sqrt(cosphi * cosphi + sinphi * sinphi);\n cosphi /= norm;\n sinphi /= norm;\n for (i = k; i < n + 1; ++i)\n {\n temp = cosphi * E[j * (n + 1) + i] - sinphi * E[k * (n + 1) + i];\n E[k * (n + 1) + i] = sinphi * E[j * (n + 1) + i] + cosphi * E[k * (n + 1) + i];\n E[j * (n + 1) + i] = temp;\n }\n }\n // Back-Gaussing\n for (k = n - 1; k >= 0; --k)\n {\n x[k] = E[k*(n + 1) + n];\n for (i = k + 1; i < n; ++i)\n x[k] -= E[k*(n + 1) + i] * x[i];\n x[k] /= E[k*(n + 1) + k];\n }\n return;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position_postprocessor/polynomial_fit.h", "content": "/** polynomial_fit.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef POLYNOMIAL_FIT_H\n#define POLYNOMIAL_FIT_H\n\n#include \n#include \n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nvoid solveLinearSystem(const std::vector & A, const std::vector & b,\n std::vector & x, const int n);\n\nclass PolynomialFit\n{\n public:\n PolynomialFit(int power = 1, double timeInterval = 2.0, double timeShift = 0.7, double newWeight = 0.2);\n\n void addSequencePoint(double x, double y);\n double predict(double x);\n void clear();\n \n private:\n const int mPower;\n const double mTimeInterval;\n const double mTimeShift;\n const double mNewSolutionWeight;\n double mLastOutput;\n\n std::deque mY;\n std::deque mX;\n std::vector mCoeff;\n\n void removeOldPoints();\n void getLeastSquaresCoeff();\n double getLagrangeSolution(const double x);\n double getLeastSquaresSolution(const double x);\n};\n\n} } // namespace navigine::navigation_core\n\n#endif\n" }, { "path": "src/position_postprocessor/position_postprocessor.cpp", "content": "#include \n#include \"../geometry.h\"\n#include \"position_postprocessor.h\"\n#include \"position_smoother_ab.h\"\n#include \"position_smoother_lstsq.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nPositionPostprocessor::PositionPostprocessor(const NavigationSettings& navProps)\n : mUseSmoothing(navProps.commonSettings.useSmoothing)\n , mUseStops(navProps.commonSettings.useStops)\n , mUseInstantGpsPosition(navProps.commonSettings.useInstantGpsPosition)\n , mUseGps(navProps.commonSettings.useGps)\n , mFuseGps(navProps.commonSettings.fuseGps)\n , mUseGpsOutsideMap(navProps.commonSettings.useGpsOutsideMap)\n , mPreferIndoorSolution(navProps.commonSettings.preferIndoorSolution)\n , mUseTimeSmoothing(navProps.commonSettings.useTimeSmoothing)\n , mUseGraphProjection(navProps.commonSettings.useGraphProjection)\n , mDeadReckonTimeMs((long long)(1000 * navProps.commonSettings.deadReckoningTime))\n , mStopUpdateTimeMs((long long)(1000 * navProps.commonSettings.stopUpdateTime))\n , mStopDetectionTimeMs((long long)(1000 * navProps.commonSettings.stopDetectionTime))\n , mGraphProjectionDistance(navProps.commonSettings.graphProjDist)\n , mPriorDev(navProps.commonSettings.priorDeviation)\n , mFuseGpsBorderM(navProps.commonSettings.fuseGpsBorderM)\n , mMotionSpeed(navProps.commonSettings.averageMovSpeed)\n , mPosIsTooOldForFusingSec(navProps.commonSettings.positionIsTooOldSec)\n , mStopsDistThresholdM(navProps.commonSettings.useStopsDistanceThresholdM)\n , mIndoorDev(navProps.commonSettings.priorDeviation)\n , mOutdoorDev(navProps.commonSettings.priorDeviation)\n , mNavigationTimeSmoother()\n , mIndoorPos()\n , mOutdoorPos()\n , mLastOutPosition()\n{\n mLastExtractionTs = 0;\n\n bool useAbFilter = navProps.commonSettings.useAbFilter;\n\n if (useAbFilter)\n mPositionSmoother = new PositionSmootherAB(navProps);\n else\n mPositionSmoother = new PositionSmootherLstsq(navProps);\n}\n\nPositionPostprocessor::~PositionPostprocessor()\n{\n if (mPositionSmoother)\n delete mPositionSmoother;\n}\n\nvoid PositionPostprocessor::fillIndoorOutdoorPositionStates(\n const Position& indoorPos, \n const Position& outdoorPos)\n{\n if (!indoorPos.isEmpty)\n {\n double priorIndoorDev = mIndoorPos.isEmpty ? mPriorDev : std::min(mPriorDev, mIndoorDev + mMotionSpeed * (indoorPos.ts - mIndoorPos.ts) / 1000.0);\n mIndoorPos = indoorPos;\n mIndoorDev = (priorIndoorDev + indoorPos.deviationM) / 2.0;\n }\n\n if (!outdoorPos.isEmpty)\n {\n double priorOutdoorDev = mOutdoorPos.isEmpty ? mPriorDev : std::min(mPriorDev, mOutdoorDev + mMotionSpeed * (outdoorPos.ts - mOutdoorPos.ts) / 1000.0);\n mOutdoorDev = (priorOutdoorDev + outdoorPos.deviationM) / 2.0;\n mOutdoorPos = outdoorPos;\n }\n}\n\nPosition PositionPostprocessor::fusePositions(\n long long curTs, \n const Position& indoorPos, \n const Position& outdoorPos,\n const NavigationStatus& navStatus)\n{\n fillIndoorOutdoorPositionStates(indoorPos, outdoorPos);\n\n // return instant gps position for debugging:\n if (mUseInstantGpsPosition && !outdoorPos.isEmpty)\n return outdoorPos;\n\n if (mPreferIndoorSolution && !indoorPos.isEmpty and navStatus==0)\n return indoorPos;\n\n if (!mUseGps || mOutdoorPos.isEmpty)\n return mIndoorPos;\n\n if (mIndoorPos.isEmpty)\n return mOutdoorPos;\n\n double indoorPositionDelay = curTs >= mIndoorPos.ts ? (curTs - mIndoorPos.ts) / 1000.0 : 0.0;\n double outdoorPositionDelay = curTs >= mOutdoorPos.ts ? (curTs - mOutdoorPos.ts) / 1000.0 : 0.0;\n\n if ((indoorPositionDelay) > mPosIsTooOldForFusingSec)\n return mOutdoorPos;\n\n if ((outdoorPositionDelay) > mPosIsTooOldForFusingSec)\n return mIndoorPos;\n\n double indoorDev = mMotionSpeed * indoorPositionDelay + mIndoorDev;\n double outdoorDev = mMotionSpeed * outdoorPositionDelay + mOutdoorDev;\n\n double positionsDist = GetDist(mIndoorPos.x, mIndoorPos.y, mOutdoorPos.x, mOutdoorPos.y);\n double accuracyDist = mIndoorPos.deviationM + mOutdoorPos.deviationM + mFuseGpsBorderM;\n\n if (mFuseGps && (accuracyDist > positionsDist))\n {\n Position fusedPos = mIndoorPos;\n fusedPos.ts = std::max(mIndoorPos.ts, mOutdoorPos.ts);\n fusedPos.x = (mIndoorPos.x * outdoorDev + mOutdoorPos.x * indoorDev) / (outdoorDev + indoorDev);\n fusedPos.y = (mIndoorPos.y * outdoorDev + mOutdoorPos.y * indoorDev) / (outdoorDev + indoorDev);\n fusedPos.deviationM = std::min(indoorDev, outdoorDev);\n fusedPos.provider = Provider::FUSED;\n return fusedPos;\n }\n else\n return indoorDev < outdoorDev ? mIndoorPos : mOutdoorPos;\n}\n\nLocationPoint getProjection(const Graph& graph, const LocationPoint& P)\n{\n LocationPoint P0 = P;\n double d0 = NAN;\n std::pair e0;\n \n for (auto edgePair = graph.edgesBegin(); edgePair != graph.edgesEnd(); ++edgePair)\n {\n const Graph::Vertex& u = graph.getVertex((*edgePair).first);\n const Graph::Vertex& v = graph.getVertex((*edgePair).second);\n\n LocationPoint P1 = P;\n double k = GetProjection(u.point.x, u.point.y, v.point.x, v.point.y, P.x, P.y, &P1.x, &P1.y);\n\n if (k < 0)\n P1 = LocationPoint{P.level, u.point.x, u.point.y};\n else if (k > 1)\n P1 = LocationPoint{P.level, v.point.x, v.point.y};\n\n double d = GetDist(P, P1);\n if (std::isnan(d0) || d < d0)\n {\n P0 = P1;\n d0 = d;\n e0 = *edgePair;\n }\n }\n\n return P0;\n}\n\nbool PositionPostprocessor::isInsideMap(const Position& pos, const Level& level)\n{\n if (mUseGpsOutsideMap)\n return true;\n return boost::geometry::covered_by(static_cast(pos), level.geometry().boundingBox());\n}\n\nPosition PositionPostprocessor::getProcessedPosition(const Position& fusedPosition,\n long long ts,\n const MotionInfo& motionInfo,\n const Level& level)\n{\n if (motionInfo.lastMotionTs > 0)\n mLastStepTs = motionInfo.lastMotionTs;\n\n if (fusedPosition.isEmpty)\n return Position(); //TODO error code \"fused position is empty!\"\n\n if (!isInsideMap(fusedPosition, level))\n return Position(); // Position is outside of map!\n\n if (ts - fusedPosition.ts >= mDeadReckonTimeMs)\n return Position(); //TODO error code \"last received position is too old!\"\n\n Position result = fusedPosition;\n\n if (mUseSmoothing)\n {\n if (mLastOutPosition.levelId != result.levelId)\n mPositionSmoother->reset(result);\n result = mPositionSmoother->smoothPosition(result, level);\n }\n\n if (mUseTimeSmoothing)\n result = mNavigationTimeSmoother.getTimeSmoothedPosition(result, ts);\n\n //TODO check if pedometer is available before using steps!\n //TODO be very carefull when use stops and gps simultaneously\n bool lastStepWasTooLongAgo = (mLastStepTs > 0) && (ts - mLastStepTs >= mStopDetectionTimeMs);\n double dist = GetDist(mLastOutPosition.x, mLastOutPosition.y, result.x, result.y);\n bool newPositionIsNotTooFar = dist < mStopsDistThresholdM;\n\n mLastOutPosition.ts = ts;\n if (mUseStops && lastStepWasTooLongAgo)\n {\n if ((ts - mLastExtractionTs > mStopUpdateTimeMs) && newPositionIsNotTooFar)\n {\n mLastExtractionTs = ts;\n mLastOutPosition = result;\n mLastOutPosition.ts = ts;\n }\n else\n {\n mLastOutPosition.ts = ts;\n }\n }\n else\n {\n mLastOutPosition = result;\n mLastOutPosition.ts = ts;\n mLastOutPosition.deviationM = mIndoorPos.deviationM;\n }\n\n\n // Projecting coordinates to graph\n if (mUseGraphProjection)\n {\n const LocationPoint P1 = {mLastOutPosition.levelId, mLastOutPosition.x, mLastOutPosition.y};\n const LocationPoint P2 = getProjection(level.graph(), P1);\n const double distance = GetDist(P1, P2);\n\n if (!std::isnan(distance) && distance < mGraphProjectionDistance)\n {\n mLastOutPosition.x = P2.x;\n mLastOutPosition.y = P2.y;\n }\n }\n\n // Fitting coordinates inside box\n Point minCorner = level.geometry().boundingBox().min_corner();\n Point maxCorner = level.geometry().boundingBox().max_corner();\n mLastOutPosition.x = std::min(std::max(mLastOutPosition.x, minCorner.x()), maxCorner.x());\n mLastOutPosition.y = std::min(std::max(mLastOutPosition.y, minCorner.y()), maxCorner.y());\n\n // Rounding coordinates to 0.01\n mLastOutPosition.x = std::floor(mLastOutPosition.x * 100) / 100;\n mLastOutPosition.y = std::floor(mLastOutPosition.y * 100) / 100;\n\n return mLastOutPosition;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position_postprocessor/position_postprocessor.h", "content": "#ifndef POSITION_POSTPROCESSOR_H\n#define POSITION_POSTPROCESSOR_H\n\n#include \n#include \n#include \n\n#include \"../position.h\"\n#include \"navigation_time_smoother.h\"\n#include \"position_smoother.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionPostprocessor\n{\n public:\n ~PositionPostprocessor();\n PositionPostprocessor(const NavigationSettings& navProps);\n\n Position getProcessedPosition(const Position& fusedPos,\n long long ts, const MotionInfo& motionInfo,\n const Level& level);\n\n Position fusePositions(long long curTs, const Position& indoorPos, const Position& outdoorPos, const NavigationStatus& navStatus);\n void fillIndoorOutdoorPositionStates(const Position& indoorPos, const Position& outdoorPos);\n const Position getIndoorPosition() { return mIndoorPos; }\n const Position getOutdoorPosition() { return mOutdoorPos; }\n\n private:\n bool isInsideMap(const Position& pos, const Level& level);\n\n const bool mUseSmoothing;\n const bool mUseStops;\n const bool mUseInstantGpsPosition;\n const bool mUseGps;\n const bool mFuseGps;\n const bool mUseGpsOutsideMap;\n const bool mPreferIndoorSolution;\n const bool mUseTimeSmoothing;\n const bool mUseGraphProjection;\n\n const long long mDeadReckonTimeMs;\n const long long mStopUpdateTimeMs;\n const long long mStopDetectionTimeMs;\n\n const double mGraphProjectionDistance;\n const double mPriorDev;\n const double mFuseGpsBorderM;\n const double mMotionSpeed;\n const double mPosIsTooOldForFusingSec;\n const double mStopsDistThresholdM;\n\n double mIndoorDev;\n double mOutdoorDev;\n\n NavigationTimeSmoother mNavigationTimeSmoother;\n PositionSmoother* mPositionSmoother;\n\n Position mIndoorPos;\n Position mOutdoorPos;\n Position mLastOutPosition;\n\n long long mLastExtractionTs = -1;\n long long mLastStepTs = -1;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // POSITION_POSTPROCESSOR_H\n" }, { "path": "src/position_postprocessor/position_smoother.h", "content": "#ifndef NAVIGINE_POSITION_SMOOTHER_H\n#define NAVIGINE_POSITION_SMOOTHER_H\n\n#include \n#include \n#include \"../position.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionSmoother\n{\npublic:\n virtual ~PositionSmoother() {}\n virtual Position smoothPosition(Position pos, const Level& level) = 0;\n virtual void reset(Position pos) = 0;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_POSITION_SMOOTHER_H\n" }, { "path": "src/position_postprocessor/position_smoother_ab.cpp", "content": "#include \n#include \"position_smoother_ab.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double MAX_ALPHA = 0.9;\nstatic const double MIN_ALPHA = 0.1;\nstatic const double TIME_THRESHOLD_SEC = 10;\nstatic const double MAX_OBJECT_VELOCITY_MPS = 4;\n\nPositionSmootherAB::PositionSmootherAB(const NavigationSettings& navProps)\n : mUseBarriers(navProps.commonSettings.useBarriers)\n , mUseSpeedConstraint(navProps.commonSettings.useSpeedConstraint)\n , mUseAccuracyForSmoothing(navProps.commonSettings.useAccuracyForSmoothing)\n , mWasCalled(false)\n , mSpeedX(0.0)\n , mSpeedY(0.0)\n{\n double smoothing = navProps.commonSettings.smoothing;\n smoothing = std::min(1.0, std::max(0.0, smoothing));\n mAlpha = MIN_ALPHA * smoothing + MAX_ALPHA * (1.0 - smoothing);\n}\n\nPosition PositionSmootherAB::smoothPosition(Position pos, const Level& level)\n{\n // assert(pos.ts > mPosition.ts);\n if (pos.ts > mPosition.ts)\n {\n double t = (pos.ts - mPosition.ts) / 1000.0;\n double a = mAlpha;\n\n if (mUseAccuracyForSmoothing)\n {\n double newAlpha = mPosition.deviationM / (pos.deviationM + mPosition.deviationM + 0.5);\n a = std::min(std::max(MIN_ALPHA, newAlpha), MAX_ALPHA);\n }\n\n /* temporary commented - it boost the filter to obtain the minimal speed */\n // if (std::sqrt(mSpeedX * mSpeedX + mSpeedY * mSpeedY) < 0.5)\n // a = MAX_ALPHA / 2.0;\n double b = a * a / (2.0 - a);\n\n double xp = mPosition.x + mSpeedX * t;\n double vxp = mSpeedX + (b / t) * (pos.x - xp);\n double xs = xp + a * (pos.x - xp);\n\n double yp = mPosition.y + mSpeedY * t;\n double vyp = mSpeedY + (b / t) * (pos.y - yp);\n double ys = yp + a * (pos.y - yp);\n \n double velocity = std::sqrt(vxp * vxp + vyp * vyp);\n\n bool timeIsTooLong = t > TIME_THRESHOLD_SEC;\n bool velocityIsTooFast = (mUseSpeedConstraint && velocity > MAX_OBJECT_VELOCITY_MPS);\n bool isInsideBarrier = (mUseBarriers && !boost::geometry::covered_by(Point(xs, ys), level.geometry().allowedArea()));\n\n if (!mWasCalled || timeIsTooLong || velocityIsTooFast || isInsideBarrier)\n {\n mPosition = pos;\n mSpeedX = 0.0;\n mSpeedY = 0.0;\n mWasCalled = true;\n return pos;\n }\n\n mSpeedX = vxp;\n mSpeedY = vyp;\n\n if (std::isfinite(xs) && std::isfinite(ys))\n {\n pos.x = xs;\n pos.y = ys;\n }\n\n mPosition = pos;\n if (mUseAccuracyForSmoothing)\n {\n //TODO more real accuracy update\n double predictedAccuracy = mPosition.deviationM + t * std::sqrt(mSpeedX * mSpeedX + mSpeedY * mSpeedY);\n mPosition.deviationM = (1.0 - a) * predictedAccuracy + a * pos.deviationM;\n }\n }\n return mPosition;\n}\n\nvoid PositionSmootherAB::reset(Position pos)\n{\n mPosition = pos;\n mSpeedX = 0.0;\n mSpeedY = 0.0;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position_postprocessor/position_smoother_ab.h", "content": "#ifndef NAVIGINE_POSITION_SMOOTHER_AB_H\n#define NAVIGINE_POSITION_SMOOTHER_AB_H\n\n#include \"position_smoother.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionSmootherAB: public PositionSmoother\n{\n public:\n PositionSmootherAB(const NavigationSettings& navProps);\n Position smoothPosition(Position pos, const Level& level) override;\n void reset(Position pos) override;\n\n private:\n const bool mUseBarriers;\n const bool mUseSpeedConstraint;\n const bool mUseAccuracyForSmoothing;\n bool mWasCalled = false;\n double mSpeedX = 0.0;\n double mSpeedY = 0.0;\n double mAlpha = 0.0;\n\n Position mPosition;\n long long mTsMs;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_POSITION_SMOOTHER_AB_H\n" }, { "path": "src/position_postprocessor/position_smoother_lstsq.cpp", "content": "#include \n#include \"position_smoother_lstsq.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nPositionSmootherLstsq::PositionSmootherLstsq(const NavigationSettings& navProps)\n : mWasCalled(false)\n , mUseBarriers(navProps.commonSettings.useBarriers)\n , mTimeInterval(navProps.commonSettings.sigAveragingTime)\n , mWindowShift(navProps.commonSettings.sigWindowShift)\n , mSmoothing(std::min(1.0, std::max(0.0, navProps.commonSettings.smoothing)))\n , mPolyFitX(1, mTimeInterval, mWindowShift, mSmoothing)\n , mPolyFitY(1, mTimeInterval, mWindowShift, mSmoothing)\n{\n}\n\nPosition PositionSmootherLstsq::smoothPosition(Position pos, const Level& level)\n{\n if (!mWasCalled)\n {\n mStartTime = pos.ts;\n mWasCalled = true;\n return pos;\n }\n\n double t = (pos.ts - mStartTime) / 1000.0;\n mPolyFitX.addSequencePoint(t, pos.x);\n mPolyFitY.addSequencePoint(t, pos.y);\n\n double xs = mPolyFitX.predict(t);\n double ys = mPolyFitY.predict(t);\n\n if (mUseBarriers && !boost::geometry::covered_by(Point(xs, ys), level.geometry().allowedArea()))\n {\n mPolyFitX.clear();\n mPolyFitY.clear();\n mStartTime = pos.ts;\n return pos;\n }\n\n if (std::isfinite(xs) && std::isfinite(ys))\n {\n pos.x = xs;\n pos.y = ys;\n }\n\n return pos;\n}\n\nvoid PositionSmootherLstsq::reset(Position pos)\n{\n mPolyFitX.clear();\n mPolyFitY.clear();\n mStartTime = pos.ts;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/position_postprocessor/position_smoother_lstsq.h", "content": "#ifndef NAVIGINE_POSITION_SMOOTHER_LSTSQ_H\n#define NAVIGINE_POSITION_SMOOTHER_LSTSQ_H\n\n#include \"position_smoother.h\"\n#include \"polynomial_fit.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionSmootherLstsq: public PositionSmoother\n{\n public:\n PositionSmootherLstsq(const NavigationSettings& navProps);\n Position smoothPosition(Position pos, const Level& level) override;\n void reset(Position pos) override;\n\n private:\n const double mTimeInterval;\n const double mWindowShift;\n const double mSmoothing;\n bool mWasCalled;\n bool mUseBarriers;\n PolynomialFit mPolyFitX;\n PolynomialFit mPolyFitY;\n long long mStartTime;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_POSITION_SMOOTHER_LSTSQ_H\n" }, { "path": "src/radiomap.cpp", "content": "/** radiomap.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nRadiomap::Radiomap(const XYReferencePoints& referencePoints)\n{\n mReferencePoints = referencePoints;\n for (std::size_t i = 0; i < referencePoints.size(); i++)\n mReferencePointsIndex.insert({referencePoints[i].id, i});\n}\n\nconst XYReferencePoints& Radiomap::referencePoints() const\n{\n return mReferencePoints;\n}\n\nbool Radiomap::hasReferencePoint(const ReferencePointId& refPointId) const\n{\n return mReferencePointsIndex.find(refPointId) != mReferencePointsIndex.end();\n}\n\nReferencePoint Radiomap::getReferencePoint(const ReferencePointId& refPointId) const\n{\n return mReferencePoints.at(mReferencePointsIndex.at(refPointId));\n}\n\n} } // namespace navigine::navigation_core\n\n" }, { "path": "src/sensor_fusion/complementary_filter.cpp", "content": "/** complementary_filter.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include \n#include \n#include \"complementary_filter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double CF_DOUBLE_EPSILON = 1e-8;\nstatic const double MIN_SAMPLING_PERIOD_SEC = 0.02;\nstatic const double LPF_TIME_CONSTANT_SEC = 1.0;\n\ndouble to_minus_Pi_Pi(double x)\n{\n while ( x >= M_PI ) x -= 2 * M_PI;\n while ( x < -M_PI ) x += 2 * M_PI;\n return x;\n}\n\nQuaternion updateQuaternion(const Quaternion& q, const Vector3d& v)\n{\n Quaternion qDot = q * Quaternion(v) * 0.5;\n Quaternion qOut = q + qDot;\n return qOut.normalized();\n}\n \nSensorMeasurement lowPassFilter(const SensorMeasurement& prevMsr, const SensorMeasurement& currMsr)\n{\n double dt = (currMsr.ts - prevMsr.ts) / 1000.0;\n double alpha = LPF_TIME_CONSTANT_SEC / (LPF_TIME_CONSTANT_SEC + std::max(dt, MIN_SAMPLING_PERIOD_SEC));\n\n SensorMeasurement res = currMsr;\n res.data.values = alpha * prevMsr.data.values + (1 - alpha) * currMsr.data.values;\n\n return res;\n}\n\n/** This function returns calculated orientation (in radians) and current timestamp */\n/* the orientation is right-handed, therefore counter-clockwise rotation has positive sign */\nOrientation ComplementaryFilter::getFusedOrientation() const\n{\n double roll, pitch, yaw;\n std::tie(roll, pitch, yaw) = mQ.toEuler();\n return Orientation{roll, pitch, yaw, mCurrentTs};\n}\n\n/** This function returns calculated fused azimuth (in radians), clockwise has positive sign */\ndouble ComplementaryFilter::getFusedAzimuth() const\n{\n double roll, pitch, yaw;\n std::tie(roll, pitch, yaw) = mQ.toEuler();\n return -yaw;\n}\n\n/** This function returns calculated magnetic azimuth (in radians), clockwise has positive sign */\ndouble ComplementaryFilter::getMagneticAzimuth() const\n{\n return mMagneticAzimuth;\n}\n\n/** This function returns azimuth calculated on device (in radians), clockwise has positive sign */\ndouble ComplementaryFilter::getDeviceAzimuth() const\n{\n return mDeviceAzimuth;\n}\n\n/** This function returns calculated gyro heading (in radians), clockwise has positive sign */\ndouble ComplementaryFilter::getGyroHeading() const\n{\n return mGyroHeading;\n}\n\n/** measurements time must be in ascending order! */\nvoid ComplementaryFilter::update(const SensorMeasurement& msr)\n{\n \n if(msr.data.type == SensorMeasurementData::Type::ACCELEROMETER)\n {\n mAccelLowPassed = lowPassFilter(mAccelLowPassed, msr);\n updateUsingAccelerometer(msr);\n }\n else if (msr.data.type == SensorMeasurementData::Type::MAGNETOMETER)\n {\n mMagnLowPassed = lowPassFilter(mMagnLowPassed, msr);\n if (mLastGyroTs == -1)\n updateUsingMagnetometer(msr);\n\n mMagneticAzimuth = calculateMagneticAzimuth(msr);\n }\n else if (msr.data.type == SensorMeasurementData::Type::GYROSCOPE)\n {\n if (mLastGyroTs == -1)\n mLastGyroTs = msr.ts;\n\n updateUsingGyroscope(msr);\n mGyroHeading = getFusedAzimuth();\n }\n else if (msr.data.type == SensorMeasurementData::Type::ORIENTATION)\n {\n mDeviceAzimuth = msr.data.values.x;\n mLastDeviceAzimuthTs = msr.ts;\n }\n\n mCurrentTs = msr.ts;\n \n return;\n}\n\nbool ComplementaryFilter::isDeviceAzimuthOutdated() const\n{\n double dt = static_cast(mCurrentTs - mLastDeviceAzimuthTs) / 1000.0;\n if (dt > mDeviceAzimuthLifetimeSeconds || mLastDeviceAzimuthTs == -1)\n return true;\n else\n return false;\n}\n\nvoid ComplementaryFilter::updateUsingGyroscope(const SensorMeasurement& gyroMeas)\n{\n double dt = static_cast(gyroMeas.ts - mLastGyroTs) / 1000.0;\n mLastGyroTs = gyroMeas.ts;\n Vector3d gyro = gyroMeas.data.values;\n Vector3d omega = gyro * dt + mIntergalError * dt;\n mQ = updateQuaternion(mQ, omega);\n} \n\nvoid ComplementaryFilter::updateUsingAccelerometer(const SensorMeasurement& accelMeas)\n{\n Vector3d acc = accelMeas.data.values.normalized();\n Vector3d gravSensorFrame = (mQ.conj() * Quaternion(0.0, 0.0, 0.0, 1.0) * mQ).toVector3d();\n Vector3d error = Vector3d::crossProduct(acc, gravSensorFrame);\n Vector3d rotation = mKaccelerometer * error;\n\n mQ = updateQuaternion(mQ, rotation);\n mIntergalError += (mKintergalGain > CF_DOUBLE_EPSILON) ? (mKintergalGain * error) : Vector3d(0.0, 0.0, 0.0);\n}\n\nvoid ComplementaryFilter::updateUsingMagnetometer(const SensorMeasurement& magnMeas)\n{\n Vector3d magn = magnMeas.data.values.normalized();\n if (magn.magnitude() < CF_DOUBLE_EPSILON)\n return;\n \n Vector3d magnGlobalFrame = (mQ * Quaternion(magn) * mQ.conj()).toVector3d();\n magnGlobalFrame.y = std::sqrt(magnGlobalFrame.y * magnGlobalFrame.y + magnGlobalFrame.x * magnGlobalFrame.x);\n magnGlobalFrame.x = 0.0;\n Vector3d magnSensorFrame = (mQ.conj() * Quaternion(magnGlobalFrame) * mQ).toVector3d();\n\n Vector3d error = Vector3d::crossProduct(magn, magnSensorFrame);\n Vector3d rotation = mKmagnetometer * error;\n\n mQ = updateQuaternion(mQ, rotation);\n mIntergalError += (mKintergalGain > CF_DOUBLE_EPSILON) ? (mKintergalGain * error) : Vector3d(0.0, 0.0, 0.0);\n}\n\ndouble ComplementaryFilter::calculateMagneticAzimuth(const SensorMeasurement& magn)\n{\n double roll, pitch, yaw;\n std::tie(roll, pitch, yaw) = mQ.toEuler();\n double halfYaw = yaw / 2.0;\n\n Quaternion quatDifference = Quaternion(sin(halfYaw), 0, 0, cos(halfYaw)).normalized();\n Quaternion quatZeroYaw = quatDifference * mQ;\n\n Vector3d magnetometer = magn.data.values.normalized();\n Vector3d magnGlobalFrame = (quatZeroYaw * Quaternion(magnetometer) * quatZeroYaw.conj()).toVector3d();\n\n return to_minus_Pi_Pi(M_PI / 2.0 + atan2(magnGlobalFrame.y, magnGlobalFrame.x));\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/sensor_fusion/complementary_filter.h", "content": "/** complementary_filter\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef COMPLEMENTARY_FILTER_H\n#define COMPLEMENTARY_FILTER_H\n\n#include \n#include \n#include \n\n#include \"quaternion.h\"\n\n#include \"../measurement_types.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\ndouble to_minus_Pi_Pi(double x);\n\nstruct Orientation\n{\n double roll = 0.0;\n double pitch = 0.0;\n double yaw = 0.0;\n long long ts = 0;\n Orientation(double roll_, double pitch_, double yaw_, long long ts_) \n : roll{roll_}\n , pitch{pitch_}\n , yaw{yaw_}\n , ts{ts_} {};\n};\n\nclass ComplementaryFilter\n{\n public:\n ComplementaryFilter (const NavigationSettings& navProps, const double Ka = 0.1, const double Km = 0.05, const double Ki = 0.0)\n : mKaccelerometer (Ka)\n , mKmagnetometer (Km)\n , mKintergalGain (Ki)\n , mDeviceAzimuthLifetimeSeconds(navProps.commonSettings.deviceAzimuthLifetimeSeconds)\n {};\n \n double getMagneticAzimuth() const;\n double getFusedAzimuth() const;\n double getDeviceAzimuth() const;\n double getGyroHeading() const;\n Orientation getFusedOrientation() const;\n void update(const SensorMeasurement& msr);\n bool isDeviceAzimuthOutdated() const;\n \n private:\n void updateUsingGyroscope (const SensorMeasurement& gyro);\n void updateUsingAccelerometer (const SensorMeasurement& accel);\n void updateUsingMagnetometer (const SensorMeasurement& magn);\n double calculateMagneticAzimuth (const SensorMeasurement& magn);\n\n long long mCurrentTs = -1;\n long long mLastGyroTs = -1;\n long long mLastDeviceAzimuthTs = -1;\n const double mKaccelerometer;\n const double mKmagnetometer;\n const double mKintergalGain;\n const double mDeviceAzimuthLifetimeSeconds;\n Vector3d mIntergalError = {};\n Quaternion mQ = {};\n double mMagneticAzimuth = 0.0;\n double mDeviceAzimuth = 0.0;\n double mGyroHeading = 0.0;\n SensorMeasurement mMagnLowPassed; // Magnetometer measurements passed through low-pass filter\n SensorMeasurement mAccelLowPassed; // Accelerometer measurements passed through low-pass filter\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // COMPLEMENTARY_FILTER_H\n" }, { "path": "src/sensor_fusion/pedometer.cpp", "content": "/** pedometer.cpp\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#include \n\n#include \n#include \n#include \n\n#include \"pedometer.h\"\n//#define DEBUG_OUTPUT_PEDOMETER \n\nnamespace navigine {\nnamespace navigation_core {\n\n#if defined (DEBUG_OUTPUT_PEDOMETER)\n#include \n#include \n static const std::string PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE = DEBUG_OUTPUT_PEDOMETER;\n\n void clearPedometerDebugOutput();\n void printDebugPedometer(const std::string &type, long long ts, double value);\n#endif\n\nstatic const long long AVERAGING_TIME_INTERVAL_MS = 2500; //2000 // Time interval on which we calculate averaged filtered magnitude in order to align it relative to zero value\nstatic const long long FILTER_TIME_INTERVAL_MS = 200; // Filtered acc measurements are calculated as average over this time interval\n\nstatic const long long UPDATE_TIME_INTERVAL_MS = 700; // 650 // The time interval(in millisec) on which we update accelMax, Min, threshold\nstatic const long long MIN_TIME_BETWEEN_STEPS_MS = 300; // We cut off possible steps if time between them is less then this thresh\n\nstatic const double STEP_LENGTH_CONST = 0.52; //0.48 // Constant using in algorithm which is responsible for step length calculation\nstatic const double MINIMAL_THRESHOLD_VALUE = 0.05 * 9.80665; // Empirical threshold values from article p.888\n\nstatic const int MINIMAL_NUMBER_OF_STEPS = 5; // First N steps in which we accumulate data about average step time\nstatic const int MAXIMUM_NUMBER_OF_STEPS = 50; // Maximum number of steps to make an assumption about average step time\nstatic const int MAX_STEP_TIME = 2000; // The maximum possible time for step duration\n\nvoid Pedometer::update(const SensorMeasurement& msr)\n{\n mMagnitudeSize = (std::max)(mFilteredAccMagnitudes.size(), size_t(1));\n\n if (msr.data.type == SensorMeasurementData::Type::ACCELEROMETER)\n {\n mAccelMeasurements.push_back(msr);\n Magnitude magn = calculateFilteredAccMagnitudes();\n if (magn.ts!=0)\n mFilteredAccMagnitudes.push_back(magn);\n\n while (mAccelMeasurements.back().ts - mAccelMeasurements.front().ts > 2 * AVERAGING_TIME_INTERVAL_MS &&\n mAccelMeasurements.size() > 0)\n {\n mAccelMeasurements.pop_front();\n }\n \n#if defined (DEBUG_OUTPUT_PEDOMETER)\n if (mAccelMeasurements.size() == 1) \n {\n //Remove output file at the start of the test\n clearPedometerDebugOutput(); \n } \n if (magn.ts != 0)\n printDebugPedometer(\"MAGNITUDE\", magn.ts, magn.value);\n\n printDebugPedometer(\"INPUT\", msr.ts, msr.getMagnitude()); \n#endif\n }\n}\n\n/**\n * This method calculates and form array with filtered acceleration magnitude\n * sliding window average is used (low pass). This function also updates average\n * filtered magnitude\n */\nMagnitude Pedometer::calculateFilteredAccMagnitudes() const\n{\n long long lastMeasTs = mAccelMeasurements.back().ts;\n\n std::deque::const_iterator lBorderAverIt = mAccelMeasurements.begin();\n while (lastMeasTs - lBorderAverIt->ts >= AVERAGING_TIME_INTERVAL_MS)\n lBorderAverIt++;\n \n double averMagnitude = std::accumulate(lBorderAverIt, mAccelMeasurements.end(), 0.0,\n [](double sum, const SensorMeasurement msr)\n {return sum+msr.data.values.magnitude();});\n double nMeasAverage = std::distance(lBorderAverIt, mAccelMeasurements.end());\n\n std::deque::const_iterator lBorderFilterIt = mAccelMeasurements.begin();\n while (lastMeasTs - lBorderFilterIt->ts >= FILTER_TIME_INTERVAL_MS)\n lBorderFilterIt++;\n\n double filterMagnitude = std::accumulate(lBorderFilterIt, mAccelMeasurements.end(), 0.0, \n [](double sum, const SensorMeasurement msr)\n {return sum+msr.data.values.magnitude();});\n double nMeasFiltered = std::distance(lBorderFilterIt, mAccelMeasurements.end());\n\n // There is no measurements at acceleration array!\\n\");\n if (nMeasAverage == 0 || nMeasFiltered == 0)\n return Magnitude(0,0);\n\n filterMagnitude /= nMeasFiltered;\n averMagnitude += filterMagnitude; // For correct processing at the very beginning\n averMagnitude /= nMeasAverage;\n filterMagnitude -= averMagnitude;\n \n return Magnitude(lastMeasTs, filterMagnitude);\n}\n\nstd::deque Pedometer::calculateSteps()\n{\n std::deque steps = {};\n if (mFilteredAccMagnitudes.size() <= 3) //There is no enough accelerometer measurements to detect steps\n return steps;\n\n long long averageStepTime = 0;\n auto nSteps = std::max(std::distance(mTimes.begin(), mTimes.end()), std::ptrdiff_t(1));\n\n if (nSteps >= MINIMAL_NUMBER_OF_STEPS) \n averageStepTime = std::accumulate(mTimes.begin(), mTimes.end(), 0, \n [](long long sum, long long s) \n { return sum + s; });\n averageStepTime /= nSteps;\n double timeBetweenSteps = std::max(1.0 * MIN_TIME_BETWEEN_STEPS_MS, 0.6 * averageStepTime);\n\n for (size_t i = mMagnitudeSize; i < mFilteredAccMagnitudes.size(); i++)\n {\n Magnitude curAcc = mFilteredAccMagnitudes[i];\n Magnitude prevAcc = mFilteredAccMagnitudes[i-1];\n \n //True if cross threshold and if new detection isn't too early.\n if (!mIsStep &&\n prevAcc.value < MINIMAL_THRESHOLD_VALUE &&\n curAcc.value > MINIMAL_THRESHOLD_VALUE &&\n curAcc.ts - mPossibleStepTs > MIN_TIME_BETWEEN_STEPS_MS &&\n timeBetweenSteps > 0) // && curAcc.ts - mPossibleStepTs > timeBetweenSteps\n {\n mIsStep = true;\n mPossibleStepTs = curAcc.ts;\n }\n\n if (mIsStep)\n {\n double stepLength = calculateStepLength(UPDATE_TIME_INTERVAL_MS, mFilteredAccMagnitudes.begin()+i);\n if (mPossibleStepTs - mStepTime < MAX_STEP_TIME)\n mTimes.push_back(mPossibleStepTs - mStepTime);\n\n steps.push_back(Step(mPossibleStepTs, stepLength));\n#if defined (DEBUG_OUTPUT_PEDOMETER)\n printDebugPedometer(\"STEP\", mPossibleStepTs, stepLength);\n#endif\n mStepTime = mPossibleStepTs;\n mIsStep = false; // prepare to detect new step\n }\n }\n\n while(mFilteredAccMagnitudes.back().ts - mFilteredAccMagnitudes.front().ts > 2 * UPDATE_TIME_INTERVAL_MS)\n mFilteredAccMagnitudes.pop_front();\n\n while (mTimes.size() > MAXIMUM_NUMBER_OF_STEPS) \n mTimes.pop_front(); \n\n return steps;\n}\n\ndouble Pedometer::calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt)\n{\n // find left border\n std::deque::const_iterator leftBorderIt = rightBorderIt;\n while (rightBorderIt->ts - leftBorderIt->ts <= timeIntervalMs &&\n leftBorderIt != mFilteredAccMagnitudes.begin() )\n leftBorderIt--;\n\n double maxMagn = std::max_element(leftBorderIt, rightBorderIt,\n [](Magnitude m1, Magnitude m2) \n {return m1.value < m2.value; })->value;\n double minMagn = std::min_element(leftBorderIt, rightBorderIt, \n [](Magnitude m1, Magnitude m2)\n {return m1.value < m2.value; })->value;\n double accAmplitude = maxMagn - minMagn;\n double stepLen = STEP_LENGTH_CONST * sqrt(sqrt(accAmplitude));\n\n return stepLen;\n}\n\n#if defined (DEBUG_OUTPUT_PEDOMETER)\nvoid clearPedometerDebugOutput()\n{\n std::cout << \"print debug output to \" << PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE << std::endl;\n std::ofstream debugOutputFile;\n debugOutputFile.open(PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE);\n debugOutputFile << \"type ts value\" << std::endl;\n debugOutputFile.close();\n}\n\nvoid printDebugPedometer(const std::string &type, long long ts, double value)\n{\n std::ofstream debugOutputFile;\n debugOutputFile.open(PREPROCESSED_PEDOMETER_MSR_DEBUG_OUTPUT_FILE, std::ofstream::out | std::ofstream::app);\n debugOutputFile << type << \" \" << ts << \" \" << value << std::endl;\n debugOutputFile.close();\n}\n#endif\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/sensor_fusion/pedometer.h", "content": "/** pedometer.h\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_PEDOMETER_H\n#define NAVIGINE_PEDOMETER_H\n\n#include \n\n#include \n\n#include \"../measurement_types.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Step\n{\n long long ts;\n double len;\n Step() : ts(0), len(0) {};\n Step(long long stepTs, double stepLength) : ts(stepTs), len(stepLength) {};\n};\n\nstruct Magnitude\n{\n long long ts {};\n double value {};\n Magnitude() : ts(0), value(0) {};\n Magnitude(long long msrTs, double magnitudeValue) : ts(msrTs), value(magnitudeValue) {};\n};\n\n/**\n * This class detects step of the human on the basis of accelerometer measurements\n * and empirical model of humans gate.\n * Step length is calculated using minimal and maximal accelerations\n*/\nclass Pedometer\n{\npublic:\n Pedometer() = default;\n \n void update(const SensorMeasurement& accMsr);\n std::deque calculateSteps();\n\nprivate:\n Magnitude calculateFilteredAccMagnitudes() const;\n double calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt);\n\n //we use deque in order to fast delete measurements of the array when we got new ones\n std::deque mAccelMeasurements; // Array for saving acceleration measurements\n std::deque mFilteredAccMagnitudes; // Array for saving filtered acceleration magnitudes\n std::deque mTimes; // Array for saving step durations\n \n std::size_t mMagnitudeSize = 0;\n long long mPossibleStepTs = -1; // The time when step possibly occurred (sec in unix)\n long long mStepTime = -1; // The time when previous step possibly occured (sec in unix)\n bool mIsStep = false; // We use this variable to mark possible step\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // NAVIGINE_PEDOMETER_H\n" }, { "path": "src/sensor_fusion/quaternion.cpp", "content": "/** quaternion.cpp\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#include \n#include \"quaternion.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nQuaternion::Quaternion(double _w, double _x, double _y, double _z)\n : w{_w}, x{_x}, y{_y}, z{_z}\n{ }\n\nQuaternion::Quaternion(double _x, double _y, double _z)\n : w{0}, x{_x}, y{_y}, z{_z}\n{ }\n\nQuaternion::Quaternion(const Vector3d& vec)\n : w{0.0}, x{vec.x}, y{vec.y}, z{vec.z}\n{ }\n\nQuaternion Quaternion::normalized() const\n{\n double norm = std::sqrt(w * w + x * x + y * y + z * z);\n\n return (std::fabs(norm) > std::numeric_limits::epsilon())\n ? Quaternion(w /norm, x/norm, y / norm, z / norm)\n : *this;\n}\n\nstd::tuple Quaternion::toEuler() const\n{\n\tdouble sinr = 2.0 * (w * x + y * z);\n\tdouble cosr = 1.0 - 2.0 * (x * x + y * y);\n\tauto roll = std::atan2(sinr, cosr);\n\n\tdouble sinp = 2.0 * (w * y - z * x);\n double pitch = 0.0;\n\tif (std::fabs(sinp) >= 1)\n\t\tpitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range\n\telse\n\t\tpitch = asin(sinp);\n\n\tdouble siny = 2.0 * (w * z + x * y);\n\tdouble cosy = 1.0 - 2.0 * (y * y + z * z); \n\tdouble yaw = std::atan2(siny, cosy);\n \n return std::make_tuple(roll, pitch, yaw);\n}\n\nQuaternion& Quaternion::operator+=(const Quaternion& r_q)\n{\n this->w += r_q.w;\n this->x += r_q.x;\n this->y += r_q.y;\n this->z += r_q.z;\n return *this;\n}\n\nQuaternion& Quaternion::operator-=(const Quaternion& r_q)\n{\n this->w -= r_q.w;\n this->x -= r_q.x;\n this->y -= r_q.y;\n this->z -= r_q.z;\n return *this;\n}\n\nQuaternion& Quaternion::operator*=(const double& scale)\n{\n this->w *= scale;\n this->x *= scale;\n this->y *= scale;\n this->z *= scale;\n return *this;\n}\n\nQuaternion& Quaternion::operator*=(const Quaternion& other)\n{\n double qw = w, qx = x, qy = y, qz = z;\n this->w = qw*other.w - qx*other.x - qy*other.y - qz*other.z;\n this->x = qw*other.x + qx*other.w + qy*other.z - qz*other.y;\n this->y = qw*other.y - qx*other.z + qy*other.w + qz*other.x;\n this->z = qw*other.z + qx*other.y - qy*other.x + qz*other.w;\n return *this;\n}\n\nQuaternion operator+(Quaternion l_q, const Quaternion& r_q)\n{\n return l_q += r_q;\n}\n\nQuaternion operator-(Quaternion l_q, const Quaternion& r_q)\n{\n return l_q -= r_q;\n}\n\nQuaternion operator*(Quaternion l_q, const double& scale)\n{\n return l_q *= scale;\n}\n\nQuaternion operator*(const double& scale, Quaternion l_q)\n{\n return l_q *= scale;\n}\n\nQuaternion operator*(Quaternion l_q, const Quaternion& r_q)\n{\n return l_q *= r_q;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/sensor_fusion/quaternion.h", "content": "/** quaternion.h\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_QUATERNION_H\n#define NAVIGINE_QUATERNION_H\n\n#include \n\n#include \n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nclass Quaternion\n{\n public:\n double w = 1.0;\n double x = 0.0;\n double y = 0.0;\n double z = 0.0;\n \n public:\n Quaternion(){};\n Quaternion(double _w, double x, double _y, double _z);\n Quaternion(double _x, double _y, double _z);\n Quaternion(const Vector3d& vec);\n\n Quaternion normalized() const;\n Quaternion conj() const { return Quaternion(w, -x, -y, -z); }\n Vector3d toVector3d() const { return Vector3d(x, y, z); }\n\n std::tuple toEuler() const;\n \n Quaternion& operator+=(const Quaternion& r_q);\n Quaternion& operator-=(const Quaternion& r_q);\n Quaternion& operator*=(const double& scale);\n Quaternion& operator*=(const Quaternion& r_q);\n};\n\nQuaternion operator+(Quaternion l_q, const Quaternion& r_q);\nQuaternion operator-(Quaternion l_q, const Quaternion& r_q);\nQuaternion operator*(Quaternion l_q, const double& scale);\nQuaternion operator*(const double& scale, Quaternion l_q);\nQuaternion operator*(Quaternion q1, const Quaternion& q2);\n\n} } // namespace navigine::navigation_core\n\n#endif //NAVIGINE_QUATERNION_H\n" }, { "path": "src/sensor_fusion/sensor_fusion.cpp", "content": "#include \n#include \n#include \n#include \"sensor_fusion.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nSensorFusion::SensorFusion(const NavigationSettings& navProps)\n : mUseCalcAzimuth (navProps.commonSettings.useCalculatedAzimuth)\n , mStepMultiplier (navProps.commonSettings.stepMultiplier)\n , mComplFilter ( ComplementaryFilter(navProps) )\n , mPedometer ( Pedometer() )\n{\n}\n\nMotionInfo SensorFusion::calculateDisplacement(const SensorMeasurement& msr, long long tsMs)\n{\n mPedometer.update(msr);\n std::deque steps = mPedometer.calculateSteps();\n \n mComplFilter.update(msr);\n\n MotionInfo motionInfo;\n motionInfo.heading = -mComplFilter.getFusedAzimuth(); // the counter-clockwise rotation needed\n \n if (mUseCalcAzimuth)\n {\n motionInfo.azimuth = mComplFilter.getMagneticAzimuth();\n motionInfo.isAzimuthValid = true;\n }\n else if (!mComplFilter.isDeviceAzimuthOutdated())\n {\n motionInfo.azimuth = mComplFilter.getDeviceAzimuth();\n motionInfo.isAzimuthValid = true;\n }\n\n motionInfo.gyroHeading = mComplFilter.getGyroHeading();\n motionInfo.isStepDetected = !steps.empty();\n if (motionInfo.isStepDetected)\n {\n motionInfo.deltaAngle = motionInfo.heading - mPrevHeading;\n mPrevHeading = motionInfo.heading;\n motionInfo.distance = std::accumulate(steps.begin(), steps.end(), 0.0,\n [](double sum, const Step& s) { return sum + s.len; });\n motionInfo.distance *= mStepMultiplier;\n motionInfo.lastMotionTs = tsMs;\n mStepCounter += steps.size();\n motionInfo.stepCounter = mStepCounter;\n }\n\n return motionInfo;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/sensor_fusion/sensor_fusion.h", "content": "/** sensor_measurement.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef _SENSOR_FUSION_H_\n#define _SENSOR_FUSION_H_\n\n#include \n#include \n#include \n\n#include \n\n#include \"complementary_filter.h\"\n#include \"pedometer.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass SensorFusion\n{\npublic:\n SensorFusion(const NavigationSettings& navProps);\n MotionInfo calculateDisplacement(const SensorMeasurement& msr, long long tsMs);\n\nprivate:\n const bool mUseCalcAzimuth;\n const double mStepMultiplier;\n ComplementaryFilter mComplFilter;\n Pedometer mPedometer;\n int mStepCounter = 0;\n double mPrevHeading = 0.0;\n};\n\n} } // namespace navigine::navigation_core\n\n#endif // _SENSOR_FUSION_H_\n" }, { "path": "src/triangulation.cpp", "content": "#include \"triangulation.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double INDENT_METERS = 10.0;\n\nstd::vector TriangulateVertices(const std::vector& vertices, double maxedge)\n{\n std::vector T;\n if (vertices.size() < 3)\n return T;\n\n // Borders of the rectangle\n double minX = std::numeric_limits::max();\n double maxX = -std::numeric_limits::max();\n double minY = std::numeric_limits::max();\n double maxY = -std::numeric_limits::max();\n\n // find rectangle containing all the vertices\n for (std::vector::const_iterator it = vertices.begin(); it != vertices.end(); it++)\n {\n const TriangleVertex& v = *it;\n if (v.x > maxX)\n maxX = v.x;\n\n if (v.x < minX)\n minX = v.x;\n\n if (v.y > maxY)\n maxY = v.y;\n\n if (v.y < minY)\n minY = v.y;\n }\n\n // Adding indents to avoid checks for boundary cases\n minX -= INDENT_METERS;\n maxX += INDENT_METERS;\n minY -= INDENT_METERS;\n maxY += INDENT_METERS;\n\n std::vector V;\n\n // Defining \"super-triangle\"\n V.push_back(TriangleVertex(minX + minY - maxY, minY, ReferencePointId(\"super1\")));\n V.push_back(TriangleVertex(maxX + maxY - minY, minY, ReferencePointId(\"super2\")));\n V.push_back(TriangleVertex((minX + maxX) / 2,\n maxY + (maxX - minX) / 2,\n ReferencePointId(\"super3\")));\n\n V.insert(V.end(), vertices.begin(), vertices.end());\n\n T.push_back(Triangle(V[0], V[1], V[2]));\n\n // Adding vertices to the triangulation\n for(size_t i = 0; i < V.size(); ++i)\n {\n // Looking for triangles, for which the circumscribed circle contains V[i]\n // Removing those triangles, saving their edges in edge buffer E\n std::vector E;\n for(size_t j = 0; j < T.size(); )\n {\n if (T[j].circumcircle_contains(V[i]))\n {\n E.push_back(T[j].getAB());\n E.push_back(T[j].getBC());\n E.push_back(T[j].getCA());\n T.erase(T.begin() + j);\n continue;\n }\n ++j;\n }\n\n // Adding all triangles between edges and current vertex\n // ignoring the duplicating edges\n std::sort(E.begin(), E.end());\n for(size_t j = 0; j < E.size(); ++j)\n {\n if (j + 1 < E.size() && E[j] == E[j+1])\n ++j;\n else\n T.push_back(Triangle(V[i], E[j].begin(), E[j].end()));\n }\n }\n\n double minsin = -1;\n for(size_t i = 0; i < T.size(); ++i)\n {\n if (T[i].getA() == V[0] || T[i].getB() == V[0] || T[i].getC() == V[0] ||\n T[i].getA() == V[1] || T[i].getB() == V[1] || T[i].getC() == V[1] ||\n T[i].getA() == V[2] || T[i].getB() == V[2] || T[i].getC() == V[2] ||\n T[i].getAB().length() > maxedge ||\n T[i].getBC().length() > maxedge ||\n T[i].getAC().length() > maxedge ||\n T[i].sinA() < minsin ||\n T[i].sinB() < minsin ||\n T[i].sinC() < minsin)\n T.erase(T.begin() + i--);\n }\n V.erase(V.begin(), V.begin() + 3); // Erasing elements V[0], V[1], V[2]\n std::sort(V.begin(), V.end());\n std::sort(T.begin(), T.end());\n\n return T;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/triangulation.h", "content": "#ifndef NAVIGINE_TRIANGULATION_H\n#define NAVIGINE_TRIANGULATION_H\n\n#include \n#include \n#include \n\n#include \"geometry.h\"\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct TriangleVertex\n{\n TriangleVertex(double x_, double y_, ReferencePointId id_):\n x(x_), y(y_), id(id_) { }\n\n bool operator==(const TriangleVertex& v)const { return id == v.id; }\n bool operator!=(const TriangleVertex& v)const { return id != v.id; }\n bool operator< (const TriangleVertex& v)const { return x < v.x || (x == v.x && y < v.y); }\n bool operator> (const TriangleVertex& v)const { return x > v.x || (x == v.x && y > v.y); }\n bool operator<=(const TriangleVertex& v)const { return !(*this > v); }\n bool operator>=(const TriangleVertex& v)const { return !(*this < v); }\n\n double x;\n double y;\n ReferencePointId id;\n};\n\nclass TriangleEdge\n{\n public:\n TriangleEdge(const TriangleVertex& a, const TriangleVertex& b)\n : mBegin(std::min(a, b)), mEnd(std::max(a, b))\n { }\n\n TriangleVertex begin()const { return mBegin; }\n TriangleVertex end()const { return mEnd; }\n\n double length()const { return sqrt((mBegin.x - mEnd.x) * (mBegin.x - mEnd.x) +\n (mBegin.y - mEnd.y) * (mBegin.y - mEnd.y)); }\n\n bool operator==(const TriangleEdge& e)const { return mBegin == e.mBegin && mEnd == e.mEnd; }\n bool operator!=(const TriangleEdge& e)const { return mBegin != e.mBegin || mEnd != e.mEnd; }\n bool operator< (const TriangleEdge& e)const { return mBegin < e.mBegin || (mBegin == e.mBegin && mEnd < e.mEnd); }\n bool operator> (const TriangleEdge& e)const { return mBegin > e.mBegin || (mBegin == e.mBegin && mEnd > e.mEnd); }\n bool operator<=(const TriangleEdge& e)const { return !(*this > e); }\n bool operator>=(const TriangleEdge& e)const { return !(*this < e); }\n\n private:\n TriangleVertex mBegin;\n TriangleVertex mEnd;\n};\n\nclass Triangle\n{\n public:\n Triangle(const TriangleVertex& a,\n const TriangleVertex& b,\n const TriangleVertex& c)\n : mA(a), mB(b), mC(c)\n { }\n\n TriangleVertex getA()const { return mA; }\n TriangleVertex getB()const { return mB; }\n TriangleVertex getC()const { return mC; }\n\n TriangleEdge getAB()const { return TriangleEdge(mA, mB); }\n TriangleEdge getBA()const { return TriangleEdge(mA, mB); }\n\n TriangleEdge getBC()const { return TriangleEdge(mB, mC); }\n TriangleEdge getCB()const { return TriangleEdge(mB, mC); }\n\n TriangleEdge getAC()const { return TriangleEdge(mA, mC); }\n TriangleEdge getCA()const { return TriangleEdge(mA, mC); }\n\n bool operator==(const Triangle& t)const { return mA == t.mA && mB == t.mB && mC == t.mC; }\n bool operator!=(const Triangle& t)const { return mA != t.mA || mB != t.mB || mC != t.mC; }\n bool operator< (const Triangle& t)const { return mA < t.mA || (mA == t.mA && mB < t.mB) || (mA == t.mA && mB == t.mB && mC < t.mC); }\n bool operator> (const Triangle& t)const { return mA > t.mA || (mA == t.mA && mB > t.mB) || (mA == t.mA && mB == t.mB && mC > t.mC); }\n bool operator<=(const Triangle& t)const { return !(*this > t); }\n bool operator>=(const Triangle& t)const { return !(*this < t); }\n\n double square()const\n {\n return 0.5 * fabs(Determinant(mA.x, mB.x, mC.x, mA.y, mB.y, mC.y, 1, 1, 1));\n }\n\n double sinA()const\n {\n return 2 * square() / (getAC().length() * getAB().length());\n }\n\n double sinB()const\n {\n return 2 * square() / (getBC().length() * getAB().length());\n }\n\n double sinC()const\n {\n return 2 * square() / (getBC().length() * getAC().length());\n }\n\n bool circumcircle_contains(const TriangleVertex& X)const\n {\n if (X == mA || X == mB || X == mC)\n return false;\n double D1 = Determinant(mA.x, mB.x, mC.x, mA.y, mB.y, mC.y, 1, 1, 1);\n double D2 = Determinant(mA.x - X.x, mA.y - X.y, (mA.x * mA.x - X.x * X.x) + (mA.y * mA.y - X.y * X.y),\n mB.x - X.x, mB.y - X.y, (mB.x * mB.x - X.x * X.x) + (mB.y * mB.y - X.y * X.y),\n mC.x - X.x, mC.y - X.y, (mC.x * mC.x - X.x * X.x) + (mC.y * mC.y - X.y * X.y));\n return D1 * D2 > GEOMETRY_DOUBLE_EPSILON;\n }\n\n private:\n TriangleVertex mA;\n TriangleVertex mB;\n TriangleVertex mC;\n};\n\nstd::vector TriangulateVertices(const std::vector &vertices, double maxedge);\n\n} } // namespace navigine::navigation_core\n\n#endif // TRIANGULATION_H\n" }, { "path": "src/trilateration.cpp", "content": "/** trilateration.cpp\n *\n * Copyright (c) 2021 Navigine.\n *\n */\n#include \"trilateration.h\"\n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nRadioMeasurementsData getIntersectedMeasurements(const Level &level, const RadioMeasurementsData& radioMsr)\n{\n RadioMeasurementsData newMsr = radioMsr;\n RadioMeasurementsData badMsr;\n\n // Sorting distances in ascending order\n std::sort(newMsr.begin(), newMsr.end(),\n [](const RadioMeasurementData& lhs, const RadioMeasurementData& rhs)\n {\n return lhs.distance < rhs.distance;\n });\n\n // Method for determining the Access Point that fails\n for (const auto& msr1 : newMsr)\n {\n if (std::find_if(badMsr.begin(), badMsr.end(),\n [msr1](const RadioMeasurementData& msr) \n {\n return msr1.id == msr.id;\n }) == badMsr.end())\n {\n Transmitter tx1 = level.transmitter(msr1.id);\n const double x1 = tx1.point.x;\n const double y1 = tx1.point.y;\n const double z1 = tx1.point.z;\n for (const auto& msr2 : newMsr)\n {\n Transmitter tx2 = level.transmitter(msr2.id);\n const double x2 = tx2.point.x;\n const double y2 = tx2.point.y;\n const double z2 = tx2.point.z;\n const double dx = x2 - x1;\n const double dy = y2 - y1;\n const double dz = z2 - z1;\n const double delta = std::sqrt(dx * dx + dy * dy + dz * dz);\n if (delta > (msr1.distance + msr2.distance))\n badMsr.push_back(msr2);\n }\n }\n }\n\n // Update measurement vector with bad Access Points\n for (const auto& msr : badMsr)\n {\n newMsr.erase(std::remove_if(newMsr.begin(), newMsr.end(),\n [msr](const RadioMeasurementData& elem)\n {\n return msr.id == elem.id;\n } ),newMsr.end());\n }\n\n return newMsr;\n}\n\nboost::optional Trilateration::calculateAltitude(const Level& level, const RadioMeasurementsData& radioMeasurements)\n{\n const RadioMeasurementsData levelMsrs = getLevelRadioMeasurements(level, radioMeasurements);\n const RadioMeasurementsData radioMsrs = getIntersectedMeasurements(level, levelMsrs);\n\n if (radioMsrs.size() < 4)\n return boost::none;\n\n // Define the matrix that we are going to use\n size_t count = radioMsrs.size();\n size_t rows = count - 1;\n Eigen::MatrixXd m(rows, 3);\n Eigen::VectorXd b(rows);\n\n // Fill in matrix according to the equations\n size_t row = 0;\n double x1, x2, y1, y2, z1, z2, r1, r2;\n\n Transmitter tx1 = level.transmitter(radioMsrs[0].id);\n x1 = tx1.point.x, y1 = tx1.point.y, z1 = tx1.point.z;\n r1 = radioMsrs[0].distance;\n for (size_t i = 1; i < count; ++i)\n {\n Transmitter tx2 = level.transmitter(radioMsrs[i].id);\n x2 = tx2.point.x, y2 = tx2.point.y, z2 = tx2.point.z;\n r2 = radioMsrs[i].distance;\n m(row, 0) = x2 - x1;\n m(row, 1) = y2 - y1;\n m(row, 2) = z2 - z1;\n b(row) = (pow(r1, 2) - pow(r2, 2) + (pow((x2 - x1), 2) + pow((y2 - y1), 2) + pow((z2 - z1), 2))) / 2;\n row++;\n }\n Eigen::Vector3d rawLocation = m.bdcSvd(Eigen::ComputeThinU|Eigen::ComputeThinV).solve(b);\n double x = rawLocation[0] + x1;\n double y = rawLocation[1] + y1;\n double sum1 = 0.0;\n // double sum2 = 0.0;\n bool nanFlag = true;\n for (const auto& msr : radioMsrs)\n {\n Transmitter tx = level.transmitter(msr.id);\n double deltaZ = std::sqrt(pow(msr.distance, 2) - pow((x - tx.point.x), 2 - pow((y - tx.point.y), 2)));\n if (!std::isnan(deltaZ))\n {\n nanFlag = false;\n sum1 += (tx.point.z + deltaZ) / msr.distance; //if the tracked position is below all beacons, then use the sign '-', else: '+'\n //in theory it should also be calculated: sum2 += 1.0 / msr.distance, but in practice gives worse results\n }\n }\n\n if (nanFlag)\n return boost::none;\n else\n return sum1; //sum1 / sum2;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/trilateration.h", "content": "#pragma once\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nclass Trilateration\n{\n public:\n boost::optional calculateAltitude(const Level& level, const RadioMeasurementsData& radioMeasurements);\n};\n\n} } // namespace navigine::navigation_core\n" }, { "path": "src/vector3d.cpp", "content": "/** vector3d.cpp\n*\n** Copyright (c) 2018 Navigine.\n*\n*/\n\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace {\nstatic constexpr double DOUBLE_EPSILON = 1e-8;\n}\n\nVector3d::Vector3d(double _x, double _y, double _z)\n{\n x = _x;\n y = _y;\n z = _z;\n}\n\ndouble Vector3d::magnitude() const\n{\n return std::sqrt(x*x + y*y + z*z);\n}\n\nVector3d Vector3d::normalized() const\n{\n double length = this->magnitude();\n return (std::fabs(length) > std::numeric_limits::epsilon())\n ? Vector3d(x / length, y / length, z / length)\n : *this;\n}\n\nVector3d& Vector3d::normalize()\n{\n double length = this->magnitude();\n \n if (std::fabs(length) > std::numeric_limits::epsilon())\n *this /= length;\n return *this;\n}\n\nbool Vector3d::isNull() const\n{\n return x == 0 && y == 0 && z == 0;\n}\n\nbool Vector3d::operator==(const Vector3d& v)\n{\n return std::fabs(this->x - v.x) < DOUBLE_EPSILON &&\n std::fabs(this->y - v.y) < DOUBLE_EPSILON &&\n std::fabs(this->z - v.z) < DOUBLE_EPSILON;\n}\n\nbool Vector3d::operator!=(const Vector3d& v)\n{\n return !(*this == v);\n}\n\nVector3d& Vector3d::operator+=(const Vector3d& v)\n{\n this->x += v.x;\n this->y += v.y;\n this->z += v.z;\n return *this;\n}\n\nVector3d& Vector3d::operator-=(const Vector3d& v)\n{\n this->x -= v.x;\n this->y -= v.y;\n this->z -= v.z;\n return *this;\n}\n\nVector3d& Vector3d::operator*=(double multiplier)\n{\n this->x *= multiplier;\n this->y *= multiplier;\n this->z *= multiplier;\n return *this;\n}\n\nVector3d& Vector3d::operator/=(double divisor)\n{\n this->x /= divisor;\n this->y /= divisor;\n this->z /= divisor;\n return *this;\n}\n\ndouble Vector3d::dotProduct(const Vector3d& v1, const Vector3d& v2)\n{\n return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;\n}\n\nVector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.y * v2.z - v1.z * v2.y,\n v1.z * v2.x - v1.x * v2.z, \n v1.x * v2.y - v1.y * v2.x);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/README.md", "content": "\n\n# Indoor-navigation-algorithms\n\nThis repository includes a set of different algorithms combined in Navigine core positioning library. The application scope of Navigine positioning library includes (and is not limited to) cases of indoor navigation in airports, shopping malls, universities, and so on.\n\n\n## Useful Links\n\n- Refer to the [Navigine official website](https://navigine.com/) for complete list of downloads, useful materials, information about the company, and so on.\n- [Get started](http://client.navigine.com/login) with Navigine to get full access to Navigation services, SDKs, and applications.\n- Refer to the Navigine [User Manual](http://docs.navigine.com/) for complete product usage guidelines.\n- Find company contact information at the official website under [Contacts](https://navigine.com/contacts/) tab.\n\n## Project roadmap for 2021 Q1\n\n### January-February 2021\n\nAdd positioning block utilizing simple positioning algorithm and all the processing blocks:\n- Add C++ Code to repository\n- Add set of tests for position estimation\n- Add comprehensive documentation and references to related scientific articles.\n\n### March 2021\n\nAdd optional positioning algorithms to be used in core positioning block\n- Add C++ Code\n- Add set of tests for position estimation\n\n\n### April 2021\n\nAdd first version on indoor navigation algorithms\n- Add C++ basic code\n- Add set of tests for algorithms\n\n### May 2021\n\n- Upfload first version of navgine navigation library\n" }, { "path": "standalone_algorithms/System description.md", "content": "The performance of indoor positioning system depends on correct description of building structure that is used for navigation and\ncorrect installation of infrastructure such as various signals transmitters, beacons and base stations.\n\nTo describe the indoor environment we propose the API of indoor positioning algorithms described below.\n\n## Navigation structures\n\nAny indoor space, such as building or warehouse is further referred to as `Location` and is represented by a set of 2D (plane) areas called `Level`'s. A typical example of `Location` is a multistored building, where each `Level`s corresponds to one of the floors of the building.\n\n![Alt text](illustrations/Location.jpg?raw=true \"Location\")\n[Figure 1. Location with levels]\n\nEach `Level` is specified by all the data, necessary to calculate position on this `Level`. This data includes: geometry of the `Level` (walls, open/closed areas, bounding box), route graph (information about available routes on this `Level`), list of installed devices, called `Transmitters` (bluetooth beacons, wifi base stations, etc) and collected radio map (if present) that is called RSSI fingerprints [1, 2]. All these pieces of data will be described below.\n\n![Alt text](illustrations/Level.jpg?raw=true \"Level\")\n[Figure 2. The scheme of the level]\n\nAll these infrastructure is described in the following struct:\n\n```cpp\nstruct Level\n{\n Id id;\n ...\n std::vector transmitters;\n std::map referencePoints;\n Geometry geometry;\n Graph graph;\n};\n```\n\n### Walls, open areas and routes\n\nThe physical structure of the `Level` is described by struct `Geometry` which includes boundind box `Box` of the `Level` area and the set of polygons of free space joint together in `Multipolygon` class. Bounding box is basically a rectangle in physical space defining the boundaries of the area, selected for positioning. Allowed Area (`Multipolygon`) is necessary to introduce a natural restriction on possible movements inside the area of this `Level`. These restrictions maybe introduced by closed rooms, walls, holes, and etc. Each of such obstacle is presented by a `Polygon` in physical space. And altogether these obstacle are joined in a `Multipolygon` structure. During positioning, only the area, that is not covered by this `Multipolygon` is available for positioning.\n\n```cpp\n struct Geometry {\n Multipolygon allowedArea;\n Box boundingBox;\n };\n```\n\n`Multipolygon` here is just an alias for boost multi_polygon: `using Multipolygon = boost::geometry::model::multi_polygon;`\n\nMost of positioning engines must provide the functionality of building Routes (between points A and B, or from current position to some destination point B) on top of positioning. To make it possible, `Level` should contain routing information. This information is presented by a set of physical points - `Vertices`, and a set of links (`Edged`) between them. A route can be built from point `A` to point `B` only if these points are connencted by a multi-link of `Edges`. Altogether, `Vertices` and `Edges` form a mathmetical structure called `Graph` of routes.\n\n```cpp\nstruct Graph\n{\n struct Vertex\n {\n double x;\n double y;\n };\n\n std::vector vertices;\n std::set> edges;\n};\n```\n\n### Transmitters\n\nThe indoor positioning system can use different technologies to calculate position of a mobile object. One of the most popular technology is based on measurements of received signal strength from base stations or beacons with known positions to a mobile tag. The signal strength can then be transformed to distance to an object. The set of such distances with corresponding coordinates of base stations is used to perform thrilateration and to calculate the position. The struct `Transmitter` is used to describe the type, the position and other properties of a base station or a beacon.\n\nThe essential parameters of each transmitter are: its position on Level (`x`,`y`) and its unique identificator `id`. `Position` of device is essential as all solvers are based on measurements of distance (some of device can measure distance explicitly, while other provide some other types of signal, that will be converted to distance using existing physical formulas) to the transmitter. Thus to get correct position estimate, one has to correctly specify the positions of all transmitters. `ID` of device is essential as it is used to match the signal, received on the device side, to the specific transmitter from which this signal was emitted.\n\nOther parameters include the type `Type` of the device and parameters, specific to different types of devices.\n\n```cpp\nstruct Transmitter \n{\n enum class Type { WIFI, BEACON, BLUETOOTH, RTT, UNKNOWN };\n\n Id id;\n double x;\n double y;\n Type type;\n ... //other parameters describing transmition power, etc ...\n};\n\n```\n \n\n### RSS Fingerprints\nAnother way of calculation of position of a mobile device is to use radiomap of received signal strength of available stationary devices such as wifi hotspots and ble devices. This approach is used even when position of base stations are not known. The idea is based on comparison of the set of wifi or ble signals registered by mobile device at current time moment with statistics of corresponding signals registered on different positions inside the building captured during preliminary experiments.\nThe set of all available signals `SignalStatistics` captured during preliminary experiments in given position from different transmitters is stored in `std::map fingerprints`. These information together with coordinates of the points, at which the signals were registered, is joint into `ReferencePoint` struct. To describe radiomap on certain `Level` the map of `ReferencePoint` with corresponding ids is used\n```cpp\nstruct SignalStatistics\n{\n Transmitter::Type type;\n double mean;\n double variance;\n int nMeasurements;\n ...\n};\n\nstruct ReferencePoint\n{\n Id id;\n double x;\n double y;\n double magnIntensity;\n std::map fingerprints;\n ...\n};\n\n```\n \n\n### References\n1. Panyov, A., Golovan, A. A., Smirnov, A. S., Kosyanchuk, V. V. Indoor Positioning Using Wi-Fi Fingerprinting, Magnetometer and Redestrian Dead Reckoning. 21st Saint Petersburg International Conference on Integrated Navigation Systems. Saint-Petersburg 2014\n2. Panyov, A., Golovan, A. A., Smirnov, A. S., Kosyanchuk, V. V. Efficient Localization Using Different Mean Offset Models In Gaussian Processes. Proceeding of the 5th IEEE International Conference on Indoor Positioning Indoor Navigation 2014\n" }, { "path": "standalone_algorithms/complementary_filter/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.7)\nproject(Navigation-core)\n\noption(BUILD_TESTS \"Build tests\" ON)\noption(BUILD_EXAMPLES \"Build examples\" OFF)\n\nset(BOOST_ENABLE_CMAKE ON)\nset(Boost_USE_STATIC_LIBS OFF)\n\nfind_package(Boost COMPONENTS system unit_test_framework REQUIRED)\nIF (Boost_FOUND)\n INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})\n message(${Boost_INCLUDE_DIR})\n include_directories(${Boost_SOURCE_DIR})\nENDIF ()\n\nset(\"target\" \"complementary-filter\")\nset(\"test-target\" \"test-filter\")\n\nif(BUILD_TESTS)\n file(GLOB ${PROJECT_NAME}_TEST_SOURCES src/*.cpp tests/*.cpp)\n add_executable(\"${test-target}\" ${${PROJECT_NAME}_TEST_SOURCES})\n target_include_directories(\"${test-target}\" PRIVATE \"include/\")\n set_property(TARGET \"${test-target}\" PROPERTY CXX_STANDARD 17)\n set_target_properties(\"${test-target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\n set_target_properties(\"${test-target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions \")\n target_link_libraries(\"${test-target}\"\n #\"-lboost_unit_test_framework\"\n # Boost::unit_test_framework\n ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}\n )\nendif()\n\nif(BUILD_EXAMPLES)\n list(APPEND \"${target}__cxx_srcs\"\n \"examples/main.cpp\"\n \"src/complementary_filter.cpp\"\n \"src/quaternion.cpp\"\n \"src/vector3d.cpp\")\n add_executable(\"${target}\" ${${target}__cxx_srcs})\n target_include_directories(\"${target}\" PRIVATE \"include/\")\n set_property(TARGET \"${target}\" PROPERTY CXX_STANDARD 17)\n set_target_properties(\"${target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\n set_target_properties(\"${target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions \")\nendif()\n" }, { "path": "standalone_algorithms/complementary_filter/README.md", "content": "### Complementary filter\n\nComplementary filter fuses data from IMU (Intertial Measurement Units) sensors such as accelerometer, magnetometer, gyroscope and pedometer. Configuration of sensors that are in use during the position estimation process may vary depending on the quality of signal from sensors. For instance, it is common to work with accelerometer and gyroscope, rejecting other sensors if signal from corresponding sensors is noisy.\n\nThe example directory contains code that simulates IMU measuremetns of pendulum and estimates pendulum orientation with the complementary filter. It is possible to plot the result of orientation estimation with python script located at helpers directory.\n\n### What is a complementary filter\n\nThe basic complementary filter is shown in a picture below.\n\n\"Markdown\n\nWhere z is an input signal, x and y are noisy measurements of this signal. an estimation of the output signal produced buy the filter. Assume that\nthe noise in y is mostly high frequency, and the noise in x is mostly low frequency. Then G(s) can be made a low-pass filter to filter out the high-frequency noise in y. If G(s) is low-pass, [1 - G(s)] is the complement, i.e., a high-pass filter which filters out the low-frequency noise in x. The complementary filter can be reconfigured as in figure B. In this case the input to G(s) is\n\n

\n \n so that the filter G(s) just operates on the noise or error in the measurements x and y. In the case of noise less or error-free measurements\n\n

\n\n\ni.e., the signal is estimated perfectly.\n\nGyroscope obtains the angular position of the object by integrating the angular velocity over time. Gyroscope is a very precise sensor and not susceptible to external forces. But because of integration, the measurement tends to drift, not returning to zero when the system goes back to its original position. Accelerometer is a sensor which takes into consideration every small forse therefore it can be easily disturbed. That is why the accelerometer data is reliable only on a long term. The advantage of accelerometer is that it does not drift. A magnetometer is a device that measures magnetic field or magnetic dipole moment.\n\nIn case if the measurements from different sources give precise information in different frequency regions, it is convenient to use a weighted sum of measurements from these sources.\n\n[//]: # \"angle = alpha * (angle + gyrData * dt) + beta * accData + gamma * magnData)\"\n\n

\"angle

\n\nwhere alpha, beta, gamma are constant weights of different measurement sources.\n\n### Low and high pass filters\n\nLow-pass filter is a filter that passes signals with a frequency lower than a selected cutoff frequency.\nTransfer function of low-pass filter\n\n[//]: # \"https://en.wikipedia.org/wiki/Low-pass_filter\"\n\n

\n\nwhere is a cutoff frequency of a filter\n\nLow-pass filter is the complement of a high-pass filter. High-pass filter is a filter that passes signals with a frequency higher than a certain cutoff frequency.\nTransffer function of high-pass filter\n\n

\n\nConsider a first-order integrator\n\n

\n\nwith the folowing measurement characteristics\n\n

\n\nMeasurements can be fused into an estimate of a state via the filter\n\n

\n\nA filter is called a complementary filter if \n

\n\n### Comparison with Kalman filter\n\nComplementary filter outperforms Kalman filter significantly by using less computational and processing power and providing more accuracy. The Complementary filter can be applied by having only vector and quaternion mathematical operators. On other hand, the traditional Kalman filter needs an enormous number of matrix operations, including multiplications and taking inverses of these matrices, which, besides the complexity, also results in high computational and processing costs.\n\n### Calculations\n\nAcceleration is stored in quaternion form. The weight part of it is a numeric value of the acceleration, the vector part -- vector components.\n\nThe goal of developing an estimator is to provide a smooth estimate of a state R(t) that is evolving due\nto some external input based on a set of measurements.\n\nFrame of reference allows to determine the error. It can be calculated as follows\n\n

\n\n

\n\nThe direct complementary filter dynamics are specified by\n\n

\n\nThe R* operation is an inverse operation on SO(3)\n\nThe kinematics can be written directly in terms of the quaternion representation of SO(3) by\n\n

\n\nQuaternion is multiplied by the pure rotation quaternion , where real part is equal to zero.\nIn the source code provided 'updateQuaternion(mQ, rotation)' function which does the same calculations.\n\nThe gyroscope data is integrated every timestep with the current angle value:\n\n```cpp\nomega = gyro * dt + mIntegralError * dt;\n```\n\n### Components\n\nThe following constant coefficients are the weights applied to accelerometer, gyroscope and magnetometer components.\n\n```cpp\ndouble mKaccelerometer;\ndouble mKmagnetometer;\ndouble mKintegralGain;\n```\n\n### Build\n\nTo build the project for running tests make sure that `BUILD_TESTS` option is turned on.\n\n```sh\ncd /standalone_algorithms/complementary_filter\ncmake -Bbuild -H.\ncmake --build build\n```\n\nRun tests:\n\n```sh\n./build/test-filter\n```\n\n### Results of orientation\n\n\"Markdown\n" }, { "path": "standalone_algorithms/complementary_filter/examples/main.cpp", "content": "#include \n#include \n#include \n#include \n#include \n\nusing namespace navigine::navigation_core;\n\nstatic const double MAGNETIC_FLUX_DENSITY = 0.48;\nstatic const double MAGNETIC_FIELD_INCLINATION = 60.48 * M_PI / 180;\nstatic const double G = 9.81;\nconst std::string TEST_DATA_FOLDER = \"test_data/\";\n\n/**\n * duration - duration of pendulum motion in seconds\n * freq - sampling frequency in Hz\n * L - pendulum length\n * theta0 - initial angle in radians\n *\n * Returns:\n * the angle, angular velocity and angular acceleration for pendulum motion\n */\nstd::tuple,\n std::vector,\n std::vector> simulatePendulumMotion(double durationStaticSec,\n double durationDynamicSec,\n double freqHz = 100,\n double L = 1.0,\n double theta0 = M_PI / 60.0)\n{\n std::vector times;\n std::vector thetas;\n std::vector thetaVels;\n\n double t = 0;\n double tick = 1.0 / freqHz;\n //first 10 seconds keep static pendulum tilt\n for (int i = 0; i < durationStaticSec / tick; i++) {\n times.emplace_back(t);\n thetas.emplace_back(theta0);\n thetaVels.emplace_back(0);\n t += tick;\n }\n\n double thetaPrev = theta0;\n for (int i = 0; i < durationDynamicSec / tick; i++)\n {\n double theta = theta0 * cos(sqrt(G / L) * t);\n double thetaVel = (theta - thetaPrev) / tick;\n\n times.emplace_back(t);\n thetas.emplace_back(theta);\n thetaVels.emplace_back(thetaVel);\n\n t += tick;\n thetaPrev = theta;\n }\n\n return std::make_tuple(times, thetas, thetaVels);\n}\n\nstd::tuple,\n std::vector,\n std::vector> generateImuPendulum(std::vector t,\n std::vector theta,\n std::vector thetaVel,\n double freqHz = 100,\n double L = 1)\n{\n long long refTs = 1583315232492;\n std::random_device rd;\n std::mt19937 gen(rd());\n\n std::vector accelMeasurements;\n for (std::size_t i = 0; i < t.size(); i++)\n {\n //the variance is choosen to be equal to typical accelerometer noise variance of cheap IMU sensor\n double var = 2 * M_PI * freqHz * G * pow(0.001, 2);\n std::normal_distribution distr(0.0, var);\n\n double ax = -G * sin(theta[i]) + distr(gen);\n double ay = 0.0 + distr(gen);\n double az = -L * pow(thetaVel.at(i), 2) + G * cos(theta[i]) + distr(gen);\n SensorMeasurement msr;\n msr.ts = refTs + 1000 * t.at(i);\n msr.values = Vector3d(ax, ay, az);\n msr.type = SensorMeasurement::Type::ACCELEROMETER;\n accelMeasurements.emplace_back(msr);\n }\n\n std::vector gyroMeasurements;\n for (std::size_t i = 0; i < t.size(); i++)\n {\n //the variance is choosen to be equal to typical gyroscope noise variance of cheap IMU sensor\n double var = 2 * M_PI * freqHz * pow(0.01 * M_PI / 180.0, 2);\n std::normal_distribution distr(0.005, var);\n\n double wx = 0.0 + distr(gen);\n double wy = thetaVel.at(i) + distr(gen);\n double wz = 0.0 + distr(gen);\n SensorMeasurement msr;\n msr.ts = refTs + 1000 * t.at(i);\n msr.values = Vector3d(wx, wy, wz);\n msr.type = SensorMeasurement::Type::GYROSCOPE;\n gyroMeasurements.emplace_back(msr);\n }\n\n std::vector magnMeasurements;\n for (std::size_t i = 0; i < t.size(); i++)\n {\n double mx = MAGNETIC_FLUX_DENSITY * cos(MAGNETIC_FIELD_INCLINATION + theta[i]);\n double my = 0.0;\n double mz = MAGNETIC_FLUX_DENSITY * sin(MAGNETIC_FIELD_INCLINATION + theta[i]);\n SensorMeasurement msr;\n msr.ts = refTs + 1000 * t.at(i);\n msr.values = Vector3d(mx, my, mz);\n msr.type = SensorMeasurement::Type::MAGNETOMETER;\n magnMeasurements.emplace_back(msr);\n }\n\n return std::make_tuple(accelMeasurements, gyroMeasurements, magnMeasurements);\n}\n\nint main()\n{\n std::string testDataFolder = TEST_DATA_FOLDER;\n std::vector times;\n std::vector thetas;\n std::vector thetaVels;\n\n std::tie(times, thetas, thetaVels) = simulatePendulumMotion(10, 10);\n\n std::vector accelMeasurements;\n std::vector gyroMeasurements;\n std::vector magnMeasurements;\n\n std::tie(accelMeasurements, gyroMeasurements, magnMeasurements) =\n generateImuPendulum(times, thetas, thetaVels);\n\n std::ofstream os;\n std::string simulatedMsrFileName = testDataFolder + \"simulation.log\";\n os.open(simulatedMsrFileName);\n for (std::size_t i = 0; i < times.size(); i++)\n {\n SensorMeasurement accelMsr = accelMeasurements.at(i);\n SensorMeasurement gyroMsr = gyroMeasurements.at(i);\n SensorMeasurement magnMsr = magnMeasurements.at(i);\n double theta = thetas.at(i);\n os << accelMsr.ts << \" \" << theta << \" \"\n << accelMsr.values.x << \" \" << accelMsr.values.y << \" \" << accelMsr.values.z << \" \"\n << gyroMsr.values.x << \" \" << gyroMsr.values.y << \" \" << gyroMsr.values.z << \" \"\n << magnMsr.values.x << \" \" << magnMsr.values.y << \" \" << magnMsr.values.z << std::endl;\n }\n os.close();\n std::cout << \"simulated data was written to \" << simulatedMsrFileName << std::endl;\n\n ComplementaryFilter complementaryFilter;\n\n std::string calculatedAngleFileName = testDataFolder + \"calculated_angles.log\";\n os.open(calculatedAngleFileName);\n for (std::size_t i = 0; i < accelMeasurements.size(); i++)\n {\n const SensorMeasurement& accelMsr = accelMeasurements.at(i);\n const SensorMeasurement& gyroMsr = gyroMeasurements.at(i);\n const SensorMeasurement& magnMsr = magnMeasurements.at(i);\n complementaryFilter.update(accelMsr);\n complementaryFilter.update(gyroMsr);\n complementaryFilter.update(magnMsr);\n Orientation ori = complementaryFilter.getFusedOrientation();\n os << accelMsr.ts << \" \" << ori.roll << \" \" << ori.pitch << \" \" << ori.yaw << std::endl;\n }\n os.close();\n std::cout << \"calculated angles were written to \" << calculatedAngleFileName << std::endl;\n\n return 0;\n}\n" }, { "path": "standalone_algorithms/complementary_filter/helpers/plot_angles.py", "content": "import pandas as pd\nimport matplotlib.pylab as plt\nimport numpy as np\n\ndf1 = pd.read_csv('./../test_data/simulation.log', sep=' ', index_col=None, names=['ts', 'angle', 'ax', 'ay', 'az', 'wx', 'wy', 'wz', 'mx', 'my', 'mz'])\n\ndf2 = pd.read_csv('./../test_data/calculated_angles.log', sep=' ', names=['ts', 'roll', 'pitch', 'yaw'])\nt = (df1['ts'] - df1['ts'][0]) / 1000.0\n\nplt.plot(t, 180 * df1.angle / 3.14, '-', label='simulated')\nplt.plot(t, 180 * df2.roll / 3.14, '-', label='roll')\nplt.plot(t, 180 * df2.pitch / 3.14, '-', label='pitch')\nplt.plot(t, 180 * df2.yaw / 3.14, '-', label='yaw')\nplt.legend()\nplt.show()\n\n" }, { "path": "standalone_algorithms/complementary_filter/include/complementary_filter.h", "content": "#pragma once\n\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct SensorMeasurement\n{\n enum class Type { ACCELEROMETER, MAGNETOMETER, GYROSCOPE, BAROMETER, LOCATION, ORIENTATION};\n\n Type type;\n long long ts = -1;\n Vector3d values = Vector3d();\n};\n\ndouble to_minus_Pi_Pi(double x);\n\nstruct Orientation\n{\n double roll = 0.0;\n double pitch = 0.0;\n double yaw = 0.0;\n long long ts = 0;\n\n Orientation(double roll_, double pitch_, double yaw_, long long ts_) \n : roll {roll_ }\n , pitch {pitch_}\n , yaw {yaw_ }\n , ts {ts_ }\n {}\n};\n\nclass ComplementaryFilter\n{\n public:\n ComplementaryFilter () {}\n ComplementaryFilter (double Ka, double Km, double Ki)\n : mKaccelerometer {Ka}\n , mKmagnetometer {Km}\n , mKintegralGain {Ki}\n {}\n \n double getMagneticAzimuth() const;\n double getFusedAzimuth() const;\n double getDeviceAzimuth() const;\n Orientation getFusedOrientation() const;\n void update(const SensorMeasurement& msr);\n \n private:\n void updateUsingGyroscope (const SensorMeasurement& gyro);\n void updateUsingAccelerometer (const SensorMeasurement& accel);\n void updateUsingMagnetometer (const SensorMeasurement& magn);\n double caclulateMagneticAzimuth (const SensorMeasurement& magn);\n\n long long mCurrentTs = -1;\n long long mLastGyroTs = -1;\n double mKaccelerometer = 0.1;\n double mKmagnetometer = 0.05;\n double mKintegralGain = 0.0;\n Vector3d mIntegralError = {};\n Quaternion mQ = {};\n double mMagneticAzimuth = 0.0;\n double mDeviceAzimuth = 0.0;\n SensorMeasurement mMagnLowPassed; // Magnetometer measurements passed through low-pass filter\n SensorMeasurement mAccelLowPassed; // Accelerometer measurements passed through low-pass filter\n};\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/complementary_filter/include/quaternion.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nclass Quaternion\n{\n public:\n double w = 1.0;\n double x = 0.0;\n double y = 0.0;\n double z = 0.0;\n \n public:\n Quaternion(){}\n Quaternion(double _w, double x, double _y, double _z);\n Quaternion(double _x, double _y, double _z);\n Quaternion(const Vector3d& vec);\n\n Quaternion normalized() const;\n Quaternion conj() const { return Quaternion(w, -x, -y, -z); }\n Vector3d toVector3d() const { return Vector3d(x, y, z); }\n\n std::tuple toEuler() const;\n \n Quaternion& operator+=(const Quaternion& r_q);\n Quaternion& operator-=(const Quaternion& r_q);\n Quaternion& operator*=(const double& scale);\n Quaternion& operator*=(const Quaternion& r_q);\n};\n\nQuaternion operator+(Quaternion l_q, const Quaternion& r_q);\nQuaternion operator-(Quaternion l_q, const Quaternion& r_q);\nQuaternion operator*(Quaternion l_q, const double& scale);\nQuaternion operator*(const double& scale, Quaternion l_q);\nQuaternion operator*(Quaternion q1, const Quaternion& q2);\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/complementary_filter/include/vector3d.h", "content": "#pragma once\n\n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Vector3d \n{\n double x = 0.0;\n double y = 0.0;\n double z = 0.0;\n\n Vector3d() = default;\n Vector3d(double _x, double _y, double _z);\n\n double magnitude() const;\n Vector3d normalized() const;\n Vector3d& normalize();\n bool isNull() const;\n\n\n static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2);\n static double dotProduct (const Vector3d& v1, const Vector3d& v2);\n\n Vector3d& operator+=(const Vector3d& v2);\n Vector3d& operator-=(const Vector3d& v2);\n Vector3d& operator*=(double multiplier);\n Vector3d& operator/=(double divisor);\n};\n\n\ninline bool operator==(const Vector3d& v1, const Vector3d& v2)\n{\n return std::fabs(v1.x - v2.x) < std::numeric_limits::epsilon() &&\n std::fabs(v1.y - v2.y) < std::numeric_limits::epsilon() &&\n std::fabs(v1.z - v2.z) < std::numeric_limits::epsilon();\n}\n\ninline bool operator!=(const Vector3d& v1, const Vector3d& v2)\n{\n return !(v1 == v2);\n}\n\ninline Vector3d operator*(double multiplier, const Vector3d& v)\n{\n return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);\n}\n\ninline Vector3d operator*(const Vector3d& v, double multiplier)\n{\n return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);\n}\n\ninline Vector3d operator+(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);\n}\n\ninline Vector3d operator-(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);\n}\n\ninline Vector3d operator-(const Vector3d& v)\n{\n return Vector3d(-v.x, -v.y, -v.z);\n}\n\ninline Vector3d operator/(const Vector3d& v1, double divisor)\n{\n return Vector3d(v1.x / divisor, v1.y / divisor, v1.z / divisor);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/complementary_filter/src/complementary_filter.cpp", "content": "#include \"complementary_filter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double MIN_SAMPLING_PERIOD_SEC = 0.02;\nstatic const double LPF_TIME_CONSTANT_SEC = 1.0;\nstatic const double DOUBLE_EPSILON = 1e-8;\n\ndouble to_minus_Pi_Pi(double x)\n{\n while ( x >= M_PI ) x -= 2 * M_PI;\n while ( x < -M_PI ) x += 2 * M_PI;\n return x;\n}\n\nQuaternion updateQuaternion(const Quaternion& q, const Vector3d& v)\n{\n Quaternion qDot = q * Quaternion(v) * 0.5;\n Quaternion qOut = q + qDot;\n return qOut.normalized();\n}\n \nSensorMeasurement lowPassFilter(const SensorMeasurement& prevMsr, const SensorMeasurement& currMsr)\n{\n double dt = (currMsr.ts - prevMsr.ts) / 1000.0;\n double alpha = LPF_TIME_CONSTANT_SEC / (LPF_TIME_CONSTANT_SEC + std::max(dt, MIN_SAMPLING_PERIOD_SEC));\n\n SensorMeasurement res = currMsr;\n res.values = alpha * prevMsr.values + (1 - alpha) * currMsr.values;\n\n return res;\n}\n\n/** This function returns calculated orientation (in radians) and current timestamp.\n * The orientation is right-handed, therefore counter-clockwise rotation has positive sign\n */\nOrientation ComplementaryFilter::getFusedOrientation() const\n{\n double roll, pitch, yaw;\n std::tie(roll, pitch, yaw) = mQ.toEuler();\n return Orientation{roll, pitch, yaw, mCurrentTs};\n}\n\n/** This function returns calculated fused azimuth (in radians), clockwise has positive sign */\ndouble ComplementaryFilter::getFusedAzimuth() const\n{\n double roll, pitch, yaw;\n std::tie(roll, pitch, yaw) = mQ.toEuler();\n return -yaw;\n}\n\n/** This function returns calculated magnetic azimuth (in radians), clockwise has positive sign */\ndouble ComplementaryFilter::getMagneticAzimuth() const\n{\n return mMagneticAzimuth;\n}\n\n/** This function returns azimuth calculated on device (in radians), clockwise has positive sign */\ndouble ComplementaryFilter::getDeviceAzimuth() const\n{\n return mDeviceAzimuth;\n}\n\n/** measurements time must be in ascending order! */\nvoid ComplementaryFilter::update(const SensorMeasurement& msr)\n{\n \n if(msr.type == SensorMeasurement::Type::ACCELEROMETER)\n {\n mAccelLowPassed = lowPassFilter(mAccelLowPassed, msr);\n updateUsingAccelerometer(msr);\n }\n else if (msr.type == SensorMeasurement::Type::MAGNETOMETER)\n {\n mMagnLowPassed = lowPassFilter(mMagnLowPassed, msr);\n if (mLastGyroTs == -1)\n updateUsingMagnetometer(msr);\n\n mMagneticAzimuth = caclulateMagneticAzimuth(msr);\n }\n else if (msr.type == SensorMeasurement::Type::GYROSCOPE)\n {\n if (mLastGyroTs == -1)\n mLastGyroTs = msr.ts;\n\n updateUsingGyroscope(msr);\n }\n else if (msr.type == SensorMeasurement::Type::ORIENTATION)\n {\n mDeviceAzimuth = msr.values.x;\n }\n\n mCurrentTs = msr.ts;\n \n return;\n}\n\nvoid ComplementaryFilter::updateUsingGyroscope(const SensorMeasurement& gyroMeas)\n{\n double dt = static_cast(gyroMeas.ts - mLastGyroTs) / 1000.0;\n mLastGyroTs = gyroMeas.ts;\n Vector3d gyro = gyroMeas.values;\n Vector3d omega = gyro * dt + mIntegralError * dt;\n mQ = updateQuaternion(mQ, omega);\n} \n\nvoid ComplementaryFilter::updateUsingAccelerometer(const SensorMeasurement& accelMeas)\n{\n Vector3d acc = accelMeas.values.normalized();\n Vector3d gravSensorFrame = (mQ.conj() * Quaternion(0.0, 0.0, 0.0, 1.0) * mQ).toVector3d();\n Vector3d error = Vector3d::crossProduct(acc, gravSensorFrame);\n Vector3d rotation = mKaccelerometer * error;\n\n mQ = updateQuaternion(mQ, rotation);\n mIntegralError += (mKintegralGain > DOUBLE_EPSILON) ? (mKintegralGain * error) : Vector3d(0.0, 0.0, 0.0);\n}\n\nvoid ComplementaryFilter::updateUsingMagnetometer(const SensorMeasurement& magnMeas)\n{\n Vector3d magn = magnMeas.values.normalized();\n if (magn.magnitude() < DOUBLE_EPSILON)\n return;\n \n Vector3d magnGlobalFrame = (mQ * Quaternion(magn) * mQ.conj()).toVector3d();\n magnGlobalFrame.y = std::sqrt(magnGlobalFrame.y * magnGlobalFrame.y + magnGlobalFrame.x * magnGlobalFrame.x);\n magnGlobalFrame.x = 0.0;\n Vector3d magnSensorFrame = (mQ.conj() * Quaternion(magnGlobalFrame) * mQ).toVector3d();\n\n Vector3d error = Vector3d::crossProduct(magn, magnSensorFrame);\n Vector3d rotation = mKmagnetometer * error;\n\n mQ = updateQuaternion(mQ, rotation);\n mIntegralError += (mKintegralGain > DOUBLE_EPSILON) ? (mKintegralGain * error) : Vector3d(0.0, 0.0, 0.0);\n}\n\ndouble ComplementaryFilter::caclulateMagneticAzimuth(const SensorMeasurement& magn)\n{\n double roll, pitch, yaw;\n std::tie(roll, pitch, yaw) = mQ.toEuler();\n double halfYaw = yaw / 2.0;\n\n Quaternion quatDifference = Quaternion(sin(halfYaw), 0, 0, cos(halfYaw)).normalized();\n Quaternion quatZeroYaw = quatDifference * mQ;\n\n Vector3d magnetometer = magn.values.normalized();\n Vector3d magnGlobalFrame = (quatZeroYaw * Quaternion(magnetometer) * quatZeroYaw.conj()).toVector3d();\n\n return to_minus_Pi_Pi(M_PI / 2.0 + atan2(magnGlobalFrame.y, magnGlobalFrame.x));\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/complementary_filter/src/quaternion.cpp", "content": "#include \"quaternion.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nQuaternion::Quaternion(double _w, double _x, double _y, double _z)\n : w{_w}, x{_x}, y{_y}, z{_z}\n{ }\n\nQuaternion::Quaternion(double _x, double _y, double _z)\n : w{0}, x{_x}, y{_y}, z{_z}\n{ }\n\nQuaternion::Quaternion(const Vector3d& vec)\n : w{0.0}, x{vec.x}, y{vec.y}, z{vec.z}\n{ }\n\nQuaternion Quaternion::normalized() const\n{\n double norm = std::sqrt(w * w + x * x + y * y + z * z);\n\n return std::fabs(norm) > std::numeric_limits::epsilon()\n ? Quaternion(w / norm, x / norm, y / norm, z / norm)\n : *this;\n}\n\nstd::tuple Quaternion::toEuler() const\n{\n double sinr = 2.0 * (w * x + y * z);\n double cosr = 1.0 - 2.0 * (x * x + y * y);\n auto roll = std::atan2(sinr, cosr);\n\n double sinp = 2.0 * (w * y - z * x);\n double pitch = 0.0;\n if (std::fabs(sinp) >= 1)\n pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range\n else\n pitch = asin(sinp);\n\n double siny = 2.0 * (w * z + x * y);\n double cosy = 1.0 - 2.0 * (y * y + z * z);\n double yaw = std::atan2(siny, cosy);\n \n return std::make_tuple(roll, pitch, yaw);\n}\n\nQuaternion& Quaternion::operator+=(const Quaternion& r_q)\n{\n this->w += r_q.w;\n this->x += r_q.x;\n this->y += r_q.y;\n this->z += r_q.z;\n return *this;\n}\n\nQuaternion& Quaternion::operator-=(const Quaternion& r_q)\n{\n this->w -= r_q.w;\n this->x -= r_q.x;\n this->y -= r_q.y;\n this->z -= r_q.z;\n return *this;\n}\n\nQuaternion& Quaternion::operator*=(const double& scale)\n{\n this->w *= scale;\n this->x *= scale;\n this->y *= scale;\n this->z *= scale;\n return *this;\n}\n\nQuaternion& Quaternion::operator*=(const Quaternion& other)\n{\n double qw = w, qx = x, qy = y, qz = z;\n this->w = qw*other.w - qx*other.x - qy*other.y - qz*other.z;\n this->x = qw*other.x + qx*other.w + qy*other.z - qz*other.y;\n this->y = qw*other.y - qx*other.z + qy*other.w + qz*other.x;\n this->z = qw*other.z + qx*other.y - qy*other.x + qz*other.w;\n return *this;\n}\n\nQuaternion operator+(Quaternion l_q, const Quaternion& r_q)\n{\n return l_q += r_q;\n}\n\nQuaternion operator-(Quaternion l_q, const Quaternion& r_q)\n{\n return l_q -= r_q;\n}\n\nQuaternion operator*(Quaternion l_q, const double& scale)\n{\n return l_q *= scale;\n}\n\nQuaternion operator*(const double& scale, Quaternion l_q)\n{\n return l_q *= scale;\n}\n\nQuaternion operator*(Quaternion l_q, const Quaternion& r_q)\n{\n return l_q *= r_q;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/complementary_filter/src/vector3d.cpp", "content": "#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nVector3d::Vector3d(double _x, double _y, double _z)\n{\n x = _x;\n y = _y;\n z = _z;\n}\n\ndouble Vector3d::magnitude() const\n{\n return std::sqrt(x*x + y*y + z*z);\n}\n\nVector3d Vector3d::normalized() const\n{\n double length = this->magnitude();\n \n return (std::fabs(length) > std::numeric_limits::epsilon()) ? Vector3d(x / length, y / length, z / length) : *this;\n}\n\nVector3d& Vector3d::normalize()\n{\n double length = this->magnitude();\n \n if (std::fabs(length) > std::numeric_limits::epsilon())\n *this /= length;\n return *this;\n}\n\nbool Vector3d::isNull() const\n{\n return x == 0 && y == 0 && z == 0;\n}\n\nVector3d& Vector3d::operator+=(const Vector3d& v)\n{\n this->x += v.x;\n this->y += v.y;\n this->z += v.z;\n return *this;\n}\n\nVector3d& Vector3d::operator-=(const Vector3d& v)\n{\n this->x -= v.x;\n this->y -= v.y;\n this->z -= v.z;\n return *this;\n}\n\nVector3d& Vector3d::operator*=(double multiplier)\n{\n this->x *= multiplier;\n this->y *= multiplier;\n this->z *= multiplier;\n return *this;\n}\n\nVector3d& Vector3d::operator/=(double divisor)\n{\n this->x /= divisor;\n this->y /= divisor;\n this->z /= divisor;\n return *this;\n}\n\ndouble Vector3d::dotProduct(const Vector3d& v1, const Vector3d& v2)\n{\n return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;\n}\n\nVector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.y * v2.z - v1.z * v2.y,\n v1.z * v2.x - v1.x * v2.z, \n v1.x * v2.y - v1.y * v2.x);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/complementary_filter/tests/complementary_filter_test.cpp", "content": "#include \n#include \n#include \n#include \n#include \n\n#define BOOST_TEST_DYN_LINK\n#define BOOST_TEST_MODULE ComplementaryFilter\n#include \n\nusing namespace navigine::navigation_core;\n\nstatic const double MAGNETIC_FLUX_DENSITY = 0.48;\nstatic const double MAGNETIC_FIELD_INCLINATION = 60.48 * M_PI / 180;\nstatic const double G = 9.81;\n\nstd::tuple,\n std::vector,\n std::vector> getTiltedImuMeasurements(double durationStaticSec,\n double tiltPitch = 0.0,\n double freqHz = 100)\n{\n std::random_device rd;\n std::mt19937 gen(rd());\n\n std::vector times;\n long long t = 1583315232492;\n double tick = 1.0 / freqHz;\n for (int i = 0; i < durationStaticSec / tick; i++) {\n times.emplace_back(t);\n t += 1000 * tick;\n }\n\n std::vector accelMeasurements;\n for (long long ts: times)\n {\n //the variance is choosen to be equal to typical accelerometer noise variance of cheap IMU sensor\n double var = 2 * M_PI * freqHz * G * pow(0.001, 2);\n std::normal_distribution distr(0.0, var);\n\n double ax = -G * sin(tiltPitch) + distr(gen);\n double ay = 0.0 + distr(gen);\n double az = G * cos(tiltPitch) + distr(gen);\n SensorMeasurement msr;\n msr.ts = ts;\n msr.values = Vector3d(ax, ay, az);\n msr.type = SensorMeasurement::Type::ACCELEROMETER;\n accelMeasurements.emplace_back(msr);\n }\n\n std::vector gyroMeasurements;\n for (long long ts: times)\n {\n //the variance is choosen to be equal to typical gyroscope noise variance of cheap IMU sensor\n double var = 2 * M_PI * freqHz * pow(0.01 * M_PI / 180.0, 2);\n std::normal_distribution distr(0.005, var);\n\n double wx = 0.0 + distr(gen);\n double wy = 0.0 + distr(gen);\n double wz = 0.0 + distr(gen);\n SensorMeasurement msr;\n msr.ts = ts;\n msr.values = Vector3d(wx, wy, wz);\n msr.type = SensorMeasurement::Type::GYROSCOPE;\n gyroMeasurements.emplace_back(msr);\n }\n\n std::vector magnMeasurements;\n for (long long ts: times)\n {\n double mx = MAGNETIC_FLUX_DENSITY * cos(MAGNETIC_FIELD_INCLINATION + tiltPitch);\n double my = 0.0;\n double mz = MAGNETIC_FLUX_DENSITY * sin(MAGNETIC_FIELD_INCLINATION + tiltPitch);\n SensorMeasurement msr;\n msr.ts = ts;\n msr.values = Vector3d(mx, my, mz);\n msr.type = SensorMeasurement::Type::MAGNETOMETER;\n magnMeasurements.emplace_back(msr);\n }\n\n return std::make_tuple(accelMeasurements, gyroMeasurements, magnMeasurements);\n}\n\nBOOST_AUTO_TEST_CASE(staticTilt)\n{\n std::vector accelMeasurements;\n std::vector gyroMeasurements;\n std::vector magnMeasurements;\n\n double durationStaticSec = 10.0;\n double tiltPitch = 0.0;\n std::tie(accelMeasurements, gyroMeasurements, magnMeasurements) =\n getTiltedImuMeasurements(durationStaticSec, tiltPitch);\n\n Orientation ori(0, 0, 0, 0);\n ComplementaryFilter complementaryFilter;\n for (std::size_t i = 0; i < accelMeasurements.size(); i++)\n {\n const SensorMeasurement& accelMsr = accelMeasurements.at(i);\n const SensorMeasurement& gyroMsr = gyroMeasurements.at(i);\n const SensorMeasurement& magnMsr = magnMeasurements.at(i);\n complementaryFilter.update(accelMsr);\n complementaryFilter.update(gyroMsr);\n complementaryFilter.update(magnMsr);\n ori = complementaryFilter.getFusedOrientation();\n }\n\n BOOST_CHECK(std::abs(ori.roll) < M_PI * 1.0 / 180);\n BOOST_CHECK(std::abs(ori.pitch - tiltPitch) < M_PI * 1.0 / 180);\n}\n" }, { "path": "standalone_algorithms/pedometer/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.7)\nproject(Navigation-core)\n\noption(BUILD_TESTS \"Build tests\" OFF)\noption(BUILD_EXAMPLES \"Build examples\" ON)\n\nset(BOOST_ENABLE_CMAKE ON)\nset(Boost_USE_STATIC_LIBS OFF)\n\nfind_package(Boost COMPONENTS system unit_test_framework REQUIRED)\nIF (Boost_FOUND)\n INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})\n message(${Boost_INCLUDE_DIR})\n include_directories(${Boost_SOURCE_DIR})\nENDIF ()\n\nset(\"target\" \"pedometer\")\nset(\"test-target\" \"test\")\n\nif(BUILD_TESTS)\n file(GLOB ${PROJECT_NAME}_TEST_SOURCES src/*.cpp tests/*.cpp)\n add_executable(\"${test-target}\" ${${PROJECT_NAME}_TEST_SOURCES})\n target_include_directories(\"${test-target}\" PRIVATE \"include/\")\n set_property(TARGET \"${test-target}\" PROPERTY CXX_STANDARD 17)\n set_target_properties(\"${test-target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\n set_target_properties(\"${test-target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions \")\n target_link_libraries(\"${test-target}\"\n #\"-lboost_unit_test_framework\"\n # Boost::unit_test_framework\n ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}\n )\nendif()\n\nif(BUILD_EXAMPLES)\n list(APPEND \"${target}__cxx_srcs\"\n \"examples/main.cpp\"\n \"src/pedometer.cpp\"\n \"src/vector3d.cpp\")\n add_executable(\"${target}\" ${${target}__cxx_srcs})\n target_include_directories(\"${target}\" PRIVATE \"include/\")\n set_property(TARGET \"${target}\" PROPERTY CXX_STANDARD 17)\n set_target_properties(\"${target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\n set_target_properties(\"${target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions \")\nendif()\n" }, { "path": "standalone_algorithms/pedometer/README.md", "content": "### Pedometer\n\nThe pedometer estimates steps based on accelerometer measurements collected from the user device.\n\nThe algorithm analyses three input acceleration components (ax, ay, az) and the time stamp.\nFor details on the algorithm implementation, please refer to the corresponding unit test in GitHub.\n\n### Algorithm\n\nOn each line of a log file there are timestamp and three input acceleration components (ax, ay, az).\nThe process starts with reading raw acceleration data. Since the data could contain noise, it is processed to get rid of any noise. Magnitudes are calculated with low-passed filter and the average magnitude values are also computed according to the time interval for average value estimation. Once filtered, the noise reduced acceleration data is analyzed to detect whether a person is walking. Step counting is only performed if a walk is detected. Otherwise, the system goes back to reading the next window of acceleration data.\n\nIf walking is detected, step counting is performed using a zero crossing technique. Signal, crossing the zero mark once in the negative direction followed by the same action but in the positive direction, can be observed. This phenomena is called zero crossing, and it can be used for step counting.\n\nEven after filtering there is still some unnecessary noise in the measurements. Zero crossing is considered\nonly if the signal has come from outside this range, otherwise it is ignored.\n\n### Build\n\nTo build the project for running tests make sure that `BUILD_TESTS` option in CMakeLists.txt is turned on.\n\nTo build the project for running example turn `BUILD_EXAMPLES` on.\n\n```sh\ncd /standalone_algorithms/pedometer && rm -rf build\ncmake -Bbuild -H.\ncmake --build build\n```\n\nRun tests:\n\n```sh\n./build/test-filter\n```\n\nRun example:\n\n```sh\n./build/pedometer logs/HuaweiLong1.log\n```\n" }, { "path": "standalone_algorithms/pedometer/examples/main.cpp", "content": "#include \n#include \n#include \n#include \n\nusing namespace navigine::navigation_core;\n\nstd::vector parseMeasurements(const std::string& logFile)\n{\n std::vector measurements;\n std::ifstream is(logFile);\n if (!is.is_open())\n {\n std::cout << \"could not open file \" << logFile << std::endl;\n }\n\n std::string line;\n std::getline(is, line); // skip test data\n while (std::getline(is, line))\n {\n SensorMeasurement msr;\n std::istringstream iss(line);\n if ((iss >> msr.ts >> msr.values.x >> msr.values.y >> msr.values.z))\n measurements.emplace_back(msr);\n else\n break;\n }\n is.close();\n return measurements;\n}\n\nint main(int argc, char *argv[])\n{\n if (argc < 2)\n {\n std::cout << \"can not find configuration file in command line argument! \" << std::endl;\n exit(-1);\n }\n\n std::string logFile = std::string(argv[1]);\n std::vector measurements = parseMeasurements(logFile);\n\n Pedometer pedometer;\n long stepCounter = 0;\n for (SensorMeasurement msr: measurements)\n {\n std::vector singleMsrVector;\n singleMsrVector.emplace_back(msr);\n pedometer.update(singleMsrVector);\n std::deque steps = pedometer.calculateSteps();\n stepCounter += steps.size();\n }\n\n std::cout << \"Number of steps: \" << stepCounter << std::endl;\n\n return 0;\n}\n" }, { "path": "standalone_algorithms/pedometer/include/pedometer.h", "content": "#pragma once\n\n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct SensorMeasurement\n{\n long long ts = -1;\n Vector3d values = Vector3d();\n};\n\nstruct Step\n{\n long long ts;\n double len;\n Step() : ts(0), len(0) {};\n Step(long long stepTs, double stepLength) : ts(stepTs), len(stepLength) {};\n};\n\nstruct Magnitude\n{\n long long ts {};\n double value {};\n Magnitude() : ts(0), value(0) {};\n Magnitude(long long msrTs, double magnitudeValue) : ts(msrTs), value(magnitudeValue) {};\n};\n\n/**\n * This class detects step of the human on the basis of accelerometer measurements\n * and empirical model of humans gate.\n * Step length is calculated using minimal and maximal accelerations\n*/\nclass Pedometer\n{\npublic:\n Pedometer() = default;\n \n void update(const std::vector& accMsrs);\n std::deque calculateSteps();\n\nprivate:\n Magnitude calculateFilteredAccMagnitudes() const;\n double calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt);\n\n //we use deque in order to fast delete measurements of the array when we got new ones\n std::deque mAccelMeasurements; // Array for saving acceleration measurements\n std::deque mFilteredAccMagnitudes; // Array for saving filtered acceleration magnitudes\n std::deque mTimes; // Array for saving step durations\n \n std::size_t mMagnitudeSize = 0;\n long long mPossibleStepTs = -1; // The time when step possibly occurred (sec in unix)\n long long mStepTime = -1; // The time when previous step possibly occured (sec in unix)\n bool mIsStep = false; // We use this variable to mark possible step\n};\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/pedometer/include/vector3d.h", "content": "#pragma once\n\n#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Vector3d \n{\n double x = 0.0;\n double y = 0.0;\n double z = 0.0;\n\n Vector3d() = default;\n Vector3d(double _x, double _y, double _z);\n\n double magnitude() const;\n Vector3d normalized() const;\n Vector3d& normalize();\n bool isNull() const;\n\n\n static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2);\n static double dotProduct (const Vector3d& v1, const Vector3d& v2);\n\n Vector3d& operator+=(const Vector3d& v2);\n Vector3d& operator-=(const Vector3d& v2);\n Vector3d& operator*=(double multiplier);\n Vector3d& operator/=(double divisor);\n};\n\n\ninline bool operator==(const Vector3d& v1, const Vector3d& v2)\n{\n return std::fabs(v1.x - v2.x) < std::numeric_limits::epsilon() &&\n std::fabs(v1.y - v2.y) < std::numeric_limits::epsilon() &&\n std::fabs(v1.z - v2.z) < std::numeric_limits::epsilon();\n}\n\ninline bool operator!=(const Vector3d& v1, const Vector3d& v2)\n{\n return !(v1 == v2);\n}\n\ninline Vector3d operator*(double multiplier, const Vector3d& v)\n{\n return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);\n}\n\ninline Vector3d operator*(const Vector3d& v, double multiplier)\n{\n return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);\n}\n\ninline Vector3d operator+(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);\n}\n\ninline Vector3d operator-(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);\n}\n\ninline Vector3d operator-(const Vector3d& v)\n{\n return Vector3d(-v.x, -v.y, -v.z);\n}\n\ninline Vector3d operator/(const Vector3d& v1, double divisor)\n{\n return Vector3d(v1.x / divisor, v1.y / divisor, v1.z / divisor);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/pedometer/src/pedometer.cpp", "content": "#include \n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace {\n static const long long AVERAGING_TIME_INTERVAL_MS = 2500; // Time interval on which we calculate averaged filtered magnitude in order to align it relative to zero value\n static const long long FILTER_TIME_INTERVAL_MS = 200; // Filtered acc measurements are calculated as average over this time interval\n\n static const long long UPDATE_TIME_INTERVAL_MS = 700; // The time interval(in millisec) on which we update accelMax, Min, threshold\n static const long long MIN_TIME_BETWEEN_STEPS_MS = 300; // We cut off possible steps if time between them is less then this thresh\n\n static const double STEP_LENGTH_CONST = 0.52; // Constant using in algorithm which is responisble for step length calculation\n static const double MINIMAL_THRESHOLD_VALUE = 0.05 * 9.80665; // Empirical threshold values from article p.888\n\n static const int MINIMAL_NUMBER_OF_STEPS = 5; // First N steps in which we accumulate data about average step time\n static const int MAXIMUM_NUMBER_OF_STEPS = 50; // Maximum number of steps to make an assumption about average step time\n static const int MAX_STEP_TIME = 2000; // The maximum possible time for step duration\n}\n\nvoid Pedometer::update(const std::vector& msrs)\n{\n mMagnitudeSize = (std::max)(mFilteredAccMagnitudes.size(), size_t(1));\n\n for (auto msr: msrs)\n {\n mAccelMeasurements.push_back(msr);\n Magnitude magn = calculateFilteredAccMagnitudes();\n if (magn.ts!=0)\n mFilteredAccMagnitudes.push_back(magn);\n\n while (mAccelMeasurements.back().ts - mAccelMeasurements.front().ts > 2 * AVERAGING_TIME_INTERVAL_MS &&\n mAccelMeasurements.size() > 0)\n {\n mAccelMeasurements.pop_front();\n }\n }\n}\n\n/**\n * This method calculates and form array with filtered acceleration magnitude\n * sliding window average is used (low pass). This function also updates average\n * filtered magnitude\n */\nMagnitude Pedometer::calculateFilteredAccMagnitudes() const\n{\n long long lastMeasTs = mAccelMeasurements.back().ts;\n\n std::deque::const_iterator lBorderAverIt = mAccelMeasurements.begin();\n while (lastMeasTs - lBorderAverIt->ts >= AVERAGING_TIME_INTERVAL_MS)\n lBorderAverIt++;\n \n double averMagnitude = std::accumulate(lBorderAverIt, mAccelMeasurements.end(), 0.0,\n [](double sum, const SensorMeasurement msr)\n {return sum+msr.values.magnitude();});\n double nMeasAverage = std::distance(lBorderAverIt, mAccelMeasurements.end());\n\n std::deque::const_iterator lBorderFilterIt = mAccelMeasurements.begin();\n while (lastMeasTs - lBorderFilterIt->ts >= FILTER_TIME_INTERVAL_MS)\n lBorderFilterIt++;\n\n double filterMagnitude = std::accumulate(lBorderFilterIt, mAccelMeasurements.end(), 0.0, \n [](double sum, const SensorMeasurement msr)\n {return sum+msr.values.magnitude();});\n double nMeasFiltered = std::distance(lBorderFilterIt, mAccelMeasurements.end());\n\n // There is no measurements at acceleration array!\\n\");\n if (nMeasAverage == 0 || nMeasFiltered == 0)\n return Magnitude(0,0);\n\n filterMagnitude /= nMeasFiltered;\n averMagnitude += filterMagnitude; // For correct processing at the very beginning\n averMagnitude /= nMeasAverage;\n filterMagnitude -= averMagnitude;\n \n return Magnitude(lastMeasTs, filterMagnitude);\n}\n\nstd::deque Pedometer::calculateSteps()\n{\n std::deque steps = {};\n if (mFilteredAccMagnitudes.size() <= 3) //There is no enough accelerometer measurements to detect steps\n return steps;\n\n long long averageStepTime = 0;\n auto nSteps = std::max(std::distance(mTimes.begin(), mTimes.end()), std::ptrdiff_t(1));\n\n if (nSteps >= MINIMAL_NUMBER_OF_STEPS) \n averageStepTime = std::accumulate(mTimes.begin(), mTimes.end(), 0, \n [](long long sum, long long s) \n { return sum + s; });\n averageStepTime /= nSteps;\n double timeBetweenSteps = std::max(1.0 * MIN_TIME_BETWEEN_STEPS_MS, 0.6 * averageStepTime);\n\n for (size_t i = mMagnitudeSize; i < mFilteredAccMagnitudes.size(); i++)\n {\n Magnitude curAcc = mFilteredAccMagnitudes[i];\n Magnitude prevAcc = mFilteredAccMagnitudes[i-1];\n \n //True if cross threshold and if new detection isn't too early.\n if (!mIsStep &&\n prevAcc.value < MINIMAL_THRESHOLD_VALUE &&\n curAcc.value > MINIMAL_THRESHOLD_VALUE &&\n curAcc.ts - mPossibleStepTs > MIN_TIME_BETWEEN_STEPS_MS &&\n timeBetweenSteps > 0) // && curAcc.ts - mPossibleStepTs > timeBetweenSteps\n {\n mIsStep = true;\n mPossibleStepTs = curAcc.ts;\n }\n\n if (mIsStep)\n {\n double stepLength = calculateStepLength(UPDATE_TIME_INTERVAL_MS, mFilteredAccMagnitudes.begin()+i);\n if (mPossibleStepTs - mStepTime < MAX_STEP_TIME)\n mTimes.push_back(mPossibleStepTs - mStepTime);\n\n steps.push_back(Step(mPossibleStepTs, stepLength));\n mStepTime = mPossibleStepTs;\n mIsStep = false; // prepare to detect new step\n }\n }\n\n while(mFilteredAccMagnitudes.back().ts - mFilteredAccMagnitudes.front().ts > 2 * UPDATE_TIME_INTERVAL_MS)\n mFilteredAccMagnitudes.pop_front();\n\n while (mTimes.size() > MAXIMUM_NUMBER_OF_STEPS) \n mTimes.pop_front(); \n\n return steps;\n}\n\ndouble Pedometer::calculateStepLength(long long timeIntervalMs, std::deque::const_iterator rightBorderIt)\n{\n std::deque::const_iterator leftBorderIt = rightBorderIt;\n while (rightBorderIt->ts - leftBorderIt->ts <= timeIntervalMs &&\n leftBorderIt != mFilteredAccMagnitudes.begin() )\n leftBorderIt--;\n\n double maxMagn = std::max_element(leftBorderIt, rightBorderIt,\n [](Magnitude m1, Magnitude m2) \n {return m1.value < m2.value; })->value;\n double minMagn = std::min_element(leftBorderIt, rightBorderIt, \n [](Magnitude m1, Magnitude m2)\n {return m1.value < m2.value; })->value;\n double accAmplitude = maxMagn - minMagn;\n double stepLen = STEP_LENGTH_CONST * sqrt(sqrt(accAmplitude));\n\n return stepLen;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/pedometer/src/vector3d.cpp", "content": "#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nVector3d::Vector3d(double _x, double _y, double _z)\n{\n x = _x;\n y = _y;\n z = _z;\n}\n\ndouble Vector3d::magnitude() const\n{\n return std::sqrt(x*x + y*y + z*z);\n}\n\nVector3d Vector3d::normalized() const\n{\n double length = this->magnitude();\n \n return (std::fabs(length) > std::numeric_limits::epsilon()) ? Vector3d(x / length, y / length, z / length) : *this;\n}\n\nVector3d& Vector3d::normalize()\n{\n double length = this->magnitude();\n \n if (std::fabs(length) > std::numeric_limits::epsilon())\n *this /= length;\n return *this;\n}\n\nbool Vector3d::isNull() const\n{\n return x == 0 && y == 0 && z == 0;\n}\n\nVector3d& Vector3d::operator+=(const Vector3d& v)\n{\n this->x += v.x;\n this->y += v.y;\n this->z += v.z;\n return *this;\n}\n\nVector3d& Vector3d::operator-=(const Vector3d& v)\n{\n this->x -= v.x;\n this->y -= v.y;\n this->z -= v.z;\n return *this;\n}\n\nVector3d& Vector3d::operator*=(double multiplier)\n{\n this->x *= multiplier;\n this->y *= multiplier;\n this->z *= multiplier;\n return *this;\n}\n\nVector3d& Vector3d::operator/=(double divisor)\n{\n this->x /= divisor;\n this->y /= divisor;\n this->z /= divisor;\n return *this;\n}\n\ndouble Vector3d::dotProduct(const Vector3d& v1, const Vector3d& v2)\n{\n return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;\n}\n\nVector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2)\n{\n return Vector3d(v1.y * v2.z - v1.z * v2.y,\n v1.z * v2.x - v1.x * v2.z, \n v1.x * v2.y - v1.y * v2.x);\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/pedometer/tests/pedometer_test.cpp", "content": "#include \n#include \n#include \n\n#include \n\n#define BOOST_TEST_DYN_LINK\n#define BOOST_TEST_MODULE Pedometer\n#include \n\nusing namespace navigine::navigation_core;\n\nnamespace\n{\n static const double STEP_NUM_TEST_THRESHOLD_PERCENTAGE = 5.0;\n static const double STEP_LEN_TEST_THRESHOLD_PERCENTAGE = 20.0;\n static const std::string TEST_DATA_FOLDER = \"logs\";\n}\n\nstd::pair, std::vector> parseTestData(const std::string& logFile)\n{\n std::vector measurements;\n std::ifstream is(logFile);\n if (!is.is_open())\n {\n std::cout << \"could not open file \" << logFile << std::endl;\n }\n std::string line;\n int stepsNum = 0;\n double stepLen = 0.0;\n while (std::getline(is, line))\n {\n std::istringstream iss(line);\n if (stepsNum == 0) // first line with ground truth\n {\n iss >> stepsNum >> stepLen;\n }\n else\n {\n SensorMeasurement msr;\n if ((iss >> msr.ts >> msr.values.x >> msr.values.y >> msr.values.z))\n measurements.emplace_back(msr);\n else\n break;\n }\n }\n is.close();\n return std::make_pair(std::make_pair(stepsNum, stepLen), measurements);\n}\n\nstd::pair getStepsNumLenPair(const std::vector& measuremetns)\n{\n Pedometer pedometer;\n int stepCounter = 0;\n double stepLen = 0.0;\n for (const SensorMeasurement& msr: measuremetns)\n {\n std::vector singleMsrVector;\n singleMsrVector.emplace_back(msr);\n pedometer.update(singleMsrVector);\n std::deque steps = pedometer.calculateSteps();\n for (const Step& s: steps)\n stepLen += s.len;\n\n stepCounter += steps.size();\n }\n stepLen /= static_cast(stepCounter);\n return std::make_pair(stepCounter, stepLen);\n}\n\nBOOST_AUTO_TEST_CASE(stepsHuaweiLong1)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/HuaweiLong1.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsHuaweiLong2)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/HuaweiLong2.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsIPhoneShort1)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/iPhoneShort1.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsIPhoneShort2)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/iPhoneShort2.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsIPhoneShort3)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/iPhoneShort3.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsIPhoneShort4)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/iPhoneShort4.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsNexusShort1)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/NexusShort1.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsNexusShort2)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/NexusShort2.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsNexusShort3)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/NexusShort3.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsXiaomiLong1)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/XiaomiLong1.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsXiaomiLong2)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/XiaomiLong2.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsXiaomiLong3)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/XiaomiLong3.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\nBOOST_AUTO_TEST_CASE(stepsXiaomiLong4)\n{\n std::string testDir = TEST_DATA_FOLDER;\n std::pair,\n std::vector > testData = parseTestData(testDir + \"/XiaomiLong4.log\");\n\n std::pair trueStepsNumLenPair = testData.first;\n std::pair stepsNumLenPair = getStepsNumLenPair(testData.second);\n BOOST_CHECK_CLOSE(static_cast(stepsNumLenPair.first),\n static_cast(trueStepsNumLenPair.first), STEP_NUM_TEST_THRESHOLD_PERCENTAGE);\n BOOST_CHECK_CLOSE(stepsNumLenPair.second, trueStepsNumLenPair.second, STEP_LEN_TEST_THRESHOLD_PERCENTAGE);\n}\n\n" }, { "path": "standalone_algorithms/position_estimation/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.7)\nproject(Position-estimation)\n\nset(\"target\" \"position-estimation\")\n\nlist(APPEND \"${target}__cxx_srcs\"\n \"examples/main.cpp\"\n \"src/measurement_preprocessor.cpp\"\n \"src/nearest_transmitter_estimator.cpp\"\n \"src/position_smoother.cpp\")\nadd_executable(\"${target}\" ${${target}__cxx_srcs})\ntarget_include_directories(\"${target}\" PRIVATE \"include/\")\nset_property(TARGET \"${target}\" PROPERTY CXX_STANDARD 17)\nset_target_properties(\"${target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\nset_target_properties(\"${target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -pipe -m64 -march=x86-64 -fstack-protector-strong -pthread -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -Wno-undefined-bool-conversion -Wno-tautological-undefined-compare -std=c++17 -frtti -fexceptions \")\n" }, { "path": "standalone_algorithms/position_estimation/README.md", "content": "### Position Estimation\n\nUsing RSSI data from transmitters it is possible to estimate the position of moving object. Estimation is being held according to the distance between the source of signals and the destination.\n\n### Measurement Preprocessor\n\nThe information about transmitters is stored in `/logs/transmitters.txt` file in a format `tid, coordinates, type`\n\n```\n(38945,2275,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 238.52 21.3 BEACON\n```\n\nMeasurements are stored in `/logs/measurements.log` in the folowing format `timestamp, tid, rssi, type`\n\n```\n1541003875242 (56813,7748,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) -82 BEACON\n```\n\nAll the measurements are divided into packets. Preprocessor checks if measurements in the packet are valid:\n\n- if the signal type is supported\n- if rssi is in cutoff borders\n\nAfter that the last measurements are added into measurements buffer. The size of measuremets package is kept limited.\n\n### Position Smoothing\n\n[//]: # \"https://en.wikipedia.org/wiki/Alpha_beta_filter\"\n\nPostprocessing of the position is made by position smoother algorithm that is called alpha-beta filter.\n\nIn this very low order approximation position `x` is obtained as the time integral of velocity `v`. Assuming that velocity remains approximately constant over the small time interval between measurements, the position state is projected forward to predict its value at the next sampling time using equation\n\n

\n\n

\n\nResidual between output measurement and prediction\n\n

\n\n coefficients are chosen to correct the position estimation\n\n

\n\n

\n\nAn extra factor conventionally serves to normalize magnitudes of the multipliers.\n\nFor convergence and stability, the values of the alpha and beta multipliers should be positive and small:\n\n

\n

\n

0\" title=\"4 - 2\\alpha - \\beta > 0\" />\n\nInitial characteristics of the smoothing are set to zero, coefficient is determined by the smoothing coefficient.\n\n### Model of signal transferring\n\nDistance between the source of signal and destination can be converted to signal strength\n\n

\n

\n

\n

\n

\n\n### Results of Position Estimation\n\nCalculated position with precision is written into `output.log` in the folowing format `timestamp, coordinates, precision`\n\n```\n1541003922247 172.704 27.2699 1.55804\n```\n\n### Build\n\n```sh\ncd /standalone_algorithms/position_estimation\ncmake -Bbuild -H.\ncmake --build build\n```\n\nRun examples:\n\n```sh\n./build/position-estimation\n```\n\n### Results of position estimation\n\n\"Markdown" }, { "path": "standalone_algorithms/position_estimation/examples/main.cpp", "content": "#include \n#include \n#include \n#include \"measurement_preprocessor.h\"\n#include \"nearest_transmitter_estimator.h\"\n\nconst std::string TEST_DATA_FOLDER = \"logs/\";\nusing namespace navigine::navigation_core;\n\nstruct NavigationPoint\n{\n NavigationPoint(){}\n\n NavigationPoint(long long _timeMs, double _x, double _y, double _angle, int _sublocation)\n : timeMs (_timeMs)\n , x (_x)\n , y (_y)\n , angle (_angle)\n , sublocation (_sublocation)\n { }\n\n long long timeMs = 0;\n double x = 0;\n double y = 0;\n double angle = 0;\n int sublocation = 0;\n};\n\nstd::vector getTransmitters(const std::string& transmittersFileName)\n{\n std::vector transmitters;\n std::ifstream is(transmittersFileName);\n if (!is.is_open())\n {\n std::cout << \"could not open file \" << transmittersFileName << std::endl;\n }\n std::string line;\n while (std::getline(is, line))\n {\n std::istringstream iss(line);\n std::string typeStr;\n Transmitter t;\n if ((iss >> t.id >> t.x >> t.y >> typeStr ))\n {\n t.type = typeStr.find(\"WIFI\") == std::string::npos\n ? Transmitter::Type::BEACON\n : Transmitter::Type::WIFI;\n transmitters.push_back(t);\n }\n else\n break;\n }\n is.close();\n\n std::cout << \"transmitters size: \" << transmitters.size() << std::endl;\n return transmitters;\n}\n\nstd::vector getMeasurements(const std::string& msrFileName)\n{\n std::vector measurements;\n std::ifstream is(msrFileName);\n if (!is.is_open())\n {\n std::cout << \"could not open file \" << msrFileName << std::endl;\n }\n std::string line;\n while (std::getline(is, line))\n {\n std::istringstream iss(line);\n RadioMeasurement msr;\n std::string typeStr;\n if ((iss >> msr.ts >> msr.id >> msr.rssi >> typeStr))\n {\n if (typeStr.find(\"WIFI\") == std::string::npos)\n msr.type = RadioMeasurement::Type::BEACON;\n else\n msr.type = RadioMeasurement::Type::WIFI;\n\n measurements.push_back(msr);\n }\n else\n break;\n }\n is.close();\n std::cout << \"measurements size: \" << measurements.size() << std::endl;\n return measurements;\n}\n\nstd::vector splitToPackets(const std::vector& inputMeasuremetns)\n{\n MeasurementsPreprocessor measurementsPreprocessor;\n std::vector inputMeasuremetnsPackets;\n for (const RadioMeasurement& msr: inputMeasuremetns) {\n measurementsPreprocessor.update(msr);\n RadioMeasurements measurementsPacket = measurementsPreprocessor.extractRadioMeasurements();\n if (!measurementsPacket.empty()) {\n inputMeasuremetnsPackets.emplace_back(measurementsPacket);\n }\n }\n std::cout << \"measurements packets size: \" << inputMeasuremetnsPackets.size() << std::endl;\n return inputMeasuremetnsPackets;\n}\n\nint main()\n{\n std::string testDataFolder = TEST_DATA_FOLDER;\n std::string transmittersFileName = testDataFolder + \"transmitters.txt\";\n std::string msrFileName = testDataFolder + \"measurements.log\";\n std::string outputFileName = testDataFolder + \"output.log\";\n\n std::vector transmitters = getTransmitters(transmittersFileName);\n std::vector inputMeasuremetns = getMeasurements(msrFileName);\n std::vector inputMeasuremetnsPackets = splitToPackets(inputMeasuremetns);\n\n NearestTransmitterPositionEstimator nearestBeaconPositionEstimator = NearestTransmitterPositionEstimator(transmitters);\n\n std::ofstream os;\n os.open(outputFileName);\n for (const RadioMeasurements &msr: inputMeasuremetnsPackets)\n {\n Position p = nearestBeaconPositionEstimator.calculatePosition(msr);\n if (!p.isEmpty)\n {\n os << p.ts << \" \" << p.x << \" \" << p.y << \" \" << p.precision << std::endl;\n }\n }\n os.close();\n std::cout << \"calculated positions were written to \" << outputFileName << std::endl;\n\n return 0;\n}\n" }, { "path": "standalone_algorithms/position_estimation/helpers/plot_positions.py", "content": "import pandas as pd\nimport matplotlib.pylab as plt\n\ndf_calculated = pd.read_csv('../logs/output.log', sep=' ', names=['ts', 'x', 'y', 'r'])\ndf_benchmark = pd.read_csv('../logs/benchmarks.log', sep=' ', names=['x', 'y', 'ts', 'ori', 'subloc'])\n\nplt.gca().set_aspect('equal')\nplt.plot(df_calculated.x, df_calculated.y, '.-', label='calculated positions')\nplt.plot(df_benchmark.x, df_benchmark.y, '-', label='reference trace')\n\nplt.legend(bbox_to_anchor=(1.0, -0.25))\nplt.show()\n\n" }, { "path": "standalone_algorithms/position_estimation/include/measurement_preprocessor.h", "content": "#pragma once\n\n#include \n#include \n#include \n#include \n\n#include \"navigation_structures.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass RadioMeasurementBuffer\n{\npublic:\n RadioMeasurementBuffer() = default;\n RadioMeasurementBuffer(long long sigWindowShiftMs, long long radioKeepPeriodMs);\n\n std::vector extractMeasurements();\n void addMeasurements(long long messageTs, const std::vector& signalEntries);\n\nprivate:\n std::vector mMeasurements = {};\n long long mLastExtractionTs = -1;\n long long mCurrentTs = -1;\n long long mRadioKeepPeriodMs = 0;\n long long mSignalWindowShiftMs = 0;\n};\n\nclass MeasurementsPreprocessor\n{\n public:\n MeasurementsPreprocessor(double sigAverageTimeSec = 3.0,\n double sigWindowShiftSec = 2.0,\n bool useWifi = true,\n bool useBle = true);\n\n void update(const RadioMeasurement &msr);\n long long getCurrentTs() const;\n std::vector extractRadioMeasurements();\n\n private:\n bool isSignalTypeSupported(RadioMeasurement::Type signalType) const;\n bool isRssiValid(double rssi) const;\n\n private:\n RadioMeasurementBuffer mRadiosBuffer;\n double mCutOffRssi = -100;\n long long mCurrentTs = -1;\n double mSigAverageTimeSec = 3.0;\n double mSigWindowShiftSec = 2.0;\n bool mUseWifi = true;\n bool mUseBle = true;\n bool mUseClosestAps = true;\n int mNumClosestAps = 5;\n};\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/position_estimation/include/navigation_structures.h", "content": "#pragma once\n\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Point2D\n{\n Point2D()\n : x(0.0)\n , y(0.0) {}\n\n Point2D(double _x, double _y)\n : x(_x)\n , y(_y) {}\n\n double x;\n double y;\n};\n\nstruct Position\n{\n double x = 0.0;\n double y = 0.0;\n double precision = 0.0;\n bool isEmpty = true;\n long long ts = -1;\n};\n\nstruct RadioMeasurement\n{\n enum class Type {WIFI, BEACON};\n\n Type type; \n long long ts = 0;\n std::string id = \"\"; // Entry id (bssid, mac, major+minor+uuid)\n double rssi = 0;\n double power = 0;\n double frequency = 0;\n double distance = 0;\n double stddev = 0;\n};\n\nstruct NavigationInput\n{\n int packetNumber = 0; // Message number\n std::string deviceId = \"\"; // Device identifier (MAC address/id/...)\n std::string checkPoint = \"\";\n long long timeStamp = 0;\n \n std::vector signalEntries = {};\n};\n\nusing RadioMeasurements = std::vector;\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/position_estimation/include/nearest_transmitter_estimator.h", "content": "#pragma once\n\n#include \n#include \"transmitter.h\"\n#include \"navigation_structures.h\"\n#include \"position_smoother.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass NearestTransmitterPositionEstimator\n{\npublic:\n NearestTransmitterPositionEstimator(const std::vector& transmitters);\n\n Position calculatePosition(const RadioMeasurements& inputMeasurements);\n\nprivate:\n std::vector getRegisteredTransmittersMeasurements(\n const std::vector& radioMsr);\n Point2D getTransmitterPosition(const std::string& txId);\n\n PositionSmoother m_smoother;\n std::map m_transmitters;\n};\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/position_estimation/include/position_smoother.h", "content": "#pragma once\n\n#include \"navigation_structures.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionSmoother\n{\n public:\n PositionSmoother(double smoothing = 0.5);\n Position smoothPosition(Position pos);\n\n private:\n bool mWasCalled = false;\n double mSpeedX = 0.0;\n double mSpeedY = 0.0;\n double mAlpha = 0.0;\n\n Position mPosition;\n long long mTsMs;\n};\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/position_estimation/include/transmitter.h", "content": "#pragma once\n\n#include \n#include \n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Transmitter \n{\n enum class Type { WIFI, BEACON };\n\n std::string id;\n double x;\n double y;\n Type type;\n\n bool operator< (const Transmitter& tx)const { return id < tx.id; }\n};\n\ntypedef std::vector Transmitters;\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/position_estimation/logs/transmitters.txt", "content": "(404,404,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 138.973 88.546 BEACON\n(284,284,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 139.712 82.5232 BEACON\n(46266,37698,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 75.06 91.82 BEACON\n(7667,53553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 84.5999 92.99 BEACON\n(60989,41353,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 95.1199 92.8 BEACON\n(284,53983,00842522-539B-9081-5BA4-768360981005) 105.37 92.51 BEACON\n(46209,34756,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 114.2 92.67 BEACON\n(51948,50362,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 123.65 92.29 BEACON\n(39652,57626,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 132.94 92.21 BEACON\n(35304,213,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 131.88 83.34 BEACON\n(24210,11385,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 142.54 82.37 BEACON\n(61521,40127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 150.69 90.33 BEACON\n(56109,26280,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 160.7 82.27 BEACON\n(8922,21995,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 182.81 82.1 BEACON\n(8367,8843,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.1 82.19 BEACON\n(40720,2489,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 222.46 83.57 BEACON\n(10085,46410,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 241.09 83.29 BEACON\n(36896,34867,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 240.93 94.79 BEACON\n(28112,40494,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 60.2799 92.17 BEACON\n(12604,13902,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 69.5099 88.88 BEACON\n(43057,47231,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 56.0599 97.22 BEACON\n(328,328,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 50.43 89.53 BEACON\n(99,99,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 60.36 87.67 BEACON\n(242,242,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 101.522 102.826 BEACON\n(156,156,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 484.076 106.81 BEACON\n(330,330,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 461.843 107.298 BEACON\n(176,176,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 436.077 106.932 BEACON\n(130,130,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 71.1359 103.53 BEACON\n(202,202,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 69.9753 90.1706 BEACON\n(183,183,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 47.4299 94.4125 BEACON\n(349,349,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 27.9794 90.2586 BEACON\n(149,149,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 17.463 106.645 BEACON\n(471,471,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 479.313 101.775 BEACON\n(23741,17177,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 249.12 121.64 BEACON\n(18464,22743,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 400.54 114.35 BEACON\n(23924,199,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 416.71 118.96 BEACON\n(54510,38378,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 428.63 103.08 BEACON\n(36927,9530,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 409.14 102.81 BEACON\n(12778,34654,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 394.37 103.21 BEACON\n(35265,63345,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 371.67 103.36 BEACON\n(17839,34553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 342.86 103.36 BEACON\n(2832,16485,009E12DB-8622-2FFC-9D1F-164961CE1005) 313.08 103.62 BEACON\n(19342,17833,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 283.65 103.58 BEACON\n(53008,63284,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 269.09 104.41 BEACON\n(52004,53550,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 246.55 101.87 BEACON\n(46240,53976,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 261.24 119.47 BEACON\n(19652,64817,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 224.55 101.47 BEACON\n(48307,21358,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 228.17 89.44 BEACON\n(8266,24512,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.65 90.81 BEACON\n(23247,40052,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 168.9 89.98 BEACON\n(5678,65103,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 138.41 89.51 BEACON\n(21263,20026,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 132.07 103.85 BEACON\n(48149,28656,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.97 103.69 BEACON\n(2379,11009,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 93.9402 111.53 BEACON\n(1888,13768,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 97.3 114.69 BEACON\n(29394,48788,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 77.3199 67.63 BEACON\n(11081,29004,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.12 21.33 BEACON\n(60638,1586,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 291.18 21.28 BEACON\n(40719,22526,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 186.5 62.27 BEACON\n(47944,17364,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 99.5099 63.86 BEACON\n(16468,35906,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 276.94 63.82 BEACON\n(4230,3488,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 277.85 58.02 BEACON\n(47429,18703,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 290.59 58.75 BEACON\n(40914,14221,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 270.55 58.61 BEACON\n(4620,59742,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 257.07 58.73 BEACON\n(29981,34287,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 244.63 58.98 BEACON\n(22433,27932,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 231.75 58.32 BEACON\n(39876,5616,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 221.21 58.64 BEACON\n(50760,2989,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 208.96 58.81 BEACON\n(47225,61209,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 188.96 58.04 BEACON\n(15885,64531,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 195.34 68.06 BEACON\n(38584,19822,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 180.61 59.05 BEACON\n(17595,6881,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 167.44 58.04 BEACON\n(41004,65021,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 154.69 58.02 BEACON\n(3722,6190,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 143.77 58.43 BEACON\n(19944,60209,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 135.08 58 BEACON\n(7200,29838,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 125.12 58.62 BEACON\n(1484,7639,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.04 58.99 BEACON\n(22296,23760,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.41 68.62 BEACON\n(37837,18741,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 89.7 61.18 BEACON\n(27819,63658,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 96.56 58.63 BEACON\n(60589,31837,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 107.97 58.72 BEACON\n(27369,16528,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 188.29 2.33001 BEACON\n(15823,16948,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 97.0499 8.04998 BEACON\n(43834,6573,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 95.9099 12.1 BEACON\n(22790,12825,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.89 12 BEACON\n(42883,2294,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 113.11 12.7 BEACON\n(17028,59968,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106 1.08999 BEACON\n(22344,41020,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 118.01 12.09 BEACON\n(55543,22194,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 130.19 11.97 BEACON\n(65209,31481,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 142.27 11.51 BEACON\n(41698,57655,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 155.14 11.77 BEACON\n(5190,14288,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 166.94 11.94 BEACON\n(6957,49198,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 176.27 11.95 BEACON\n(10561,36193,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.01 11.38 BEACON\n(64077,47494,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 192.64 2.81998 BEACON\n(53011,18269,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.1 11.98 BEACON\n(45460,17762,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 210.12 11.76 BEACON\n(13815,9970,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 222.54 11.56 BEACON\n(55589,9525,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 233.46 12 BEACON\n(58545,46080,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 242.38 12.05 BEACON\n(41990,26484,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 253.09 11.96 BEACON\n(57640,41428,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 263.06 11.96 BEACON\n(40317,44189,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 271.94 11.96 BEACON\n(29401,9995,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 277.6 11.97 BEACON\n(62753,9949,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 286.82 11.95 BEACON\n(63704,14402,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 281.72 2.91001 BEACON\n(2832,58658,00AE562C-DDDF-3479-B2EA-B8B0CCF91005) 277.1 3.32003 BEACON\n(27962,20960,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 306.24 6.91 BEACON\n(48808,22987,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 300.62 12.02 BEACON\n(19921,32863,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 298.88 58.82 BEACON\n(14474,30357,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 306.17 58.6 BEACON\n(44082,26385,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 312.89 58.44 BEACON\n(2832,48303,00F1E611-08B6-0756-F54B-5697CA341005) 320.94 58.66 BEACON\n(10532,57861,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 321.01 52.97 BEACON\n(11036,16315,0102202B-990F-0100-0082-B38ADD861005) 308.89 52.93 BEACON\n(49151,50749,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 301.51 52.96 BEACON\n(14989,10986,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 298.81 68.06 BEACON\n(19799,60213,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 317.39 68.06 BEACON\n(42577,11390,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.8 58.91 BEACON\n(32303,20995,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 306.37 44.8 BEACON\n(15305,27092,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 299.89 49.39 BEACON\n(52278,59347,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 291.34 49.25 BEACON\n(30285,39395,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 281.91 49.78 BEACON\n(56449,62259,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 275.97 49.39 BEACON\n(37937,32127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 268.18 49.62 BEACON\n(32545,64503,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 261.25 49.56 BEACON\n(56937,18553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 252.75 46.81 BEACON\n(11775,51443,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 244.27 49.36 BEACON\n(1858,36038,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 232.71 49.45 BEACON\n(28491,27829,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 227.81 47.09 BEACON\n(5486,32062,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 212.33 49.48 BEACON\n(61500,20121,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 208.65 41.6 BEACON\n(42453,13118,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 200.93 49.33 BEACON\n(47343,57014,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.17 49.51 BEACON\n(5392,63591,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 181.42 49.47 BEACON\n(17849,42956,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 173.98 45.62 BEACON\n(57772,16444,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 167.85 49.39 BEACON\n(5379,35746,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 156.5 41.8 BEACON\n(20472,41943,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 156.53 49.5 BEACON\n(28251,31240,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 145.11 49.48 BEACON\n(44619,54563,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 138.31 45.48 BEACON\n(47176,48366,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 130.54 45.75 BEACON\n(13431,60500,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 123.55 49.51 BEACON\n(991,13251,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 118.14 44.16 BEACON\n(49080,42158,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.01 49.5 BEACON\n(60472,36645,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 107.25 49.8 BEACON\n(19631,21792,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 101.76 49.62 BEACON\n(59930,7234,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 92.7101 49.37 BEACON\n(34207,61481,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 86.1298 49.78 BEACON\n(22266,24912,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 80.8001 53.89 BEACON\n(13010,42000,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 74.2999 55.89 BEACON\n(3748,62169,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 67.5899 55.96 BEACON\n(46930,2535,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 61.2899 53.82 BEACON\n(26712,61595,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 55.89 49.56 BEACON\n(7152,42738,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 54.1801 43.94 BEACON\n(40197,9211,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 54.1202 35.4 BEACON\n(62292,32638,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 54.4101 27.57 BEACON\n(42768,6484,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 47.0599 28.22 BEACON\n(64614,56909,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 42.4501 32.41 BEACON\n(34987,39476,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 40.1198 27.56 BEACON\n(49702,4902,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 30.1399 28.1 BEACON\n(2413,38956,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 21.3399 24.83 BEACON\n(10994,21572,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 8.79004 24.84 BEACON\n(40867,13112,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 12.81 45.85 BEACON\n(58618,31280,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 21.4601 46.25 BEACON\n(61437,48199,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 29.9601 42.15 BEACON\n(2145,62241,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 36.7301 35.63 BEACON\n(23798,53937,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 40.1599 42.07 BEACON\n(37270,7063,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 47.0301 46.22 BEACON\n(25055,33212,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 45.9001 38.87 BEACON\n(19598,60849,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 56.7099 20.3 BEACON\n(27386,13875,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 61.2201 17.04 BEACON\n(10527,11854,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 66.72 15.13 BEACON\n(21374,52176,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 73.3 14.82 BEACON\n(40773,47752,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 80.8299 17.09 BEACON\n(32110,7548,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 86.9401 21.17 BEACON\n(63533,13204,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 98.4301 21.26 BEACON\n(62668,50359,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 105.07 27.05 BEACON\n(60434,1366,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 105.03 34.12 BEACON\n(22082,40380,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.77 42.22 BEACON\n(54158,28577,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.21 31.77 BEACON\n(57382,11246,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.05 21.36 BEACON\n(50190,53479,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 121.18 21.3 BEACON\n(17996,39932,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 131.13 26.98 BEACON\n(31499,42427,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 131.34 21.34 BEACON\n(34664,56100,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 135.99 22.72 BEACON\n(14964,50089,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 141.62 21.26 BEACON\n(31813,31451,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 147.55 25.09 BEACON\n(15253,33808,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 153.13 21.27 BEACON\n(42124,24780,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 164.47 21.32 BEACON\n(63069,1400,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 175.69 21.19 BEACON\n(14143,28299,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.24 21.22 BEACON\n(49741,50969,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 196.27 20.99 BEACON\n(55479,26935,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.93 21.28 BEACON\n(287,17074,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 212.21 21.34 BEACON\n(52401,25667,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 223.69 21.23 BEACON\n(35744,46356,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 230.92 23.54 BEACON\n(41119,31078,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 225.7 28.52 BEACON\n(25620,58895,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 236.06 32.02 BEACON\n(33707,10271,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 245.58 26.31 BEACON\n(38945,2275,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 238.52 21.3 BEACON\n(37237,1865,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 252.15 21.21 BEACON\n(56337,58963,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 257.24 25.16 BEACON\n(38362,9852,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 264.64 21.28 BEACON\n(61348,8722,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 268.22 23.48 BEACON\n(26849,38969,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 275.75 21.32 BEACON\n(12383,4127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 285.13 21.08 BEACON\n(57301,21859,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.3 35.44 BEACON\n(22686,31629,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 277.03 40.83 BEACON\n(43226,11185,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 271.66 41.02 BEACON\n(18794,50683,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 270.74 27.25 BEACON\n(11368,23693,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 265.57 30.95 BEACON\n(57172,64088,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 260.44 37.69 BEACON\n(32551,30232,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 251.9 31.46 BEACON\n(18110,9010,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 233.58 42.02 BEACON\n(57111,61572,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 222.94 38.89 BEACON\n(41337,42866,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 211.24 37.22 BEACON\n(50894,14185,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.73 36.32 BEACON\n(32043,44935,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 119 40.58 BEACON\n(64841,49079,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 125.38 37.92 BEACON\n(41740,16004,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 134.51 38.65 BEACON\n(50306,20793,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 151.8 41.68 BEACON\n(49364,57737,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 162.3 38.66 BEACON\n(627,55268,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 172.34 41.7 BEACON\n(63714,8737,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 178.82 36.13 BEACON\n(56647,38560,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.28 37.71 BEACON\n(42303,60963,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 119.02 29.51 BEACON\n(21236,33521,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 140.09 29.37 BEACON\n(38825,2584,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 149.19 32.48 BEACON\n(46573,45203,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 158.19 34.24 BEACON\n(14200,25697,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 165.74 30.95 BEACON\n(366,14532,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 91.3001 21.2 BEACON\n(56813,7748,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 115.73 38.86 BEACON\n(34241,24381,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 135.1 49.34 BEACON\n(65397,49904,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 221.92 49.7 BEACON\n(34574,59997,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 243.01 36.17 BEACON\n(47515,53879,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.19 35.88 BEACON\n(49320,44414,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.25 36.48 BEACON\n(59662,5429,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 283.03 35.22 BEACON\n(65307,27568,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 282.97 35.77 BEACON\n(23239,33483,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 159.34 25.03 BEACON\n(23,23,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 93.3202 9.53999 BEACON\n(457,457,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 82.8198 9.06997 BEACON\n(14,14,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 71.17 8.74003 BEACON\n(478,478,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 60.39 8.16003 BEACON\n(479,479,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 54.6001 12.72 BEACON\n(434,434,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 53.5101 17.81 BEACON\n(16294,59149,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 307.01 25.9 BEACON\n(37438,30212,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 297.29 13.22 BEACON\n(49178,13451,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 288.9 13.33 BEACON\n(25643,15815,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 289.86 3.57002 BEACON\n(32946,48985,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 284.12 13.57 BEACON\n(31865,25980,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 274.44 13.74 BEACON\n(38672,41376,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 263.68 13.65 BEACON\n(5324,16642,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 253.32 13.83 BEACON\n(38641,23297,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 242.54 13.96 BEACON\n(39790,3048,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 233.08 13.44 BEACON\n(11783,41227,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 220.81 13.64 BEACON\n(39758,60188,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 212.48 13.56 BEACON\n(33989,55136,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.65 13.68 BEACON\n(32023,55448,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 197.04 3.19002 BEACON\n(27239,45545,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 191.32 13.7 BEACON\n(27254,17421,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 181.64 13.55 BEACON\n(9631,49805,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 169.7 13.6 BEACON\n(38510,34350,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 159.48 13.41 BEACON\n(24322,40944,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.11 9.67001 BEACON\n(37104,21414,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.05 3.19002 BEACON\n(40167,7053,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.1502 22.63 BEACON\n(23953,12738,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.1502 23.82 BEACON\n(25406,1613,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 51.49 31.93 BEACON\n(16328,885,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 48.75 38.4 BEACON\n(2691,3439,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.79 44.54 BEACON\n(39728,12993,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.1502 52.98 BEACON\n(10582,30372,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.41 67.52 BEACON\n(47044,55767,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 109.48 22.8 BEACON\n(26153,1691,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 109.43 52.82 BEACON\n(10950,16425,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 110.52 53.78 BEACON\n(30118,32251,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 52.2502 54.02 BEACON\n(20124,11994,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 104.05 67.36 BEACON\n(54880,10370,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 99.84 62.85 BEACON\n(21425,49970,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.17 63.18 BEACON\n(30975,62343,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 109.44 75.49 BEACON\n(30464,52580,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 122.73 62.69 BEACON\n(30954,64214,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 136.59 62.72 BEACON\n(31620,10493,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 149.23 63.27 BEACON\n(51353,41429,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 161.94 62.84 BEACON\n(31294,9902,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 174.78 62.87 BEACON\n(24511,37763,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 187.1 62.68 BEACON\n(40660,18549,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 202.29 22.28 BEACON\n(9091,23537,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.23 23.68 BEACON\n(28768,44909,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 202.37 52.74 BEACON\n(56802,50744,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.29 53.88 BEACON\n(5444,56164,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.02 72.91 BEACON\n(42471,14138,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 215.67 63 BEACON\n(61334,54776,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 230.3 62.59 BEACON\n(42913,51408,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 255.76 62.97 BEACON\n(14517,39941,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 270 62.4 BEACON\n(49519,60215,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 281.15 62.68 BEACON\n(1848,10242,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 297.12 72.94 BEACON\n(35599,49120,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 295.16 52.77 BEACON\n(1753,42535,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.19 53.79 BEACON\n(20396,60939,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.09 23.62 BEACON\n(51213,55797,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 295.22 22.86 BEACON\n(39644,49117,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 298.42 62.87 BEACON\n(34290,59075,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 290.02 62.17 BEACON\n(58273,63634,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 287.15 72.32 BEACON\n(30523,15802,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.36 62.82 BEACON\n(62784,40038,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 243.53 62.46 BEACON\n(186,186,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 45.6201 44.35 BEACON\n(493,493,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 40.4699 38.37 BEACON\n(92,92,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 45.6599 31.87 BEACON\n(33734,56790,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 154.47 13.47 BEACON\n(56216,42840,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 146.5 13.72 BEACON\n(18995,14191,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 137.89 13.53 BEACON\n(57925,33476,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 130.71 13.43 BEACON\n(53193,43741,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 122.96 13.43 BEACON\n(1813,28558,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 115.36 13.34 BEACON\n(41988,39495,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.59 13.69 BEACON\n(46183,37165,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 100.18 13.28 BEACON\n(36662,40201,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 297.08 46.15 BEACON\n(43855,127,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 294.38 38.33 BEACON\n(21470,51534,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.83 30.4 BEACON\n(244,244,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 308.13 63.19 BEACON\n(109,109,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 320.54 63.46 BEACON\n(354,354,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 329.24 62.94 BEACON\n(282,282,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 338.76 63.7 BEACON\n(1651,12932,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.46 46.19 BEACON\n(42974,4557,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.15 38.36 BEACON\n(34972,7118,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.6 30.39 BEACON\n(33765,30115,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.37 30.39 BEACON\n(59611,12333,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 108.6 38.37 BEACON\n(56314,17772,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.39 46.13 BEACON\n(32105,45524,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 84.9901 13.45 BEACON\n(47765,10946,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 92.4699 13.48 BEACON\n(58976,53535,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 68.18 14.06 BEACON\n(13698,61998,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 60.14 17.66 BEACON\n(392,392,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 62.1101 60.99 BEACON\n(251,251,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 70.7999 62.59 BEACON\n(178,178,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 79.7201 61.95 BEACON\n(66,66,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 89.5002 63.05 BEACON\n(23876,53520,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 76.1999 13.58 BEACON\n(182,182,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 118.212 16.8812 BEACON\n(175,175,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 61.9681 72.7781 BEACON\n(55085,22630,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 32.0699 60.66 BEACON\n(24688,53739,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 42.4 60.52 BEACON\n(23624,39024,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.2199 60.46 BEACON\n(33051,7125,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 53.26 65.65 BEACON\n(46614,41903,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.0099 37.95 BEACON\n(41998,49438,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 50.0999 49.64 BEACON\n(56931,53602,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 41.4499 49.53 BEACON\n(23671,3574,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 53.1499 33.33 BEACON\n(10860,36840,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 105.4 23.8 BEACON\n(23535,64754,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.59 11.36 BEACON\n(59134,25091,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 114.24 24.59 BEACON\n(60265,23475,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 122.1 24.1 BEACON\n(28869,7392,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.49 33.64 BEACON\n(54223,8064,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.32 34.56 BEACON\n(28446,12100,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 112.16 63.99 BEACON\n(62043,20652,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 103.57 21.12 BEACON\n(17260,53988,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.31 14.79 BEACON\n(42801,62584,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 61.87 67.75 BEACON\n(44312,39244,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 70.7402 71.88 BEACON\n(2832,64197,0006FBCA-2FFB-B2D6-6BB8-6E9AA8821005) 82.0901 74.83 BEACON\n(33748,2108,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 88.7998 68.57 BEACON\n(20028,15740,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 97.9999 74.8 BEACON\n(13830,33068,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.6 74.34 BEACON\n(612,59359,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 125.68 23.91 BEACON\n(9508,6979,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 136.34 24.18 BEACON\n(9,47698,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 145.12 23.97 BEACON\n(47468,42482,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 153.48 24.68 BEACON\n(18068,32673,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 161.34 23.51 BEACON\n(34942,62772,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 176.05 24.18 BEACON\n(6293,31077,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 192.12 23.39 BEACON\n(19563,7586,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 200.84 18.66 BEACON\n(10679,11847,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 204.74 24.26 BEACON\n(46839,36006,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.67 33.74 BEACON\n(27325,28878,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 207.46 34.32 BEACON\n(36711,22246,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.89 64.22 BEACON\n(43600,30858,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 216.28 23.19 BEACON\n(38649,16562,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 231.32 24.47 BEACON\n(20119,27949,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 244.87 23.6 BEACON\n(17182,44119,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 260.27 24.57 BEACON\n(48792,21373,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 274.71 23.81 BEACON\n(1646,20179,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 288.22 24.31 BEACON\n(21274,44606,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.02 18.7 BEACON\n(49284,37602,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 301.82 34.37 BEACON\n(3992,32979,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.53 64.45 BEACON\n(9385,13584,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 302.47 33.7 BEACON\n(22534,32770,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 309.36 23.44 BEACON\n(12701,38145,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 299.79 24.22 BEACON\n(49353,43721,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 310.35 73.94 BEACON\n(23532,1906,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 301.56 65.34 BEACON\n(33537,39533,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 299.04 73.58 BEACON\n(33646,25253,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 296.08 80.09 BEACON\n(55732,41553,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 293.41 84.76 BEACON\n(63398,1207,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 288.86 73.89 BEACON\n(25150,47521,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 278.06 74.27 BEACON\n(51059,4548,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 266.13 74.73 BEACON\n(24940,10271,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 255.62 74.63 BEACON\n(23453,58075,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 245.41 74.58 BEACON\n(24451,48690,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 219.43 75.1 BEACON\n(28082,59191,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 232.63 74.84 BEACON\n(47879,36416,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 211.69 74.71 BEACON\n(14600,3364,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.56 74.83 BEACON\n(42400,26195,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.82 65.21 BEACON\n(5401,6275,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 201.11 78.38 BEACON\n(13676,48626,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 198.47 85.22 BEACON\n(52236,1708,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 194.84 74.82 BEACON\n(47782,62713,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 174.65 74.4 BEACON\n(8921,30860,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 165.27 74.39 BEACON\n(41992,132,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 155.03 74.51 BEACON\n(59883,21840,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 145.71 74.08 BEACON\n(52566,61097,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 136.49 74.31 BEACON\n(11286,2926,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 126.11 74.32 BEACON\n(4664,28948,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 116.87 74.55 BEACON\n(49208,33117,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 113.01 65.21 BEACON\n(52397,25104,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 106.19 79.56 BEACON\n(12341,59620,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 111.56 88.09 BEACON\n(41242,60177,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 184.61 74.36 BEACON\n(31877,41174,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 206.58 88.09 BEACON\n(62212,29708,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 110.93 41.24 BEACON\n(5642,18086,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 110.96 57.41 BEACON\n(42926,37532,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.62 41.25 BEACON\n(58529,2251,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 205.83 49.53 BEACON\n(42856,51925,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 203.56 57.36 BEACON\n(70,70,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 315.08 24.01 BEACON\n(445,445,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 341.97 23.6 BEACON\n(55659,16422,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 303.43 41.32 BEACON\n(27639,35916,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 300.55 49.46 BEACON\n(39909,40638,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 303.61 57.39 BEACON\n(476,476,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 315.11 74.56 BEACON\n(232,232,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 327.42 74.89 BEACON\n(382,382,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 340.54 75.07 BEACON\n(472,472,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 329 23.83 BEACON\n(160,160,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 61.2101 28.74 BEACON\n(193,193,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 64.7201 24.59 BEACON\n(325,325,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 75.7001 23.8 BEACON\n(309,309,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 85.0499 23.87 BEACON\n(113,113,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 94.5499 24.31 BEACON\n(7046,52633,F7826DA6-4FA2-4E98-8024-BC5B71E0893E) 113.33 49.83 BEACON\n" }, { "path": "standalone_algorithms/position_estimation/src/measurement_preprocessor.cpp", "content": "#include \n#include \n\n#include \"measurement_preprocessor.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nMeasurementsPreprocessor::MeasurementsPreprocessor(double sigAverageTimeSec,\n double sigWindowShiftSec,\n bool useWifi,\n bool useBle)\n : mCurrentTs ( -1 )\n , mSigAverageTimeSec ( sigAverageTimeSec )\n , mSigWindowShiftSec ( sigWindowShiftSec )\n , mUseWifi ( useWifi )\n , mUseBle ( useBle )\n{\n long long sigAverageTime = (long long)(1000 * mSigAverageTimeSec);\n long long sigWindowShift = (long long)(1000 * mSigWindowShiftSec);\n if (sigWindowShift == 0 || sigWindowShift > sigAverageTime)\n sigWindowShift = sigAverageTime;\n \n long long keepRadioTimeMs = sigAverageTime - sigWindowShift;\n mRadiosBuffer = RadioMeasurementBuffer(sigWindowShift, keepRadioTimeMs);\n}\n\nvoid MeasurementsPreprocessor::update(const RadioMeasurement& msr)\n{\n if (isSignalTypeSupported(msr.type) && isRssiValid(msr.rssi))\n {\n std::vector msrVect;\n msrVect.push_back(msr);\n mRadiosBuffer.addMeasurements(msr.ts, msrVect);\n }\n\n if (msr.ts > mCurrentTs)\n mCurrentTs = msr.ts;\n}\n\nstd::vector MeasurementsPreprocessor::extractRadioMeasurements()\n{\n std::vector radioMeasurements = mRadiosBuffer.extractMeasurements();\n if (mUseClosestAps)\n {\n std::sort(radioMeasurements.rbegin(), radioMeasurements.rend(),\n [](const RadioMeasurement& lhs, const RadioMeasurement& rhs)\n {\n return lhs.rssi < rhs.rssi;\n });\n\n if (radioMeasurements.size() > (unsigned int) mNumClosestAps)\n radioMeasurements.resize(mNumClosestAps);\n }\n\n return radioMeasurements;\n}\n\nlong long MeasurementsPreprocessor::getCurrentTs() const\n{\n return mCurrentTs;\n}\n\nbool MeasurementsPreprocessor::isSignalTypeSupported(RadioMeasurement::Type signalType) const\n{\n return (signalType == RadioMeasurement::Type::WIFI && mUseWifi) ||\n (signalType == RadioMeasurement::Type::BEACON && mUseBle);\n}\n\nbool MeasurementsPreprocessor::isRssiValid(double rssi) const\n{\n return rssi > mCutOffRssi && rssi < 0.0;\n}\n\nRadioMeasurementBuffer::RadioMeasurementBuffer(\n long long sigWindowShiftMs, \n long long radioKeepPeriodMs)\n{\n mRadioKeepPeriodMs = radioKeepPeriodMs;\n mSignalWindowShiftMs = sigWindowShiftMs;\n}\n\nvoid RadioMeasurementBuffer::addMeasurements(long long messageTs, const std::vector& msrs)\n{\n if (mCurrentTs == -1)\n {\n mCurrentTs = messageTs;\n mLastExtractionTs = messageTs;\n }\n\n for (const RadioMeasurement& msr : msrs)\n {\n RadioMeasurement radioMsr = msr;\n radioMsr.rssi = msr.rssi;\n\n // check if it is a fresh measurement\n if (radioMsr.ts > mLastExtractionTs)\n mMeasurements.push_back(radioMsr);\n\n mCurrentTs = std::max(radioMsr.ts, mCurrentTs);\n }\n}\n\nstd::vector RadioMeasurementBuffer::extractMeasurements()\n{\n std::vector measurements;\n if (mCurrentTs - mLastExtractionTs < mSignalWindowShiftMs)\n {\n // if not enough time passed\n return measurements;\n }\n\n mLastExtractionTs = mCurrentTs;\n\n // Averaging measurements from same Transmitters\n std::sort(mMeasurements.begin(), mMeasurements.end(),\n [](const RadioMeasurement& lhs, const RadioMeasurement& rhs)\n {\n return lhs.id < rhs.id;\n });\n\n auto sameIdFirst = mMeasurements.cbegin();\n while (sameIdFirst != mMeasurements.cend())\n {\n auto sameIdLast = std::upper_bound(sameIdFirst, mMeasurements.cend(), *sameIdFirst,\n [](const RadioMeasurement& lhs, const RadioMeasurement& rhs)\n {\n return lhs.id < rhs.id;\n });\n\n double nSignals = std::distance (sameIdFirst, sameIdLast);\n double sumRssi = std::accumulate (sameIdFirst, sameIdLast, 0.0,\n [](double sum, const RadioMeasurement& m)\n {\n return sum + m.rssi;\n });\n\n double averRssi = sumRssi / nSignals;\n\n double sumDistances = std::accumulate (sameIdFirst, sameIdLast, 0.0,\n [](double sum, const RadioMeasurement& m)\n {\n return sum + m.distance;\n });\n\n double averDistance = sumDistances / nSignals;\n\n RadioMeasurement radioMsr = *sameIdFirst;\n radioMsr.rssi = averRssi;\n radioMsr.distance = averDistance;\n measurements.push_back(radioMsr);\n sameIdFirst = sameIdLast;\n }\n\n // Erasing old measurements\n if (mRadioKeepPeriodMs == 0)\n {\n mMeasurements.clear();\n }\n else\n {\n long long keepFromTs = mCurrentTs - mRadioKeepPeriodMs;\n\n std::sort(mMeasurements.begin(), mMeasurements.end(),\n [](const RadioMeasurement& lhs, const RadioMeasurement& rhs)\n {\n return lhs.ts < rhs.ts;\n });\n\n auto eraseUpperBound = std::find_if(mMeasurements.begin(),\n mMeasurements.end(),\n [keepFromTs](const RadioMeasurement& rm)\n {\n return rm.ts >= keepFromTs;\n });\n\n mMeasurements.erase(mMeasurements.begin(), eraseUpperBound);\n }\n\n return measurements;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/position_estimation/src/nearest_transmitter_estimator.cpp", "content": "#include \n#include \n#include \"nearest_transmitter_estimator.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double A = -82.0;\nstatic const double B = 3.0;\n\nNearestTransmitterPositionEstimator::NearestTransmitterPositionEstimator(const std::vector& transmitters)\n{\n m_smoother = PositionSmoother(0.9);\n for (const Transmitter& t: transmitters)\n m_transmitters[t.id] = t;\n}\n\nstd::vector NearestTransmitterPositionEstimator::getRegisteredTransmittersMeasurements(\n const std::vector& radioMsr)\n{\n std::vector registeredMeasurements;\n for (const RadioMeasurement& msr: radioMsr)\n if (m_transmitters.find(msr.id) != m_transmitters.end())\n registeredMeasurements.push_back(msr);\n return registeredMeasurements;\n}\n\nPosition NearestTransmitterPositionEstimator::calculatePosition(const RadioMeasurements& inputMeasurements)\n{\n Position position;\n position.isEmpty = true;\n std::vector radioMeasurements = getRegisteredTransmittersMeasurements(inputMeasurements);\n if (radioMeasurements.empty())\n return position;\n\n auto nearestTx = std::max_element(radioMeasurements.begin(), radioMeasurements.end(),\n [](RadioMeasurement msr1, RadioMeasurement msr2) {return msr1.rssi < msr2.rssi; });\n\n std::string nearestTxId = nearestTx->id;\n double nearestTxRssi = nearestTx->rssi;\n if (m_transmitters.find(nearestTxId) == m_transmitters.end())\n return position;\n else\n {\n const Transmitter& t= m_transmitters[nearestTxId];\n position.x = t.x;\n position.y = t.y;\n position.precision = std::sqrt(std::exp((A - nearestTxRssi) / B)) + 1.0;\n position.isEmpty = false;\n position.ts = radioMeasurements.back().ts;\n return m_smoother.smoothPosition(position);\n }\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/position_estimation/src/position_smoother.cpp", "content": "#include \"position_smoother.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double MAX_ALPHA = 0.9;\nstatic const double MIN_ALPHA = 0.1;\nstatic const double TIME_THRESHOLD_SEC = 10;\n\nPositionSmoother::PositionSmoother(double smoothing)\n : mWasCalled ( false )\n , mSpeedX ( 0.0 )\n , mSpeedY ( 0.0 )\n{\n smoothing = std::min(1.0, std::max(0.0, smoothing));\n mAlpha = MIN_ALPHA * smoothing + MAX_ALPHA * (1.0 - smoothing);\n}\n\nPosition PositionSmoother::smoothPosition(Position pos)\n{\n if (pos.ts > mPosition.ts)\n {\n double t = (pos.ts - mPosition.ts) / 1000.0;\n double a = mAlpha;\n double b = a * a / (2.0 - a);\n\n double xp = mPosition.x + mSpeedX * t;\n double vxp = mSpeedX + (b / t) * (pos.x - xp);\n double xs = xp + a * (pos.x - xp);\n\n double yp = mPosition.y + mSpeedY * t;\n double vyp = mSpeedY + (b / t) * (pos.y - yp);\n double ys = yp + a * (pos.y - yp);\n\n bool timeIsTooLong = t > TIME_THRESHOLD_SEC;\n\n if (!mWasCalled || timeIsTooLong)\n {\n mPosition = pos;\n mSpeedX = 0.0;\n mSpeedY = 0.0;\n mWasCalled = true;\n return pos;\n }\n\n mSpeedX = vxp;\n mSpeedY = vyp;\n\n pos.x = xs;\n pos.y = ys;\n\n mPosition = pos;\n }\n\n return mPosition;\n}\n\n} } // namespace navigine::navigation_core\n" }, { "path": "standalone_algorithms/trilateteration/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.7)\nproject(Trilateration)\n\nset(\"test-target\" \"test\")\n\nfile(GLOB ${PROJECT_NAME}_TEST_SOURCES src/*.cpp)\nadd_executable(\"${test-target}\" ${${PROJECT_NAME}_TEST_SOURCES})\ntarget_include_directories(\"${test-target}\" PRIVATE \"include/\")\n\nset_property(TARGET \"${test-target}\" PROPERTY CXX_STANDARD 17)\nset_target_properties(\"${test-target}\" PROPERTIES COMPILE_DEFINITIONS \"BUILD_FOR_LINUX;_DEBUG;\")\nset_target_properties(\"${test-target}\" PROPERTIES COMPILE_FLAGS \"-fno-strict-aliasing -funwind-tables -fPIC -m64 -march=x86-64 -fstack-protector-strong -O3 -fno-omit-frame-pointer -g2 -fvisibility-inlines-hidden -std=c++17 -frtti -fexceptions \")\ntarget_link_libraries(\"${test-target}\")\n" }, { "path": "standalone_algorithms/trilateteration/README.md", "content": "### Trilateration\n\nThis is a method of determining coordinates of a device using measurements of pseudo distances to all visible beacons or signal transmitters. For navigation purposes you may also operate with RSSI readings. We use this algorithm for indoor navigation along with the distance/RSSI measurements from Bluetooth LE 4.0 Beacons.\n\nAlgorithm with all required functions and data structures is presented as class in trilateration.h, beacon.h files.\n\nExample of the algorithm usage and filling of data structures you can find in a unit test for this source code in the test_trilateration.cpp file.\n\n### Algorithm\n\nMost indoor positioning are implemented based on Received Signal Strength Index (RSSI). Mobile device receives signal from beacon with known location. The distance between mobile device and beacon is calculated using RSSI of received signal.\n\nThe algorithm consists of several phases:\n\n1. Preprocessing\n2. Trilateration\n3. Triangulation\n4. Least squares estimation\n\nPreprocessing includes deleting duplicated measurements and filtering unknowing beacons.\n\n### Triangulation\n\nTriangulation is the process of determining the location of a point by forming triangles to the point from known points. Once mobile device knows distance from three known beacon, triangulation is performed to determine its coordinates. In the figure below there are three circles, centered at each beacon with radius equals to the distance between each beacon and mobile device. The triangulation location is the centroid of the triangle ABC, which consists of cords of the intersection part of three circles.\n\n\"Markdown\n\nHowever if we get all distances between the mobile and reference nodes, they are not likely intersecting into one point. Therefore we need some algorithms to calculate the estimated location of the mobile.\n\n### Least Squares Estimation\n\nLSE is widely used in the distance based positioning systems. Equation on the mobile location can be set as\n\n

\n\nwhere Y is a known n-dimensional vector, A is NxM matrix. If N>M, the number of equation is greater than the\nnumber of unknown numbers, we can obtain the optimal X using LSE. The idea of LSE is to make the least value of sum\nof square of error\n\n

\n\nWe get the minimum of the above function, if is nonsingular\n\n

\n\n### Trilateration Algorithm\n\nTrilateration is the geometrical problem of determining an unknown position on a plane based on the distance to other two known vertices of a triangle (the length of two sides).\nPositioning by ranging is the determination of an object’s position by measuring the range to a number of objects at known locations. It is also known as lateration with trilateration denoting ranging using three signals and\nmultilateration ranging using more than three signals.\n\n### Build\n\n```sh\ncd /standalone_algorithms/trilateration\ncmake -Bbuild -H.\ncmake --build build\n```\n\nRun tests:\n\n```sh\n./build/test\n```" }, { "path": "standalone_algorithms/trilateteration/src/beacon.cpp", "content": "/** beacon.cpp\n *\n * Author: Aleksei Smirnov \n * Copyright (c) 2014 Navigine. All rights reserved.\n *\n */\n\n#include \"beacon.h\"\n\n#include \n\nusing namespace std;\n\nBeacon::Beacon()\n: mX(0.)\n, mY(0.)\n, mLocationId(0)\n{\n\n}\n\n\n//copy constructor\nBeacon::Beacon(const Beacon& beacon)\n: mX (beacon.mX)\n, mY (beacon.mY)\n, mId (beacon.mId)\n, mName (beacon.mName)\n, mLocationName (beacon.mLocationName)\n, mLocationId (beacon.mLocationId)\n{\n\n}\n\n\nvoid Beacon::setBeaconId(const string& beaconId)\n{\n Beacon::mId = beaconId;\n}\n\n\nvoid Beacon::fillData(const double x,\n /*planar coordinate: x */ const double y,\n /*planar coordinate: y */ const std::string& id,\n /*beacon identifier */ const std::string& name,\n /*beacon name */ const std::string& locName)\n{\n Beacon::mX = x;\n Beacon::mY = y;\n Beacon::mId = id;\n Beacon::mName = name;\n Beacon::mLocationName = locName;\n}\n\n\nconst char* Beacon::getId () const\n{\n return mId.c_str();\n}\n\n\ndouble Beacon::getX() const\n{\n return Beacon::mX;\n}\n\n\ndouble Beacon::getY( ) const\n{\n return Beacon::mY;\n}\n\n\nvoid Beacon::setX( const double x )\n{\n Beacon::mX = x;\n}\n\n\nvoid Beacon::setY( const double y )\n{\n Beacon::mY = y;\n}\n\n\nvoid Beacon::setLocationName( const char* name )\n{\n Beacon::mLocationName += name;\n}\n\n\nvoid Beacon::setLocationId( int sublocId )\n{\n Beacon::mLocationId = sublocId;\n}\n\n\nint Beacon::getLocationId()\n{\n return Beacon::mLocationId;\n}\n\n\nbool Beacon::operator==(const Beacon& entry)const\n{\n return strcmp(getId(), entry.getId()) == 0;\n}\n\n\nbool Beacon::operator!=(const Beacon& entry)const\n{\n return !(*this == entry);\n}\n\n\nIBeacon::IBeacon()\n: Beacon()\n, major(0)\n, minor(0)\n{\n\n}\n\n\n//copy constructor\nIBeacon::IBeacon(const IBeacon& iBeacon)\n: Beacon(iBeacon)\n, uuid(iBeacon.uuid)\n, major(iBeacon.major)\n, minor(iBeacon.minor)\n{\n \n}\n\n\nconst char* IBeacon::getUuid ()const\n{\n return uuid.c_str();\n}\n\n\nint IBeacon::getMajor()const\n{\n return major;\n}\n\n\nint IBeacon::getMinor()const\n{\n return minor;\n}\n\n\nvoid IBeacon::setMajor( const unsigned int major )\n{\n IBeacon::major = major;\n}\n\n\nvoid IBeacon::setMinor( const unsigned int minor )\n{\n IBeacon::minor = minor;\n}\n\n\nvoid IBeacon::setUuid( const char * uuid )\n{\n IBeacon::uuid = uuid;\n}\n\n\n// constructor\nBeaconMeas::BeaconMeas()\n: mBeaconPtr (0)\n, mRssi (TRANSMITTER_POINT_UNUSED)\n{\n\n}\n\n\n//constructor\nBeaconMeas::BeaconMeas( Beacon* beacon,\n double rssi,\n double distance )\n{\n BeaconMeas::mBeaconPtr = beacon;\n BeaconMeas::mRssi = rssi;\n BeaconMeas::mDistance = distance;\n mBeaconId = beacon->getId(); \n}\n\n\n//copy constructor\nBeaconMeas::BeaconMeas(const BeaconMeas& beaconMeas)\n: mBeaconId (beaconMeas.mBeaconId)\n, mBeaconPtr (beaconMeas.mBeaconPtr)\n, mRssi (beaconMeas.mRssi)\n, mDistance (beaconMeas.mDistance)\n{\n\n}\n\n\nBeaconMeas::~BeaconMeas()\n{\n mBeaconPtr = 0;\n}\n\n\ndouble BeaconMeas::getRssi()const\n{\n return mRssi;\n}\n\n\ndouble BeaconMeas::getDistance()const\n{\n return mDistance;\n}\n\n\nvoid BeaconMeas::setRssi(const double rssi)\n{\n BeaconMeas::mRssi = rssi;\n}\n\n\nvoid BeaconMeas::setDistance(const double distance)\n{\n BeaconMeas::mDistance = distance;\n}\n\n\nbool BeaconMeas::operator<(const BeaconMeas& entry)const\n{\n return mDistance < entry.mDistance;\n}\n\n\nbool BeaconMeas::operator>(const BeaconMeas& entry)const\n{\n return entry < *this;\n}\n\n\nbool BeaconMeas::operator==(const BeaconMeas& entry)const\n{\n if (getBeaconId() == 0 || entry.getBeaconId() == 0)\n {\n printf (\"ERROR: id of measurement == NULL!\\n\");\n }\n if ( *getBeaconId() == '\\0' || *entry.getBeaconId() == '\\0')\n printf(\"ERROR: id is empty\\n\");\n\n return strcmp( getBeaconId(), entry.getBeaconId() ) == 0;\n}\n\n\nbool BeaconMeas::operator!=(const BeaconMeas& entry)const\n{\n return !(*this == entry);\n}\n\n\n// specify Ptr that correspond to beacon from wich we got *this measurement\nvoid BeaconMeas::setBeaconPtr( const Beacon* beaconPtr )\n{\n BeaconMeas::mBeaconPtr = const_cast (beaconPtr);\n}\n\n\nBeacon* BeaconMeas::getBeaconPtr() const\n{\n if (BeaconMeas::mBeaconPtr == NULL)\n printf( \"beaconId = %s : mBeaconPtr == NULL\\n\", BeaconMeas::getBeaconId() );\n //throw exception here\n\n return BeaconMeas::mBeaconPtr;\n}\n\n\nvoid BeaconMeas::setBeaconId( const string& beaconId )\n{\n BeaconMeas::mBeaconId = beaconId;\n}\n\n\nconst char* BeaconMeas::getBeaconId() const\n{\n return mBeaconId.c_str();\n}\n\n" }, { "path": "standalone_algorithms/trilateteration/src/beacon.h", "content": "/** beacon.h\n *\n * Author: Aleksei Smirnov \n * Copyright (c) 2014 Navigine. All rights reserved.\n *\n */\n\n#ifndef NAVIGINE_BEACON_CLASS\n#define NAVIGINE_BEACON_CLASS\n\n#include \n#include \n\nstatic const int TRANSMITTER_POINT_UNUSED = 1;\n\n\n// Beacon is transmitter emitting the signal, that we can use to obtain pseudoDistances\n// and process trilateration navigation algorithms\nclass Beacon\n{\n public:\n Beacon();\n Beacon(const Beacon&);\n ~Beacon() {};\n\n //specify unique beacon identifier that allows us to distinguish beacon \n // and its measurements\n void setBeaconId(const std::string& beaconId);\n\n // func allows simultaneously fill the data: \n void fillData(const double x, //planar coordinate: x \n const double y, //planar coordinate: y \n const std::string& id, //beacon identifier \n const std::string& name, //beacon name \n const std::string& locName); //name of location\n\n const char * getId() const;\n double getX() const;\n double getY() const;\n \n // specify x,y, beacon planar coordinates\n void setX( const double x );\n void setY( const double y );\n\n // specify x,y, beacon planar coordinates \n void setLocationName( const char* name);\n void setLocationId( int id );\n\n int getLocationId();\n\n bool operator==(const Beacon& beacon)const;\n bool operator!=(const Beacon& beacon)const;\n \n private:\n double mX, mY;\n \n std::string mId; // id of the beacon (e.g. major+minor+uuid)\n std::string mName; // name of the beacon\n std::string mLocationName; // name of sublocation\n \n int mLocationId; // sublocation id\n};\n\n\n// IBeacon is a beacon, invented by apple, that has additional fields:\n// Major,Minor, UUID identifiers\nclass IBeacon : public Beacon\n{\n public:\n IBeacon();\n IBeacon(const IBeacon&);\n \n const char* getUuid() const;\n void setUuid( const char * uuid );\n \n int getMajor() const;\n int getMinor() const;\n\n void setMajor( const unsigned int major );\n void setMinor( const unsigned int minor );\n private:\n std::string uuid;\n unsigned int major;\n unsigned int minor;\n};\n\n\n// class that keep measurements from certain transmitter (e.g. beacon)\nclass BeaconMeas\n{\n public:\n\n BeaconMeas();\n BeaconMeas( Beacon* beacon, double rssi, double distance );\n ~BeaconMeas();\n \n // copy constructor\n BeaconMeas( const BeaconMeas& );\n\n double getRssi()const; //get RSSI of current measurement\n double getDistance()const; //get distance of current measurement\n const char* getBeaconId()const; //get beacon identifier\n Beacon* getBeaconPtr()const; //get pointer to beacon\n\n void setRssi( const double ); //set measurement RSSI\n void setDistance( const double ); //set measurement distance\n void setBeaconPtr( const Beacon* beaconPtr ); //set pointer to beacon (from which we got meas)\n void setBeaconId( const std::string& beaconId ); //set beacon it (that identifies measurement)\n\n bool operator<(const BeaconMeas& entry)const;\n bool operator>(const BeaconMeas& entry)const;\n bool operator==(const BeaconMeas& entry)const;\n bool operator!=(const BeaconMeas& entry)const;\n\n private:\n std::string mBeaconId; // id that allow us identify beacon and set mBeaconPtr\n Beacon* mBeaconPtr; // pointer to beacon from which we got measurement\n double mRssi; // RSSI of the measurement\n double mDistance; // Distance to the beacon\n};\n\n#endif // NAVIGINE_BEACON_CLASS\n" }, { "path": "standalone_algorithms/trilateteration/src/test_trilateration.cpp", "content": "/** test_trilateration.cpp\n *\n * Author: Aleksei Smirnov \n * Copyright (c) 2014 Navigine. All rights reserved.\n *\n */\n\n#include \"test_trilateration.h\"\n#include \n\nvoid PrintHelp()\n{\n printf( \"This is unit test that demonstrates how you can use \\n\"\n \"Navigine trilateration class in order to get planar coordinates \\n\"\n \"of the user on the basis of distance measurements \\n\"\n \"For more questions please contact aleksei.smirnov@navigine.com \\n\\n\"\n );\n}\n\n\nint main( void )\n{\n PrintHelp();\n\n int sublocationIndex = 0;\n //fill map beacons and beaconEntries\n Trilateration trilateration;\n \n //fill beacon measurements depending from scenario\n std::vector beaconMeasurements;\n //fill mapBeacons entries depending from scenario\n std::vector mapBeacons;\n\n Beacon beacon;\n \n //test scenario. Set 3 beacons on the map\n beacon.fillData( 54.69, 29.51, \"(53580,22667,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)\",\n \"beacon1\", \"floor13\" );\n mapBeacons.push_back( beacon );\n\n beacon.fillData( 54.68, 29.51, \"(49599,56896,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)\",\n \"beacon2\", \"floor13\" );\n mapBeacons.push_back( beacon );\n\n beacon.fillData( 49.05, 32.16, \"(57506,19633,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)\",\n \"beacon3\", \"floor13\" );\n mapBeacons.push_back( beacon );\n\n\n //specify beacon Measurements\n BeaconMeas measurement;\n\n measurement.setBeaconId( \"(53580,22667,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)\" );\n measurement.setRssi( -86.57 );\n measurement.setDistance( 4.47 );\n beaconMeasurements.push_back( measurement );\n\n\n measurement.setBeaconId( \"(49599,56896,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)\" );\n measurement.setRssi( -90.80 );\n measurement.setDistance( 14.13 );\n beaconMeasurements.push_back( measurement );\n\n\n measurement.setBeaconId( \"(57506,19633,F7826DA6-4FA2-4E98-8024-BC5B71E0893E)\");\n measurement.setRssi( -86.41 );\n measurement.setDistance( 15.85 );\n beaconMeasurements.push_back( measurement );\n\n //measurement from unknown beacon\n measurement.setBeaconId( \"22211,00231\" );\n measurement.setRssi( -21.41 );\n measurement.setDistance( 15.85 );\n beaconMeasurements.push_back( measurement );\n\n\n if (beaconMeasurements.size() == 0)\n {\n printf( \"Number of measurements == 0\\n\" );\n return 0;\n }\n\n trilateration.updateMeasurements( beaconMeasurements );\n\n trilateration.setCurrentLocationId( 0 );\n\n trilateration.fillLocationBeacons(mapBeacons );\n\n int errorCode = trilateration.calculateCoordinates();\n \n\n double x = trilateration.getX();\n double y = trilateration.getY();\n printf( \"x = %lf y = %lf \\n\", trilateration.getX(), trilateration.getY() );\n\n if (x != 53.692 && y != 29.977 && errorCode)\n {\n printf( \"test not passed !\\n check whether you changed \"\n \"something in code\\n\");\n }\n else\n {\n printf(\"test passed!\\n\");\n }\n\n if (errorCode)\n return errorCode;\n\n // std::cin.get();\n return 0;\n}\n\n\n\n" }, { "path": "standalone_algorithms/trilateteration/src/test_trilateration.h", "content": "/** test_trilateration.h\n *\n * Author: Aleksei Smirnov \n * Copyright (c) 2014 Navigine. All rights reserved.\n *\n */\n\n#ifndef NAVIGINE_TEST_TRILATETARATION\n#define NAVIGINE_TEST_TRILATETARATION\n\n\n#include \n\n#include \"beacon.h\"\n#include \"trilateration.h\"\n\n#endif\n\n\n" }, { "path": "standalone_algorithms/trilateteration/src/trilateration.cpp", "content": " /** trilateration.cpp\n *\n * Author: Aleksei Smirnov \n * Copyright (c) 2014 Navigine. All rights reserved.\n *\n */\n\n#if defined WIN32 || defined _WIN32\n#define _CRT_SECURE_NO_WARNINGS\n#endif\n\n#include \n\n#include \"trilateration.h\"\n\nusing namespace std;\n\n\n\nTrilateration::Trilateration()\n{\n mXY.assign( 2, 0.0 );\n mCurLocationId = 0;\n}\n\nTrilateration::Trilateration( const Trilateration& trilat )\n : mLocationBeacons( trilat.mLocationBeacons )\n , mBeaconMeas ( trilat.mBeaconMeas )\n , mXY ( trilat.mXY )\n , mCurLocationId ( trilat.mCurLocationId )\n , mErrorMessage ( trilat.mErrorMessage )\n{\n\n}\n\n\nTrilateration::~Trilateration()\n{\n\n}\n\n\nvoid Trilateration::updateMeasurements( std::vector < BeaconMeas >& beaconMeas )\n{\n Trilateration::mBeaconMeas = beaconMeas;\n}\n\n\nvoid Trilateration::fillLocationBeacons( std::vector & beaconsOnFloor )\n{\n Trilateration::mLocationBeacons = beaconsOnFloor;\n}\n\n\nint Trilateration::calculateCoordinates()\n{\n int errorCode = deleteDuplicateMeasurements( Trilateration::mBeaconMeas );\n\n if (errorCode)\n return errorCode;\n\n // sort into ascending order (from -100 to 0)\n std::sort( mBeaconMeas.begin(), mBeaconMeas.end() );\n\n //filter out beacons that are not in the map\n filterUnknownBeacons (mBeaconMeas, mLocationBeacons);\n\n if (mBeaconMeas.size () == 0 )\n {\n printf(\"The number of visible beacon = %ld \\n\",\n mBeaconMeas.size ());\n return ERROR_NO_SOLUTION_TRILATERATION;\n }\n\n // number of measurements that we'll use to obtain coordinates\n unsigned int nOfMeas = (mBeaconMeas.size() >= 3 ? 3 : mBeaconMeas.size());\n \n std::vector mostStrongMeas;\n if (mBeaconMeas.size() >= 3)\n {\n for (unsigned int i = 0; i < nOfMeas; i++)\n mostStrongMeas.push_back(mBeaconMeas.at(i));\n }\n\n errorCode = calculateTrilaterationCoordinates();\n if (errorCode)\n return errorCode;\n\n const int UseOls = 0;\n if (UseOls)\n {\n getLeastSquaresCoordinates();\n getLeastSquaresCoordinates();\n }\n\n#ifdef TRILATERATION_DEBUG_\n printXyToFile (\"trilateration_xy_debug.txt\");\n printf (\"x = %lf \\n y = %lf \\n\",mXY[0],mXY[1]);\n#endif\n return 0;\n}\n\n\nint Trilateration::getCurrentLocationId() const\n{\n return Trilateration::mCurLocationId;\n}\n\n\nvoid Trilateration::setCurrentLocationId( int curSubloc )\n{\n Trilateration::mCurLocationId = curSubloc;\n}\n\n\n// this function erase unknown transmitters that don't presence in a map\n// and fill x,y, coordinates of beacons\nvoid Trilateration::filterUnknownBeacons( vector & beaconMeas,\n const vector& mapBeacons )\n{\n // don't increment iterator in for 'cause there is erase inside loop.\n // otherwise we'll skip 1 element after erased\n for (vector ::iterator it = beaconMeas.begin();\n it != beaconMeas.end(); )\n {\n std::vector::const_iterator itBeacon;\n itBeacon = findBeaconForMeas( mapBeacons,\n it->getBeaconId() );\n\n if (itBeacon == mapBeacons.end())\n {\n it = beaconMeas.erase( it );\n if (it == beaconMeas.end())\n break;\n }\n else\n {\n it->setBeaconPtr( &(*itBeacon) );\n ++it;\n }\n\n }\n return;\n}\n\n\ndouble Trilateration::getX() const\n{\n return Trilateration::mXY[ 0 ];\n}\n\n\ndouble Trilateration::getY() const\n{\n return Trilateration::mXY[ 1 ];\n}\n\n\nconst char* Trilateration::getErrorMessage() const\n{\n return mErrorMessage.c_str();\n}\n\n\nvoid Trilateration::printXyToFile( char* filename ) const\n{\n FILE *f;\n f = fopen( filename, \"w\" );\n fprintf( f, \"%lf %lf \\n\", mXY[ 0 ], mXY[ 1 ] );\n fclose( f );\n}\n\n\nint Trilateration::calculateTrilaterationCoordinates()\n{\n double normalizeCoefficient = 0.0;\n //take revert values, because lower is the distance, greater the weight gets\n for (unsigned int i = 0; i < mBeaconMeas.size(); i++)\n normalizeCoefficient += 1.0 / fabs( mBeaconMeas[ i ].getDistance() );\n\n vector weight( mBeaconMeas.size(), 0.0 );\n\n for (unsigned int i = 0; i < mBeaconMeas.size(); i++)\n {\n if (mBeaconMeas[i].getBeaconPtr() == 0)\n {\n setErrorMessage(\"ERROR: BeaconMes it =%s : Beacon ptr == NULL\\n\");\n printf( \"%s\\n\", getErrorMessage() );\n return ERROR_IN_TRILATER;\n }\n // calculate probability of being at beacons x,y coordinates\n weight[ i ] += 1.0 / (fabs( mBeaconMeas[ i ].getDistance() *\n normalizeCoefficient ));\n\n double beaconX = 0, beaconY = 0.;\n beaconX = mBeaconMeas[ i ].getBeaconPtr()->getX();\n beaconY = mBeaconMeas[ i ].getBeaconPtr()->getY();\n\n //find final coordinates according to probability\n mXY[ 0 ] += weight[ i ] * beaconX;\n mXY[ 1 ] += weight[ i ] * beaconY;\n }\n return 0;\n}\n\n\n// this function deletes Duplicate beacon measurements and averages rssi signals\n// from equal beacons\nint Trilateration::deleteDuplicateMeasurements( vector& beaconMeas )\n{\n //sort measurements in beacon name order\n std::sort( beaconMeas.begin(), beaconMeas.end(), compareBeaconMeasByName );\n //for (int i = 0; i < beaconEntry.size(); i++)\n for (std::vector::iterator itBeaconMeas = beaconMeas.begin();\n itBeaconMeas != beaconMeas.end(); ++itBeaconMeas)\n {\n //count number of occurrences of itBeaconMeas\n std::vector::iterator it = beaconMeas.begin();\n\n //find first occurrence\n std::count( beaconMeas.begin(), beaconMeas.end(), *itBeaconMeas);\n it = std::find( it, beaconMeas.end(), *itBeaconMeas );\n \n int nOfMeasFromSameAp = 0;\n double rssi = 0.0, distance = 0.0;\n\n //find all similar entries\n while (it != beaconMeas.end())\n {\n nOfMeasFromSameAp++;\n\n if ( it->getDistance() < 0)\n {\n printf( \"beacon id = %s distance = %lf < 0\\n\", \n it->getBeaconId(), it->getDistance() );\n \n return ERROR_IN_TRILATER;\n }\n\n //calc sum to get average\n rssi += it->getRssi();\n distance += it->getDistance();\n \n //clear duplicate entries\n // we don't clear first occurrence!\n if (nOfMeasFromSameAp != 1)\n it = beaconMeas.erase( it );\n else it++;\n \n if (it == beaconMeas.end())\n break;\n it = std::find( it, beaconMeas.end(), *itBeaconMeas );\n }\n \n if (nOfMeasFromSameAp == 0)\n {\n setErrorMessage(\"number of measurements from the same AP == 0 ! something wrong!\\n\");\n printf( \"%s\\n\", getErrorMessage() );\n return ERROR_IN_TRILATER;\n }\n \n //set average rssi to the beacon that doesn't have duplicates now\n rssi /= (std::max)(nOfMeasFromSameAp, 1);\n distance /= (std::max)(nOfMeasFromSameAp, 1);\n \n itBeaconMeas->setRssi( rssi );\n }\n\n if (beaconMeas.size() < 3)\n {\n setErrorMessage( \"less then 3 visible beacons in measurements \"\n \"It's not enough for trilateration\\n\" );\n printf( \"%s\\n\", getErrorMessage() );\n return ERROR_NO_SOLUTION_TRILATERATION;\n }\n\n return 0;\n}\n\n\nvoid Trilateration::getLinearSystem( vector &matrixA,\n vector &b,\n int dim )\n{\n int nOfVisibleBeacons = mBeaconMeas.size ();\n \n BeaconMeas firstBeacon = mBeaconMeas.front ();\n double xFirstBeacon = 0, yFirstBeacon = 0;\n xFirstBeacon = mBeaconMeas[ 0 ].getBeaconPtr()->getX();\n yFirstBeacon = mBeaconMeas[ 0 ].getBeaconPtr()->getY();\n\n double firstBeaconDistance = mBeaconMeas.front().getDistance();\n double normaFirstBeacon = xFirstBeacon * xFirstBeacon +\n yFirstBeacon * yFirstBeacon;\n\n for ( int i = 0; i < nOfVisibleBeacons - 1; i++ )\n {\n // fill the matrix A and right part of linear system b\n double x = 0.0, y = 0.0;\n x = mBeaconMeas[ i + 1 ].getBeaconPtr()->getX();\n y = mBeaconMeas[ i + 1 ].getBeaconPtr()->getY();\n\n double distance = mBeaconMeas[ i + 1].getDistance();\n \n matrixA[ i * dim ] = 2 * (x - xFirstBeacon);\n matrixA[ i * dim + 1 ] = 2 * ( y - yFirstBeacon );\n\n double norma = x * x + y * y;\n b[ i ] = firstBeaconDistance * firstBeaconDistance - distance * distance -\n normaFirstBeacon + norma;\n }\n\n return;\n}\n\n\n// This function estimate x,y coordinates by triangulation algorithm\n// using ordinary least squares method for solving linear system\nvoid Trilateration::getLeastSquaresCoordinates()\n{\n // How many coordinates do we consider? In planar case it equal to 2\n int dim = 2;\n int nOfVisibleBeacons = mBeaconMeas.size();\n // create matrix for triangulation linear system, that we will solve\n // for obtaining the coordinates we need at least dim + 1 beacons\n // index [i][j] = i * dim + j\n\n // By subtracting the last equation from each other we bring our \n // nonlinear system to linear matrixA\n \n vector matrixA((nOfVisibleBeacons - 1) * dim, 0.0);\n vector b(nOfVisibleBeacons - 1, 0.0 );\n\n getLinearSystem(matrixA, b, dim);\n solveLinearSystem (matrixA, b);\n}\n\n\n// this function solve overdetermined linear system \n// by ordinary least squares method x = A_ * B\n// where A_ - pseudo inverse matrix\nvoid Trilateration::solveLinearSystem( vector matrixA,\n vector b )\n{\n int nOfEquations = b.size();\n int dim = matrixA.size() / nOfEquations;\n\n vector xy(dim, 0.);\n vector aTransposeA(dim * dim, 0.);\n \n // find pseudoInverseMatrix\n for (int row = 0; row < dim; row++)\n {\n for (int col = 0; col < dim; col++)\n {\n for ( int inner = 0; inner < nOfEquations; inner++ ) \n {\n // Multiply the row of A_transpose by the column of A \n // to get the row, column of multiplyAATranspose.\n aTransposeA[ row * dim + col ] +=\n matrixA[ inner * dim + row ] * matrixA[ inner * dim + col ];\n }\n }\n }\n\n vector revertMatrix( dim * dim, 0. );\n double det = aTransposeA[ 0 ] * aTransposeA[ 3 ] -\n aTransposeA[ 2 ] * aTransposeA[ 1 ];\n\n //simple formula for invert matrix 2x2\n revertMatrix[ 0 ] = aTransposeA[ 3 ] / det;\n revertMatrix[ 1 ] = -aTransposeA[ 1 ] / det;\n revertMatrix[ 2 ] = - aTransposeA[2] / det;\n revertMatrix[ 3 ] = aTransposeA[0] / det;\n\n //Multiply revertMatrix on A transpose\n vector matrix2xN (dim * nOfEquations, 0.0);\n for ( int row = 0; row < dim; row++ )\n {\n for ( int col = 0; col < nOfEquations; col++ )\n {\n for ( int inner = 0; inner < dim; inner++ )\n {\n // Multiply the row of A_transpose by the column of A \n // to get the row, column of multiplyAATranspose.\n matrix2xN[ row * nOfEquations + col ] +=\n revertMatrix[ row * dim + inner] * matrixA[ col * dim + inner ];\n }\n }\n }\n\n //Multiply matrix2xN on B vector \n for ( int col = 0; col < dim; col++ )\n {\n for ( int inner = 0; inner < nOfEquations; inner++ )\n {\n xy[col] += matrix2xN[col * nOfEquations + inner] * b[inner];\n }\n }\n return;\n}\n\n\nvoid Trilateration::setErrorMessage(const char* errorMsg )\n{\n Trilateration::mErrorMessage += errorMsg;\n}\n\n\nbool compareBeaconMeasByName( BeaconMeas first, BeaconMeas second )\n{\n return strcmp( first.getBeaconId(), second.getBeaconId() ) < 0;\n}\n\n\nstd::vector::const_iterator findBeaconForMeas( const vector& mapBeacons,\n const string& measureBeaconId )\n{\n std::vector::const_iterator it;\n for (it = mapBeacons.begin(); it != mapBeacons.end(); ++it)\n {\n if (it->getId() == measureBeaconId)\n return it;\n }\n return it;\n}\n\n" }, { "path": "standalone_algorithms/trilateteration/src/trilateration.h", "content": "/** trilateration.h\n *\n * Author: Aleksei Smirnov \n * Copyright (c) 2014 Navigine. All rights reserved.\n *\n */\n\n#ifndef NAVIGINE_TRILATERATION_ALGORITHM\n#define NAVIGINE_TRILATERATION_ALGORITHM\n\nstatic const int ERROR_NO_SOLUTION_TRILATERATION = 4;\nstatic const int ERROR_IN_TRILATER = 28;\n\n#include \n#include \n#include \n#include \n\n#include \"beacon.h\"\n\n\n// this class allows calculate coordinates of the user using distance or RSSI\n// measurements to nearest transmitters such as bluetooth Ibeacons or WiFi Access Points\n// \nclass Trilateration\n{\n public:\n //constructor and destructor\n Trilateration();\n ~Trilateration();\n //copy constructor\n Trilateration(const Trilateration& trilat);\n\n // update measurements if we got new ones\n void updateMeasurements( std::vector < BeaconMeas >& beaconMeasurements );\n\n // fill all the beacons located at the map\n void fillLocationBeacons( std::vector & beaconsOnFloor );\n\n // calculate desired coordinates on the measurement basis\n int calculateCoordinates( );\n\n //get id of current location\n int getCurrentLocationId() const;\n\n //set id of current location\n void setCurrentLocationId( int curLoc );\n\n static void filterUnknownBeacons( std::vector & beaconMeas,\n const std::vector& mapBeacons );\n\n // get X and Y coordinate calculated by trilateration\n double getX() const;\n double getY() const;\n \n //get error message in a case of error\n const char* getErrorMessage() const;\n\n //print X,Y to file in \"%lf %lf \\n\" format\n void printXyToFile (char* filename) const;\n\n private:\n\n std::vector mLocationBeacons; //set of beacons at the current location\n std::vector mBeaconMeas; //set of beacon measurements\n std::vector mXY; //desired X,Y coordinates\n int mCurLocationId; //id of location\n \n std::string mErrorMessage; //descriptive error message\n\n int calculateTrilaterationCoordinates( ); //calculate coordinates (when data is prepared)\n\n int deleteDuplicateMeasurements (std::vector& BeaconMeasurements );\n ////delete duplicate meas and get average RSSI, Dist.\n \n void getLinearSystem( //if we have precise measurements,\n std::vector &matrixA, //we determine coordinates by OLS method\n std::vector &b, //by solving overdetermined system\n int dim );\n\n void getLeastSquaresCoordinates( ); \n\n void solveLinearSystem( //solve overdetermined system if\n std::vector matrixA, //pseudo distance equations\n std::vector b );\n \n void setErrorMessage( const char* errorMsg ); //set error message\n};\n\n\n//we use this func in order to sort the beacon measurements\nbool compareBeaconMeasByName(\n BeaconMeas first,\n BeaconMeas second );\n\n\n//find iterator of beacon, from which we got measurements\nstd::vector::const_iterator findBeaconForMeas( \n const std::vector& mapBeacons,\n const std::string& measureBeaconId );\n\n\n\n#endif\n" }, { "path": "tests/navigation_test.cpp", "content": "/** navigation_test.cpp\n *\n * Copyright (c) 2014 Navigine.\n *\n */ \n\n#include \n#include \n#include \n#include \n#include \n\n#include \n#include \n#include \n#include \n\n#include \"../tools/verification/helpers.h\"\n#include \n\nusing namespace navigine::navigation_core;\n\nvoid PrintUsage(const char* program)\n{\n printf(\"Usage:\\n\"\n \"\\t%s {MAP_FILE} {LOG_FILE} {SETTINGS_JSON} {NAV_BATCH_SIZE}\\n\",\n program);\n}\n\nvoid BuildPdrTrack(const double pdrDistance, const double pdrHeading, double& pdrX, double& pdrY)\n{\n static double prevPdrDistance = 0.0, prevPdrHeading = 0.0;\n if (pdrDistance != prevPdrDistance && pdrHeading != prevPdrHeading)\n {\n prevPdrDistance = pdrDistance;\n prevPdrHeading = pdrHeading;\n pdrX += pdrDistance * std::cos(pdrHeading);\n pdrY += pdrDistance * std::sin(pdrHeading);\n }\n}\n\nint main(int argc, char** argv)\n{\n if (argc < 5)\n {\n PrintUsage(argv[0]);\n return -1;\n }\n \n std::string geojsonFile = argv[1];\n std::string logFile = argv[2];\n std::string settingsFile = argv[3];\n const int navBatchSize = std::stoi(argv[4]); // This variable responsible for how many messages we want to parse and pass to navigation in one batch.\n\n int errorCode = 0;\n GeoLevels geolevels = ParseGeojson(geojsonFile, errorCode);\n NavigationSettings navProps = CreateSettingsFromJson(settingsFile, errorCode);\n\n if (errorCode != 0)\n {\n std::cerr << \"error parsing json file \" << errorCode << std::endl;\n return errorCode;\n }\n\n std::shared_ptr levelCollector = createLevelCollector();\n for (const auto& geolevel: geolevels)\n {\n levelCollector->addGeoLevel(*(geolevel.get()));\n }\n\n std::shared_ptr navClient = createNavigationClient(levelCollector, navProps);\n\n double pdrX = 0.0, pdrY = 0.0;\n auto navMessages = GetNavMessages(logFile);\n std::vector> navBatchMessages;\n\n for(size_t i = 0; i < navMessages.size(); i += navBatchSize) \n {\n auto last = std::min(navMessages.size(), i + navBatchSize);\n navBatchMessages.emplace_back(navMessages.begin() + i, navMessages.begin() + last);\n }\n\n long long firstTs = 0;\n for (const auto& navBatchInput: navBatchMessages)\n {\n if (firstTs == 0) {\n firstTs = navBatchInput[0].ts;\n }\n const std::vector navBatchOutput = navClient->navigate(navBatchInput);\n const std::vector navStates = navClient->getStates();\n \n for (std::size_t i = 0; i < navBatchOutput.size(); ++i)\n {\n if (navBatchOutput[i].status == NavigationStatus::OK)\n {\n BuildPdrTrack(navStates[i].getStepLen(), navStates[i].getHeading(), pdrX, pdrY);\n }\n std::string providerStr;\n switch (navBatchOutput[i].provider)\n {\n case Provider::GNSS:\n providerStr = \"GNSS\";\n break;\n case Provider::INDOOR:\n providerStr = \"Indoor\";\n break;\n case Provider::FUSED:\n providerStr = \"Fused\";\n break;\n case Provider::NONE:\n providerStr = \"none\";\n break;\n default:\n providerStr = \"unknown provider\";\n }\n\n std::cout << std::setprecision(10);\n std::cout << std::fixed;\n\n long long currentTs = navBatchInput[i].ts;\n std::cout << ((currentTs - firstTs) / 1000) << \" \"\n << navBatchOutput[i].posLatitude << \" \"\n << navBatchOutput[i].posLongitude << \" \"\n << navBatchOutput[i].posAltitude << \" \"\n << pdrX << \" \"\n << pdrY << \" \"\n << navBatchOutput[i].posOrientation << \" \"\n << navBatchOutput[i].posLevel.value.c_str() << \" \"\n << navBatchOutput[i].status << std::endl;\n }\n }\n return 0;\n}\n" }, { "path": "tools/verification/helpers.cpp", "content": "#include \n#include \n\n#include \n#include \n#include \n#include \n#include \n#include \n\n#include \"helpers.h\"\n#include \n#include \"../../src/navigation_error_codes.h\"\n\nusing namespace navigine::navigation_core;\n\nstatic double const GRAPH_SAME_POINTS_GEO_DIST = 0.1;\n\nnamespace nlohmann {\n\nNLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::RadioMeasurementData::Type, {\n {navigine::navigation_core::RadioMeasurementData::Type::WIFI, 201},\n {navigine::navigation_core::RadioMeasurementData::Type::BEACON, 203},\n {navigine::navigation_core::RadioMeasurementData::Type::BLUETOOTH, 202},\n {navigine::navigation_core::RadioMeasurementData::Type::WIFI_RTT, 205}\n})\n\nNLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::TransmitterType, {\n {navigine::navigation_core::TransmitterType::WIFI, \"WIFI\"},\n {navigine::navigation_core::TransmitterType::BEACON, \"BEACON\"},\n {navigine::navigation_core::TransmitterType::BLUETOOTH, \"BLUETOOTH\"},\n {navigine::navigation_core::TransmitterType::EDDYSTONE, \"EDDYSTONE\"},\n {navigine::navigation_core::TransmitterType::LOCATOR, \"LOCATOR\"},\n {navigine::navigation_core::TransmitterType::UNKNOWN, \"UNKNOWN\"}\n})\n\nNLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::SensorMeasurementData::Type, {\n {navigine::navigation_core::SensorMeasurementData::Type::ACCELEROMETER, 101},\n {navigine::navigation_core::SensorMeasurementData::Type::GYROSCOPE, 102},\n {navigine::navigation_core::SensorMeasurementData::Type::MAGNETOMETER, 103},\n {navigine::navigation_core::SensorMeasurementData::Type::BAROMETER, 104},\n {navigine::navigation_core::SensorMeasurementData::Type::LOCATION, 106},\n {navigine::navigation_core::SensorMeasurementData::Type::ORIENTATION, 105}\n})\n\nNLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::CommonSettings::UseAlgorithm, {\n {navigine::navigation_core::CommonSettings::UseAlgorithm::PF_PDR, \"PF_PDR\"},\n {navigine::navigation_core::CommonSettings::UseAlgorithm::PF_NOPDR, \"PF_NOPDR\"},\n {navigine::navigation_core::CommonSettings::UseAlgorithm::ZONES, \"ZONES\"},\n {navigine::navigation_core::CommonSettings::UseAlgorithm::GRAPH, \"GRAPH\"},\n {navigine::navigation_core::CommonSettings::UseAlgorithm::KNN, \"KNN\"}\n})\n\nNLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::CommonSettings::MeasurementType, {\n {navigine::navigation_core::CommonSettings::MeasurementType::RSSI, \"rssi\"},\n {navigine::navigation_core::CommonSettings::MeasurementType::DISTANCE_V1, \"distance_v1\"},\n {navigine::navigation_core::CommonSettings::MeasurementType::DISTANCE_V2, \"distance_v2\"}\n})\n\nNLOHMANN_JSON_SERIALIZE_ENUM(navigine::navigation_core::CommonSettings::SignalsToUse, {\n {navigine::navigation_core::CommonSettings::SignalsToUse::BLE, \"ble\"},\n {navigine::navigation_core::CommonSettings::SignalsToUse::WIFI, \"wifi\"},\n {navigine::navigation_core::CommonSettings::SignalsToUse::BOTH, \"both\"}\n})\n\nstatic void from_json(const nlohmann::json& j, SensorMeasurementData& sensorMsr)\n{\n j.at(\"type\").get_to(sensorMsr.type);\n std::vector values;\n j.at(\"values\").get_to(values);\n if (values.size() == 3)\n sensorMsr.values = Vector3d(values.at(0), values.at(1), values.at(2));\n}\n\nstatic void from_json(const nlohmann::json& j, RadioMeasurementData& radioMsr)\n{\n j.at(\"type\").get_to(radioMsr.type);\n\n std::string id;\n j.at(\"bssid\").get_to(id);\n radioMsr.id = TransmitterId(id);\n\n if (j.count(\"rssi\"))\n j.at(\"rssi\").get_to(radioMsr.rssi);\n\n if (j.count(\"distance\"))\n j.at(\"distance\").get_to(radioMsr.distance);\n}\n\nstatic void from_json(const nlohmann::json& j, NmeaMeasurementData& nmeaMsr)\n{\n j.at(\"sentence_number\").get_to(nmeaMsr.sentenceNumber);\n j.at(\"num_sats\").get_to(nmeaMsr.satellitesNumber);\n}\n\nstatic void from_json(const nlohmann::json& j, Measurement& msr)\n{\n j.at(\"timestamp\").get_to(msr.ts);\n int msrType;\n j.at(\"type\").get_to(msrType);\n if (msrType == 101 || msrType == 102 || msrType == 103 || msrType == 104 || msrType == 105 || msrType == 106)\n msr.data = j.get();\n else if (msrType == 201 || msrType == 202 || msrType == 203 || msrType == 204 || msrType == 205)\n msr.data = j.get();\n else if (msrType == 300)\n msr.data = j.get();\n else\n std::cerr << \"Unknown measurement.\" << std::endl;\n}\n\nstatic void from_json(const nlohmann::json& j, CommonSettings& commonSettings)\n{\n std::vector keys;\n for (auto& el : j.items())\n {\n keys.push_back(el.key());\n }\n try \n {\n for (const auto& key : keys)\n {\n if (key == \"Use_algorithm\")\n j.at(key).get_to(commonSettings.useAlgorithm);\n else if (key == \"Correction_noise\")\n j.at(key).get_to(commonSettings.lklCorrectionNoise);\n else if (key == \"Use_enu_azimuth\")\n j.at(key).get_to(commonSettings.useEnuAzimuth);\n else if (key == \"Differention_mode\")\n j.at(key).get_to(commonSettings.useDifferentionMode);\n else if (key == \"Use_beacon_power\")\n j.at(key).get_to(commonSettings.useTxPower);\n else if (key == \"Use_radiomap\")\n j.at(key).get_to(commonSettings.useRadiomap);\n else if (key == \"Use_stops\")\n j.at(key).get_to(commonSettings.useStops);\n else if (key == \"Use_tracking\")\n j.at(key).get_to(commonSettings.useTracking);\n else if (key == \"Stop_detection_time\")\n j.at(key).get_to(commonSettings.stopDetectionTime);\n else if (key == \"Min_deviation_m\")\n j.at(key).get_to(commonSettings.minDeviationM);\n else if (key == \"Average_speed\")\n j.at(key).get_to(commonSettings.averageMovSpeed);\n else if (key == \"Measurement_type\")\n j.at(key).get_to(commonSettings.measurementType);\n else if (key == \"Use_best_ref_point_level\")\n j.at(key).get_to(commonSettings.useBestRefPointLevel);\n else if (key == \"Use_barometer\")\n j.at(key).get_to(commonSettings.useBarometer);\n\n else if (key == \"Use_unknown_beacons\")\n j.at(key).get_to(commonSettings.useUnknownTransmitters);\n else if (key == \"Cutoff_rss\")\n j.at(key).get_to(commonSettings.sigCutOffRss);\n else if (key == \"Use_closest_aps\")\n j.at(key).get_to(commonSettings.useClosestAps);\n else if (key == \"N_closest_aps\")\n j.at(key).get_to(commonSettings.numClosestAps);\n else if (key == \"Sig_averaging_time\")\n j.at(key).get_to(commonSettings.sigAveragingTime);\n else if (key == \"Sig_window_shift\")\n j.at(key).get_to(commonSettings.sigWindowShift);\n else if (key == \"Use_Signals\")\n j.at(key).get_to(commonSettings.signalsToUse);\n\n else if (key == \"N_particles\")\n j.at(key).get_to(commonSettings.numParticles);\n else if (key == \"Min_msr_num_for_mutation\")\n j.at(key).get_to(commonSettings.minMsrNumForMutation);\n else if (key == \"Resampling_threshold_num\")\n j.at(key).get_to(commonSettings.resamplingThreshold);\n else if (key == \"Min_num_of_particles\")\n j.at(key).get_to(commonSettings.minNumParticlesAlive);\n else if (key == \"Gyro_noise_degree\")\n j.at(key).get_to(commonSettings.gyroNoiseDegree);\n else if (key == \"Step_noise_deviation_meters\")\n j.at(key).get_to(commonSettings.stepNoiseDeviationMeters);\n else if (key == \"Use_projection_to_white_area\")\n j.at(key).get_to(commonSettings.useProjectionToWhiteArea);\n\n else if (key == \"N_particles_around_closest_AP\")\n j.at(key).get_to(commonSettings.numParticlesAroundClosestAp);\n else if (key == \"N_particles_to_mutate\")\n j.at(key).get_to(commonSettings.numParticlesToMutate);\n else if (key == \"Num_sampling_attempts\")\n j.at(key).get_to(commonSettings.numSamplingAttempts);\n else if (key == \"Num_initialization_attempts\")\n j.at(key).get_to(commonSettings.numInitializationAttempts);\n else if (key == \"Use_uniform_sampling\")\n j.at(key).get_to(commonSettings.useUniformSampling);\n\n else if (key == \"K\")\n j.at(key).get_to(commonSettings.kNeighbors);\n else if (key == \"Use_triangles\")\n j.at(key).get_to(commonSettings.useTriangles);\n else if (key == \"Use_diff_mode\")\n j.at(key).get_to(commonSettings.useDiffMode);\n else if (key == \"Min_num_of_measurements_for_position_calculation\")\n j.at(key).get_to(commonSettings.minNumOfMeasurementsForPositionCalculation);\n\n else if (key == \"Smoothing\")\n j.at(key).get_to(commonSettings.smoothing);\n else if (key == \"Dead_reckon_time\")\n j.at(key).get_to(commonSettings.deadReckoningTime);\n else if (key == \"Stop_update_time\")\n j.at(key).get_to(commonSettings.stopUpdateTime);\n else if (key == \"Graph_prog_distance\")\n j.at(key).get_to(commonSettings.graphProjDist);\n else if (key == \"Stopped_distance_threshold_m\")\n j.at(key).get_to(commonSettings.useStopsDistanceThresholdM);\n\n else if (key == \"Use_smoothing\")\n j.at(key).get_to(commonSettings.useSmoothing);\n else if (key == \"Use_graph_projection\")\n j.at(key).get_to(commonSettings.useGraphProjection);\n else if (key == \"Use_accuracy_for_smoothing\")\n j.at(key).get_to(commonSettings.useAccuracyForSmoothing);\n else if (key == \"Use_speed_constraint\")\n j.at(key).get_to(commonSettings.useSpeedConstraint);\n else if (key == \"Use_barriers\")\n j.at(key).get_to(commonSettings.useBarriers);\n else if (key == \"Use_time_smoothing\")\n j.at(key).get_to(commonSettings.useTimeSmoothing);\n else if (key == \"Use_ab_filter\")\n j.at(key).get_to(commonSettings.useAbFilter);\n\n else if (key == \"Use_gps\")\n j.at(key).get_to(commonSettings.useGps);\n else if (key == \"Use_instant_gps_position\")\n j.at(key).get_to(commonSettings.useInstantGpsPosition);\n else if (key == \"Prefer_indoor_solution\")\n j.at(key).get_to(commonSettings.preferIndoorSolution);\n else if (key == \"Fuse_gps\")\n j.at(key).get_to(commonSettings.fuseGps);\n else if (key == \"Use_gps_outside_map\")\n j.at(key).get_to(commonSettings.useGpsOutsideMap);\n else if (key == \"Use_gps_sigma_filter\")\n j.at(key).get_to(commonSettings.useGpsSigmaFilter);\n else if (key == \"Sigma_filter_window\")\n j.at(key).get_to(commonSettings.sigmaFilterWindow);\n else if (key == \"Sigma_filter_delta\")\n j.at(key).get_to(commonSettings.sigmaFilterDelta);\n else if (key == \"Min_num_sats\")\n j.at(key).get_to(commonSettings.minNumSats);\n\n else if (key == \"Prior_deviation\")\n j.at(key).get_to(commonSettings.priorDeviation);\n else if (key == \"Gps_deviation\")\n j.at(key).get_to(commonSettings.minGpsDeviation);\n else if (key == \"Maximum_to_accept_gps_measurement\")\n j.at(key).get_to(commonSettings.maxGpsDeviation);\n else if (key == \"Fuse_gps_border_m\")\n j.at(key).get_to(commonSettings.fuseGpsBorderM);\n else if (key == \"Position_old_for_fusing_sec\")\n j.at(key).get_to(commonSettings.positionIsTooOldSec);\n else if (key == \"Gps_valid_time_window_sec\")\n j.at(key).get_to(commonSettings.gpsValidTimeWindowSec);\n\n else if (key == \"Use_calculated_azimuth\")\n j.at(key).get_to(commonSettings.useCalculatedAzimuth);\n else if (key == \"Device_azimuth_lifetime_seconds\")\n j.at(key).get_to(commonSettings.deviceAzimuthLifetimeSeconds);\n else if (key == \"Step_multiplier\")\n j.at(key).get_to(commonSettings.stepMultiplier);\n\n else if (key == \"Has_accelerometer\")\n j.at(key).get_to(commonSettings.hasAccelerometer);\n else if (key == \"Has_gyroscope\")\n j.at(key).get_to(commonSettings.hasGyroscope);\n\n else if (key == \"No_signal_time_threshold\")\n j.at(key).get_to(commonSettings.noSignalTimeThreshold);\n else if (key == \"No_action_time_threshold\")\n j.at(key).get_to(commonSettings.noActionTimeThreshold);\n\n else if (key == \"WiFi_RTT_Offset\")\n j.at(key).get_to(commonSettings.wiFiRttOffset);\n else\n std::cerr << \"unknown setting: \" << key << '\\n';\n }\n } \n catch (json::exception& e) \n {\n std::cerr << \"parsing json \" << j << \" following exception occurred\" << '\\n'\n << \"message: \" << e.what() << '\\n';\n }\n}\n \nstatic void from_json(const nlohmann::json& j, LevelSettings& levelSettings)\n{\n std::vector keys;\n for (auto& el : j.items())\n {\n keys.push_back(el.key());\n }\n try\n {\n for (const auto& key : keys)\n {\n if (key == \"A\")\n j.at(key).get_to(levelSettings.normalModeA);\n else if (key == \"B\")\n j.at(key).get_to(levelSettings.normalModeB);\n else\n std::cerr << \"unknown setting: \" << key << '\\n';\n }\n }\n catch (json::exception& e) \n {\n std::cerr << \"parsing json \" << j << \" following exception occurred\" << '\\n'\n << \"message: \" << e.what() << '\\n';\n }\n}\n} // nlohmann\n\nstd::vector GetNavMessages(const std::string& jsonFile)\n{\n std::vector inputs;\n\n std::ifstream infile(jsonFile);\n if (!infile.good())\n std::cout << \"GetNavMessages ERROR: CAN NOT READ FILE \" << jsonFile << \" !!!\" << std::endl;\n\n std::string line;\n while (std::getline(infile, line))\n {\n auto j = nlohmann::json::parse(line);\n inputs.emplace_back(j.get());\n }\n return inputs;\n}\n\ndouble getGeoDist(const GeoPoint& p1, const GeoPoint& p2)\n{\n static const double R = 6378000;\n double lat1Rad = M_PI * p1.latitude / 180.0;\n double lat2Rad = M_PI * p2.latitude / 180.0;\n double lng1Rad = M_PI * p1.longitude / 180.0;\n double lng2Rad = M_PI * p2.longitude / 180.0;\n double dlat = lat1Rad - lat2Rad;\n double dlng = lng1Rad - lng2Rad;\n double a = std::pow(std::sin(dlat / 2.0), 2)\n + std::cos(lat1Rad) * std::cos(lat2Rad) * std::pow(std::sin(dlng / 2.0), 2);\n double c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a));\n double d = R * c;\n return d;\n}\n\nGeoPoint removeSame(std::vector& allPoints)\n{\n assert(!allPoints.empty());\n GeoPoint p = allPoints.front();\n auto it = allPoints.begin();\n\n int counter = 0;\n while (it != allPoints.end())\n {\n if (getGeoDist(*it, p) < GRAPH_SAME_POINTS_GEO_DIST)\n {\n it = allPoints.erase(it);\n counter++;\n }\n else\n it++;\n }\n return p;\n}\n\nint getVertex(const GeoPoint& firstPoint, const std::unordered_map& vertexMap)\n{\n for (const auto& it: vertexMap)\n {\n int v = it.first;\n GeoPoint p = it.second;\n if (getGeoDist(firstPoint, p) < GRAPH_SAME_POINTS_GEO_DIST)\n return v;\n }\n throw std::logic_error(\"Vertex was not found!\");\n return -1;\n}\n\nstd::pair> parseGraph(const nlohmann::json& j)\n{\n auto propertiesJson = j.at(\"properties\");\n std::string levelId = propertiesJson.at(\"level\");\n auto geometryJson = j.at(\"geometry\");\n auto coordinatesJson = geometryJson.at(\"coordinates\");\n std::vector allPoints;\n std::vector> edgesPoints;\n for (const auto& lineJson: coordinatesJson)\n {\n std::vector points;\n for (const auto& pointJson: lineJson)\n {\n std::vector pointCoordinates;\n pointJson.get_to(pointCoordinates);\n //geojson has longitude:latitude format\n GeoPoint geoPoint(pointCoordinates.at(1), pointCoordinates.at(0));\n points.push_back(geoPoint);\n allPoints.push_back(geoPoint);\n }\n edgesPoints.push_back(points);\n }\n\n int v = 0;\n std::unordered_map vertexMap;\n while (!allPoints.empty())\n {\n v++;\n GeoPoint p = removeSame(allPoints);\n vertexMap[v] = p;\n }\n\n std::map> adjset;\n for (std::vector& edge: edgesPoints)\n {\n assert(edge.size() == 2);\n GeoPoint p1 = edge[0];\n int v1 = getVertex(p1, vertexMap);\n GeoPoint p2 = edge[1];\n int v2 = getVertex(p2, vertexMap);\n\n if (adjset.find(v1) == adjset.end())\n adjset[v1] = std::set();\n adjset[v1].insert(v2);\n\n if (adjset.find(v2) == adjset.end())\n adjset[v2] = std::set();\n adjset[v2].insert(v1);\n }\n\n Graph graph;\n std::map::Vertex> addedVertices;\n for (auto& it: adjset)\n {\n int v1 = it.first;\n std::set adjacentVertexes = it.second;\n GeoPoint p1 = vertexMap[v1];\n Graph::Vertex vertex1 = graph.addVertex(p1);\n addedVertices.insert({v1, vertex1});\n for (int v2: adjacentVertexes)\n {\n if (addedVertices.find(v2) == addedVertices.end())\n {\n GeoPoint p2 = vertexMap[v2];\n Graph::Vertex vertex2 = graph.addVertex(p2);\n addedVertices.insert({v2, vertex2});\n graph.addEdge(vertex1.id, vertex2.id);\n }\n else\n {\n Graph::Vertex vertex2 = addedVertices[v2];\n graph.addEdge(vertex1.id, vertex2.id);\n }\n }\n }\n return std::make_pair(LevelId(levelId), graph);\n}\n\nstd::pair> parseTransmitter(nlohmann::json j)\n{\n auto properties = j.at(\"properties\");\n std::string levelId = properties.at(\"level\");\n //TODO transmitters do not contain pathloss model in configuration file!\n\n double A = -80.0;\n double B = 3.0;\n if (properties.count(\"A\"))\n A = properties.at(\"A\");\n if (properties.count(\"B\"))\n B = properties.at(\"B\");\n\n PathlossModel pathlossModel{A, B, 0};\n std::string id = properties.at(\"uuid\");\n\n TransmitterType type;\n auto transmitterTypeJson = properties.at(\"transmitter_type\");\n transmitterTypeJson.get_to(type);\n std::vector pointCoordinates;\n auto geometryJson = j.at(\"geometry\");\n geometryJson.at(\"coordinates\").get_to(pointCoordinates);\n //geojson has longitude:latitude format\n GeoPoint3D geopoint3D(pointCoordinates.at(1), pointCoordinates.at(0), 0.0);\n if (pointCoordinates.size() == 3)\n geopoint3D = GeoPoint3D(pointCoordinates.at(1), pointCoordinates.at(0), pointCoordinates.at(2));\n Transmitter transmitter(TransmitterId(id), geopoint3D, pathlossModel, type);\n return std::make_pair(LevelId(levelId), transmitter);\n}\n\nstd::pair> parseBarriers(const nlohmann::json& j)\n{\n auto propertiesJson = j.at(\"properties\");\n auto levelId = propertiesJson.at(\"level\");\n auto geometryJson = j.at(\"geometry\");\n auto barriersPolygonsJson = geometryJson.at(\"coordinates\");\n std::list levelPolygons;\n for (const auto& polygonJson: barriersPolygonsJson)\n {\n Polygon barrier;\n for (const auto& pointJson: polygonJson[0])\n {\n std::vector pointCoordinates;\n pointJson.get_to(pointCoordinates);\n //geojson has longitude:latitude format\n boost::geometry::append(barrier, Point(pointCoordinates.at(1), pointCoordinates.at(0)));\n }\n levelPolygons.emplace_back(barrier);\n }\n\n return std::make_pair(LevelId(levelId), levelPolygons);\n}\n\nstd::pair> parseReferencePoint(const nlohmann::json& j)\n{\n auto properties = j.at(\"properties\");\n std::string levelId = properties.at(\"level\");\n std::string id = properties.at(\"uuid\");\n std::vector pointCoordinates;\n auto geometry = j.at(\"geometry\");\n geometry.at(\"coordinates\").get_to(pointCoordinates);\n //geojson has longitude:latitude format\n GeoPoint geopoint(pointCoordinates.at(1), pointCoordinates.at(0));\n\n std::map fingerprints;\n auto entriesJson = properties.at(\"entries\");\n for (const auto& entryJson: entriesJson)\n {\n std::string bssid = entryJson.at(\"bssid\");\n TransmitterType sigType;\n entryJson.at(\"type\").get_to(sigType);\n std::map distribution;\n for (const auto& valueJson: entryJson.at(\"value\"))\n {\n int rssi;\n int count;\n valueJson.at(\"rssi\").get_to(rssi);\n valueJson.at(\"count\").get_to(count);\n distribution.insert({ rssi, count });\n }\n // Compute number of measurements, mean and variance of signal\n double meanRssi = 0.0;\n double varianceRssi = 0.0;\n double sumOfSquares = 0.0;\n double squaredSum = 0.0;\n size_t nMeas = 0;\n\n for (auto const& pair : distribution)\n {\n nMeas += pair.second;\n meanRssi += (-pair.first) * pair.second;\n sumOfSquares += pair.second * pair.first * pair.first;\n squaredSum += pair.second * pair.first;\n }\n meanRssi /= nMeas;\n squaredSum *= squaredSum;\n\n SignalStatistics sigStat;\n sigStat.mean = meanRssi;\n sigStat.variance = varianceRssi;\n sigStat.nMeasurements = nMeas;\n sigStat.type = sigType;\n fingerprints.insert({TransmitterId(bssid), sigStat});\n }\n\n return std::make_pair(LevelId(levelId), ReferencePoint(ReferencePointId(id), geopoint, fingerprints));\n}\n\nstd::pair> parseLevel(const nlohmann::json& j)\n{\n auto propertiesJson = j.at(\"properties\");\n auto levelId = propertiesJson.at(\"level\");\n auto geometryJson = j.at(\"geometry\");\n auto allowedAreaPolygonsJson = geometryJson.at(\"coordinates\");\n std::list levelPolygons;\n for (const auto& polygonJson: allowedAreaPolygonsJson)\n {\n int count = 0;\n Polygon allowedArea;\n allowedArea.inners().resize(polygonJson.size() - 1);\n for (const auto& circleJson: polygonJson)\n {\n for (const auto& pointJson: circleJson)\n {\n std::vector pointCoordinates;\n pointJson.get_to(pointCoordinates);\n //geojson has longitude:latitude format\n if (count == 0)\n boost::geometry::append(allowedArea.outer(), Point(pointCoordinates.at(1), pointCoordinates.at(0)));\n else\n boost::geometry::append(allowedArea.inners()[count - 1], Point(pointCoordinates.at(1), pointCoordinates.at(0)));\n }\n count++;\n }\n levelPolygons.emplace_back(allowedArea);\n }\n\n return std::make_pair(LevelId(levelId), levelPolygons);\n}\n\nGeoLevels ParseGeojson(const std::string& jsonFile, int& errorCode)\n{\n errorCode = 0;\n std::ifstream is(jsonFile);\n nlohmann::json j = nlohmann::json::parse(is);\n\n std::map> levels;\n auto features = j.at(\"features\");\n for (const auto& feature: features)\n {\n auto featureProperties = feature.at(\"properties\");\n std::string featureType = featureProperties.at(\"type\");\n if (featureType == \"level\")\n {\n std::pair> levelPair = parseLevel(feature);\n levels.insert(levelPair);\n }\n }\n\n std::map> barriers;\n std::map transmitters;\n std::map> graphs;\n std::map referencePoints;\n\n for (const auto& feature: features)\n {\n auto featureProperties = feature.at(\"properties\");\n std::string featureType = featureProperties.at(\"type\");\n if (featureType == \"graph\")\n {\n std::pair> graphPair = parseGraph(feature);\n graphs.insert(graphPair);\n }\n\n if (featureType == \"transmitter\")\n {\n std::pair> transmitterPair = parseTransmitter(feature);\n if (transmitters.find(transmitterPair.first) == transmitters.end())\n transmitters[transmitterPair.first] = std::vector>();\n transmitters[transmitterPair.first].push_back(transmitterPair.second);\n }\n\n if (featureType == \"reference_point\")\n {\n std::pair> refpointsPair = parseReferencePoint(feature);\n if (referencePoints.find(refpointsPair.first) == referencePoints.end())\n referencePoints[refpointsPair.first] = GeoReferencePoints();\n referencePoints[refpointsPair.first].push_back(refpointsPair.second);\n }\n }\n\n //TODO it is possible to have levels without transmitters so why we should\n //check levels mismatch???\n //if (!transmitters.empty() && !haveEqualKeys(transmitters, levels))\n //{\n // errorCode = MISSING_LEVELS;\n // std::cout << \"transmitters levels mismatch!\" << std::endl;\n // return GeoLevels();\n //}\n\n //if (!graphs.empty() && !haveEqualKeys(graphs, levels))\n //{\n // errorCode = MISSING_LEVELS;\n // std::cout << \"graphs levels mismatch!\" << std::endl;\n // return GeoLevels();\n //}\n\n //if (!referencePoints.empty() && !haveEqualKeys(referencePoints, levels))\n //{\n // errorCode = MISSING_LEVELS;\n // std::cout << \"refpoints levels mismatch!\" << std::endl;\n // return GeoLevels();\n //}\n\n std::vector > vLevels;\n for (const auto& entry : levels)\n {\n auto geolevel = std::make_shared();\n geolevel->id.value = entry.first.value;\n geolevel->altitude = 100.0; //TODO\n\n if (graphs.find(entry.first) != graphs.end())\n geolevel->graph = graphs.at(entry.first);\n\n geolevel->geometry = getGeometry(entry.second);\n\n if (transmitters.find(entry.first) != transmitters.end())\n geolevel->transmitters = transmitters.at(entry.first);\n\n if (referencePoints.find(entry.first) != referencePoints.end())\n geolevel->referencePoints = referencePoints.at(entry.first);\n\n vLevels.push_back(std::move(geolevel));\n }\n\n return vLevels;\n}\n\nNavigationSettings CreateSettingsFromJson(\n const std::string& jsonFile,\n int& errorCode)\n{\n NavigationSettings navigationSettings;\n\n errorCode = 0;\n std::ifstream is(jsonFile);\n if (!is.good())\n {\n std::cout << \"CreateSettingsFromJson ERROR: CAN NOT READ FILE \" << jsonFile << \" !!!\" << std::endl;\n errorCode = ERROR_OPEN_FILE;\n return navigationSettings;\n }\n\n nlohmann::json j = nlohmann::json::parse(is);\n auto levelsSettingsJson = j.at(\"level_settings\");\n for (const auto& levelSettingsJson: levelsSettingsJson)\n {\n std::string levelId = levelSettingsJson.at(\"level\");\n navigationSettings.levelsSettings[LevelId(levelId)] = levelSettingsJson.at(\"settings\")[0].get();\n }\n navigationSettings.commonSettings = j.at(\"common_settings\")[0].get();\n\n return navigationSettings;\n}\n" }, { "path": "tools/verification/helpers.h", "content": "#ifndef _HELPERS_H_\n#define _HELPERS_H_\n\n#include \n#include \n#include \n#include \n#include \n\nstruct NavigationPoint\n{\n NavigationPoint(){}\n\n NavigationPoint(long long _timeMs, double _lat, double _lng,\n double _angle, const navigine::navigation_core::LevelId& _level)\n : timeMs (_timeMs)\n , lat (_lat)\n , lng (_lng)\n , angle (_angle)\n , level (_level)\n { }\n\n long long timeMs = 0;\n double lat = 0;\n double lng = 0;\n double angle = 0;\n\n navigine::navigation_core::LevelId level;\n};\n\ntemplate \nbool haveEqualKeys(const Map1& lhs, const Map2& rhs)\n{\n auto pred = [](decltype(*lhs.begin()) a, decltype(*rhs.begin()) b)\n {\n return a.first == b.first;\n };\n\n return lhs.size() == rhs.size() && std::equal(lhs.begin(), lhs.end(), rhs.begin(), pred);\n}\n\nstd::vector GetNavMessages(const std::string& jsonFile);\n\nnavigine::navigation_core::GeoLevels ParseGeojson(\n const std::string& jsonFile,\n int& errorCode);\n\nnavigine::navigation_core::NavigationSettings CreateSettingsFromJson(\n const std::string& jsonFile,\n int& errorCode);\n\n#endif //_HELPERS_H_\n" } ]