Showing preview only (458K chars total). Download the full file or copy to clipboard to get everything.
Repository: Navigine/Indoor-Positioning-And-Navigation-Algorithms
Branch: master
Commit: 67e11c4d398a
Files: 149
Total size: 418.2 KB
Directory structure:
gitextract_n3y_rmqw/
├── .gitattributes
├── .github/
│ ├── ISSUE_TEMPLATE/
│ │ ├── bug_report.md
│ │ ├── custom.md
│ │ └── feature_request.md
│ └── workflows/
│ ├── macos.yml
│ └── ubuntu.yml
├── .gitignore
├── CMakeLists.txt
├── CODE_OF_CONDUCT.md
├── CONTRIBUTING.md
├── LICENSE
├── README.md
├── include/
│ └── navigine/
│ └── navigation-core/
│ ├── CHANGELOG.md
│ ├── README.md
│ ├── barriers_geometry_builder.h
│ ├── boost_geometry_adaptation.h
│ ├── declare_identifier.h
│ ├── geolevel.h
│ ├── graph.h
│ ├── graph_particle.h
│ ├── level.h
│ ├── level_collector.h
│ ├── level_geometry.h
│ ├── location_point.h
│ ├── logger.h
│ ├── motion_info.h
│ ├── navigation_client.h
│ ├── navigation_input.h
│ ├── navigation_output.h
│ ├── navigation_settings.h
│ ├── navigation_state.h
│ ├── point.h
│ ├── radiomap.h
│ ├── reference_point.h
│ ├── transmitter.h
│ ├── vector3d.h
│ ├── version.h
│ └── xy_particle.h
├── src/
│ ├── README
│ ├── barriers_geometry_builder.cpp
│ ├── device_properties.h
│ ├── geometry.cpp
│ ├── geometry.h
│ ├── knn/
│ │ ├── knn_utils.cpp
│ │ └── knn_utils.h
│ ├── level.cpp
│ ├── level_collector.cpp
│ ├── level_estimator/
│ │ ├── barometer.cpp
│ │ ├── barometer.h
│ │ ├── level_estimator.cpp
│ │ ├── level_estimator.h
│ │ ├── level_estimator_radiomap.cpp
│ │ ├── level_estimator_radiomap.h
│ │ ├── level_estimator_transmitters.cpp
│ │ ├── level_estimator_transmitters.h
│ │ ├── level_history.cpp
│ │ └── level_history.h
│ ├── level_geometry.cpp
│ ├── likelihood/
│ │ ├── likelihood.cpp
│ │ ├── likelihood.h
│ │ ├── likelihood_radiomap.cpp
│ │ └── likelihood_radiomap.h
│ ├── measurement_types.h
│ ├── measurements/
│ │ ├── measurement_preprocessor.cpp
│ │ └── measurement_preprocessor.h
│ ├── navigation_client_impl.cpp
│ ├── navigation_client_impl.h
│ ├── navigation_error_codes.h
│ ├── point.cpp
│ ├── position.h
│ ├── position_estimator/
│ │ ├── Readme.md
│ │ ├── docs/
│ │ │ └── KNN.md
│ │ ├── position_estimator.h
│ │ ├── position_estimator_knn.cpp
│ │ ├── position_estimator_knn.h
│ │ ├── position_estimator_outdoor.cpp
│ │ ├── position_estimator_outdoor.h
│ │ ├── position_estimator_zone.cpp
│ │ └── position_estimator_zone.h
│ ├── position_postprocessor/
│ │ ├── navigation_time_smoother.cpp
│ │ ├── navigation_time_smoother.h
│ │ ├── polynomial_fit.cpp
│ │ ├── polynomial_fit.h
│ │ ├── position_postprocessor.cpp
│ │ ├── position_postprocessor.h
│ │ ├── position_smoother.h
│ │ ├── position_smoother_ab.cpp
│ │ ├── position_smoother_ab.h
│ │ ├── position_smoother_lstsq.cpp
│ │ └── position_smoother_lstsq.h
│ ├── radiomap.cpp
│ ├── sensor_fusion/
│ │ ├── complementary_filter.cpp
│ │ ├── complementary_filter.h
│ │ ├── pedometer.cpp
│ │ ├── pedometer.h
│ │ ├── quaternion.cpp
│ │ ├── quaternion.h
│ │ ├── sensor_fusion.cpp
│ │ └── sensor_fusion.h
│ ├── triangulation.cpp
│ ├── triangulation.h
│ ├── trilateration.cpp
│ ├── trilateration.h
│ └── vector3d.cpp
├── standalone_algorithms/
│ ├── README.md
│ ├── System description.md
│ ├── complementary_filter/
│ │ ├── CMakeLists.txt
│ │ ├── README.md
│ │ ├── examples/
│ │ │ └── main.cpp
│ │ ├── helpers/
│ │ │ └── plot_angles.py
│ │ ├── include/
│ │ │ ├── complementary_filter.h
│ │ │ ├── quaternion.h
│ │ │ └── vector3d.h
│ │ ├── src/
│ │ │ ├── complementary_filter.cpp
│ │ │ ├── quaternion.cpp
│ │ │ └── vector3d.cpp
│ │ └── tests/
│ │ └── complementary_filter_test.cpp
│ ├── pedometer/
│ │ ├── CMakeLists.txt
│ │ ├── README.md
│ │ ├── examples/
│ │ │ └── main.cpp
│ │ ├── include/
│ │ │ ├── pedometer.h
│ │ │ └── vector3d.h
│ │ ├── src/
│ │ │ ├── pedometer.cpp
│ │ │ └── vector3d.cpp
│ │ └── tests/
│ │ └── pedometer_test.cpp
│ ├── position_estimation/
│ │ ├── CMakeLists.txt
│ │ ├── README.md
│ │ ├── examples/
│ │ │ └── main.cpp
│ │ ├── helpers/
│ │ │ └── plot_positions.py
│ │ ├── include/
│ │ │ ├── measurement_preprocessor.h
│ │ │ ├── navigation_structures.h
│ │ │ ├── nearest_transmitter_estimator.h
│ │ │ ├── position_smoother.h
│ │ │ └── transmitter.h
│ │ ├── logs/
│ │ │ └── transmitters.txt
│ │ └── src/
│ │ ├── measurement_preprocessor.cpp
│ │ ├── nearest_transmitter_estimator.cpp
│ │ └── position_smoother.cpp
│ └── trilateteration/
│ ├── CMakeLists.txt
│ ├── README.md
│ └── src/
│ ├── beacon.cpp
│ ├── beacon.h
│ ├── test_trilateration.cpp
│ ├── test_trilateration.h
│ ├── trilateration.cpp
│ └── trilateration.h
├── tests/
│ └── navigation_test.cpp
└── tools/
└── verification/
├── helpers.cpp
└── helpers.h
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitattributes
================================================
# Auto detect text files and perform LF normalization
* text=auto
# Custom for Visual Studio
*.cs diff=csharp
*.sln merge=union
*.csproj merge=union
*.vbproj merge=union
*.fsproj merge=union
*.dbproj merge=union
# Standard to msysgit
*.doc diff=astextplain
*.DOC diff=astextplain
*.docx diff=astextplain
*.DOCX diff=astextplain
*.dot diff=astextplain
*.DOT diff=astextplain
*.pdf diff=astextplain
*.PDF diff=astextplain
*.rtf diff=astextplain
*.RTF diff=astextplain
================================================
FILE: .github/ISSUE_TEMPLATE/bug_report.md
================================================
---
name: Bug report
about: Create a report to help us improve
title: ''
labels: ''
assignees: ''
---
**Describe the bug**
A clear and concise description of what the bug is.
**To Reproduce**
Steps to reproduce the behavior:
1. Go to '...'
2. Click on '....'
3. Scroll down to '....'
4. See error
**Expected behavior**
A clear and concise description of what you expected to happen.
**Screenshots**
If applicable, add screenshots to help explain your problem.
**Desktop (please complete the following information):**
- OS: [e.g. iOS]
- Browser [e.g. chrome, safari]
- Version [e.g. 22]
**Smartphone (please complete the following information):**
- Device: [e.g. iPhone6]
- OS: [e.g. iOS8.1]
- Browser [e.g. stock browser, safari]
- Version [e.g. 22]
**Additional context**
Add any other context about the problem here.
================================================
FILE: .github/ISSUE_TEMPLATE/custom.md
================================================
---
name: Custom issue template
about: Describe this issue template's purpose here.
title: ''
labels: ''
assignees: ''
---
================================================
FILE: .github/ISSUE_TEMPLATE/feature_request.md
================================================
---
name: Feature request
about: Suggest an idea for this project
title: ''
labels: ''
assignees: ''
---
**Is your feature request related to a problem? Please describe.**
A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
**Describe the solution you'd like**
A clear and concise description of what you want to happen.
**Describe alternatives you've considered**
A clear and concise description of any alternative solutions or features you've considered.
**Additional context**
Add any other context or screenshots about the feature request here.
================================================
FILE: .github/workflows/macos.yml
================================================
name: macOS
on:
push:
branches:
- dev
- master
pull_request:
jobs:
xcode:
runs-on: macos-10.15
strategy:
matrix:
xcode: [12.4, 12.3, 12.2, 12.1.1, 12.1, 12]
env:
DEVELOPER_DIR: /Applications/Xcode_${{ matrix.xcode }}.app/Contents/Developer
steps:
- uses: actions/checkout@v2
# install dependencies
- name: boost
run: brew install boost
- name: cmake
run: cmake -S . -B build -D CMAKE_BUILD_TYPE=Debug -DJSON_BuildTests=On -DJSON_FastTests=ON
- name: build
run: cmake --build build --parallel 10
- name: test
run: cd build ; ctest -j 10 --output-on-failure
================================================
FILE: .github/workflows/ubuntu.yml
================================================
name: Ubuntu
on:
push:
branches:
- dev
- master
pull_request:
jobs:
ci_test_clang:
runs-on: ubuntu-latest
steps:
# install dependencies
- name: boost
run: sudo apt-get update && sudo apt-get install libboost-all-dev
- uses: actions/checkout@v2
- name: install_ninja
run: |
sudo apt update
sudo apt install ninja-build
shell: bash
- name: install_clang
run: |
wget https://apt.llvm.org/llvm.sh
chmod +x llvm.sh
sudo ./llvm.sh 11
sudo apt-get install clang-tools-11
shell: bash
- name: cmake
run: cmake -S . -B build -DJSON_CI=On
- name: build
run: cmake --build build
ci_test_gcc:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
# install dependencies
- name: boost
run: sudo apt-get update && sudo apt-get install libboost-all-dev
- name: cmake
run: cmake -S . -B build -DJSON_CI=On
- name: build
run: cmake --build build
================================================
FILE: .gitignore
================================================
# Windows image file caches
Thumbs.db
ehthumbs.db
# Folder config file
Desktop.ini
# Recycle Bin used on file shares
$RECYCLE.BIN/
# Windows Installer files
*.cab
*.msi
*.msm
*.msp
# =========================
# Operating System Files
# =========================
# OSX
# =========================
.DS_Store
.AppleDouble
.LSOverride
# Icon must end with two \r
Icon
# Thumbnails
._*
# Files that might appear on external disk
.Spotlight-V100
.Trashes
# Directories potentially created on remote AFP share
.AppleDB
.AppleDesktop
Network Trash Folder
Temporary Items
.apdisk
*.sdf
*.pdb
*.sbr
*.obj
*.tlog
*.ilk
*.log
*.idb
*.bsc
*.opensdf
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
CMakeLists.txt.user
CMakeCache.txt
CMakeFiles
CMakeScripts
Testing
Makefile
cmake_install.cmake
install_manifest.txt
compile_commands.json
CTestTestfile.cmake
_deps
[Bb][Uu][Ii][Ll][Dd]/
.vscode/
================================================
FILE: CMakeLists.txt
================================================
cmake_minimum_required(VERSION 3.7)
project(Navigation-core)
include(FetchContent)
FetchContent_Declare(json
GIT_REPOSITORY https://github.com/ArthurSonzogni/nlohmann_json_cmake_fetchcontent.git
GIT_TAG v3.10.3)
FetchContent_GetProperties(json)
if(NOT json_POPULATED)
FetchContent_Populate(json)
add_subdirectory(${json_SOURCE_DIR} ${json_BINARY_DIR} EXCLUDE_FROM_ALL)
endif()
FetchContent_Declare(
Eigen
GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
GIT_TAG master
GIT_SHALLOW TRUE
GIT_PROGRESS TRUE)
set(EIGEN_BUILD_DOC OFF)
set(BUILD_TESTING OFF)
set(EIGEN_BUILD_PKGCONFIG OFF)
set( OFF)
FetchContent_MakeAvailable(Eigen)
set(BOOST_ENABLE_CMAKE ON)
set(Boost_USE_STATIC_LIBS ON)
find_package(Boost REQUIRED)
IF (Boost_FOUND)
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIR})
message(${Boost_INCLUDE_DIR})
include_directories(${Boost_SOURCE_DIR})
ENDIF ()
#tests:navigation_test
set("target" "navigation_test")
list(APPEND "${target}__cxx_srcs"
"tests/navigation_test.cpp"
"tools/verification/helpers.cpp")
add_executable("${target}" ${${target}__cxx_srcs})
target_include_directories("${target}" PRIVATE
"tests/include/"
"tests/build/"
"include/")
set_property(TARGET "${target}" PROPERTY CXX_STANDARD 17)
set_target_properties("${target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;")
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 ")
target_link_libraries("${target}" PRIVATE
"navigation-core"
nlohmann_json::nlohmann_json
Eigen3::Eigen)
#navigation-core:copy_public_include_dir
set("target" "navigation-core_copy_public_include_dir")
set("${target}__other_srcs" "include")
set("${target}__output" "tests/build/obj/include")
add_custom_command(OUTPUT ${${target}__output}
COMMAND ${CMAKE_COMMAND} -E copy_directory "include" "tests/build/obj/include"
DEPENDS ${${target}__other_srcs}
WORKING_DIRECTORY "tests/build/"
COMMENT "Copy ${target}"
VERBATIM)
add_custom_target("${target}" SOURCES ${${target}__other_srcs} DEPENDS ${${target}__output})
#navigation-core:navigation-core
set("target" "navigation-core")
list(APPEND "${target}__cxx_srcs"
"src/geometry.cpp"
"src/triangulation.cpp"
"src/knn/knn_utils.cpp"
"src/likelihood/likelihood.cpp"
"src/likelihood/likelihood_radiomap.cpp"
"src/measurements/measurement_preprocessor.cpp"
"src/position_estimator/position_estimator_knn.cpp"
"src/position_estimator/position_estimator_zone.cpp"
"src/position_estimator/position_estimator_outdoor.cpp"
"src/position_postprocessor/navigation_time_smoother.cpp"
"src/position_postprocessor/polynomial_fit.cpp"
"src/position_postprocessor/position_postprocessor.cpp"
"src/position_postprocessor/position_smoother_ab.cpp"
"src/position_postprocessor/position_smoother_lstsq.cpp"
"src/sensor_fusion/complementary_filter.cpp"
"src/sensor_fusion/quaternion.cpp"
"src/sensor_fusion/pedometer.cpp"
"src/sensor_fusion/sensor_fusion.cpp"
"src/level_estimator/barometer.cpp"
"src/level_estimator/level_estimator.cpp"
"src/level_estimator/level_estimator_radiomap.cpp"
"src/level_estimator/level_estimator_transmitters.cpp"
"src/level_estimator/level_history.cpp"
"src/navigation_client_impl.cpp"
"src/barriers_geometry_builder.cpp"
"src/level.cpp"
"src/point.cpp"
"src/level_collector.cpp"
"src/level_geometry.cpp"
"src/radiomap.cpp"
"src/vector3d.cpp"
"tools/verification/helpers.cpp"
"src/trilateration.cpp")
add_library("${target}" STATIC ${${target}__cxx_srcs})
# add_dependencies("${target}" boost_geometry)
target_include_directories("${target}" PRIVATE
"include/"
"tests/build/")
set_property(TARGET "${target}" PROPERTY CXX_STANDARD 17)
set_target_properties("${target}" PROPERTIES COMPILE_DEFINITIONS "BUILD_FOR_LINUX;_DEBUG;")
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 ")
target_link_libraries("${target}" PRIVATE nlohmann_json::nlohmann_json
Eigen3::Eigen)
================================================
FILE: CODE_OF_CONDUCT.md
================================================
# <p align="center"> Navigine Open Source Code of Conduct </p>
This 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.
## Our Standards
Examples of behavior that contributes to a positive environment for our community include:
- Demonstrating empathy and kindness toward other people
- Being respectful of differing opinions, viewpoints, and experiences
- Giving and gracefully accepting constructive feedback
- Accepting responsibility and apologizing to those affected by our mistakes, and learning from the experience
- Focusing on what is best not just for us as individuals, but for the overall community
Examples of unacceptable behavior include:
- The use of sexualized language or imagery, and sexual attention or advances of any kind
- Trolling, insulting or derogatory comments, and personal or political attacks
- Public or private harassment
- Publishing others’ private information, such as a physical or email address, without their explicit permission
- Other conduct which could reasonably be considered inappropriate in a professional setting
- Advocating for or encouraging any of the above behaviors
## Enforcement Responsibilities
Community 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.
Community 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.
## Scope
This 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.
## Enforcement
Instances 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.
All community leaders are obligated to respect the privacy and security of the reporter of any incident.
## Attribution
This 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.
For 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.
================================================
FILE: CONTRIBUTING.md
================================================
# <p align="center"> Contributing to Navigine Routing Library </p>
The 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.
When submitting a bug report, please include:
- The device and operating system version that produced the bug
- The version or commit of Routing Library that produced the bug
- Steps required to recreate the issue
- What happened
- What you expected to happen
Thanks!
## Development
To develop for the library, it is usually easiest to build and test your changes using either the Mac OS X or Ubuntu desktop targets.
## Code Style
In general, code changes should follow the style of the surrounding code.
When in doubt, you can use the provided clang-format style file for automatic styling.
Install clang-format (available through brew or apt-get):
```
brew install clang-format
```
or
```
sudo apt-get install clang-format
```
Run clang-format with specified style (use -i to modify the contents of the specified file):
```
clang-format -i -style=file [file]
```
================================================
FILE: LICENSE
================================================
MIT License
Copyright (c) 2021 Navigine Corporation
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: README.md
================================================
<a href="http://navigine.com"><img src="https://navigine.com/assets/web/images/logo.svg" align="right" height="60" width="180" hspace="10" vspace="5"></a>
Indoor-navigation-algorithms
[](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/ubuntu.yml) [](https://github.com/Navigine/Indoor-Navigation-Algorithms/actions/workflows/macos.yml)
============================
## Useful Links
- Refer to the [Navigine official website](https://navigine.com/) for complete list of downloads, useful materials, information about the company, and so on.
- [Get started](http://locations.navigine.com/login) with Navigine to get full access to Navigation services, SDKs, and applications.
- Refer to the Navigine [User Manual](http://docs.navigine.com/) for complete product usage guidelines.
- Find company contact information at the official website under [Contacts](https://navigine.com/contacts/) tab.
- Get support, contribute feedback, and connect with developers—visit [the Navigine Community](https://community.navigine.com/t/indoor-positioning-and-navigation-algorithms/173).
## Contents
This repository consists of the following components
* [src](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/src) - the source code of Navigine positioning algorithms
* [standalone_algorithms](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/standalone_algorithms) - set of algorithms used in navigation
* [tests](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/tests) - test and utilities for the evaluation of source code quality
* [tools](https://github.com/Navigine/Indoor-Positioning-And-Navigation-Algorithms/tree/master/tools/verification) - utilities for data extraction from map and settings
## Creating Navigation Client
Here 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).
First, create variables that will store your data.
```c++
// variables for storing location elements
navigine::navigation_core::GeoLevel geoLevel;
std::shared_ptr<navigine::navigation_core::LevelCollector> levelCollector;
navigine::navigation_core::NavigationSettings navigationSettings;
std::shared_ptr<navigine::navigation_core::NavigationClient> navClient;
```
Add transmitters (Beacon, Eddystone, WIFI, WIFI-RTT, etc.) as `GeoTransmitters` from your location.
Then they will be converted to `XYTransmitters` for internal evaluations
Parameters:
- id - identifier of transmitter like (major,minor,uuid) for beacon, (namespaceId,instanceId) for eddystone, mac for WIFI;
- point - latitude and longitude as GeoPoint;
- pathlossModel - A, B and power of transmitter as `PathlossModel` struct variable;
- type - type of transmitter, like Beacon, Eddystone, WIFI, WIFI-RTT, etc..
Inside for loop add all the transmitters from your location. Here is an example of adding one element
```c++
geoLevel.transmitters.emplace_back(transmitterId,
navigine::navigation_core::GeoPoint(latitude, longitude),
navigine::navigation_core::PathlossModel{A, B, power},
navigine::navigation_core::TransmitterType::BEACON);
```
Create 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
Parameters:
- barrierList - list of polygons, where each polygon describes the barrier of the level;
- allowedArea - polygon, which is created using width and height of the map;
Create the list of Polygons which will describe all the barriers
```c++
std::list<navigine::navigation_core::Polygon> barriersList;
for (size_t i = 0; i < barriers.size(); ++i)
{
auto coords = barriers.at(i);
navigine::navigation_core::Polygon barrier;
for (const auto& coord : coords)
boost::geometry::append(barrier, navigine::navigation_core::Point(coord.first, coord.second));
barriersList.push_back(barrier);
}
```
Create the polygon of allowed area
```c++
navigine::navigation_core::Polygon levelArea;
auto boundingBox = navigation_core::Box(
navigation_core::Point(leftMin.latitude, leftMin.longitude),
navigation_core::Point(rightMax.latitude, rightMax.longitude));
boost::geometry::convert(boundingBox, allowedArea);
geoLevel.geometry = navigine::navigation_core::getGeometry(barriersList, levelArea);
```
Create `LevelCollector` using method `createLevelCollector` and add all available geo levels.
```c++
levelCollector = navigine::navigation_core::createLevelCollector();
levelCollector->addGeoLevel(geoLevel);
```
Create `NavigationSettings`, with two parameters - level settings and common settings. (You can find them in navigation.xml i.e.)
Create `NavigationClient` using method `createNavigationClient` which will take as arguments level collector and navigation settings.
```c++
navClient = navigine::navigation_core::createNavigationClient(levelCollector, navigationSettings);
```
## Navigation test
The 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).
The test application takes 3 parameters:
- path to the file of the location in` geojson` format
- path to the file of logs gathered in this location in `json` format
- path to the settings file in `json` format in which parameters of the algorithm are specified
### Build
In order to build the test application go to the root directory of the repository and execute the following commands
``` sh
cmake -H. -Bbuild
cmake --build build
```
To run the tests
``` sh
cd build/
./navigation_test location.geojson log.json settings.json
```
## Evaluate the navigation
Quality of navigation can be evaluated via calculation of different navigational errors.
The most important among them are presented in the following table
| Error | Description |
| :--- | :---- |
| `nav_err` | Average navigation error in meters |
| `q_75_err` | 75% quantile of all the measurements have navigation error less than the value of `nav_75_err` |
| `avr_fi` | Average angle error in degrees |
| `level_err` | Percentage of wrongly determined levels |
| `exec_time` | Execution time in seconds |
## Navigation Algorithms explanation
Here are lectures on navigational algorithms used in this repository
- [Navigine's demo of WIFI-RTT](https://www.youtube.com/watch?v=VAzMhld3bH0&t)
- [Pedometer Algorithm Explained](https://www.youtube.com/watch?v=cpwrwPTqMac&t)
- [Complementary Filter](https://www.youtube.com/watch?v=49mJcFc2xXw)
- [Particle Filter](https://www.youtube.com/watch?v=JFZ-HARdZA8)
================================================
FILE: include/navigine/navigation-core/CHANGELOG.md
================================================
# Backlog of changes in navigation protocol
## Version 1.0
* Added first version of the navigation protocol
## Version 1.1
* Deleted accuracy
* Deleted battery
* Deleted height from LOCATION
================================================
FILE: include/navigine/navigation-core/README.md
================================================
Message format
============================
Each message consists of one line of json format and describes an individual measurement.
The line types are shown below.
### Type 101 - ACCELEROMETER
```{“timestamp”: <timestamp>, “type”: 101, “values”: [value_x, value_y, value_z]}```
### Type 102 - GYROSCOPE
```{“timestamp”: <timestamp>, “type”: 102, “values”: [value_x, value_y, value_z]}```
### Type 103 - MAGNETOMETER
```{“timestamp”: <timestamp>, “type”: 103, “values”: [value_x, value_y, value_z]}```
### Type 104 - BAROMETER
```{“timestamp”: <timestamp>, “type”: 104, “values”: [pressure, pressure, pressure]}```
### Type 105 - ORIENTATION
```{“timestamp”: <timestamp>, “type”: 105, “values”: [value_x, value_y, value_z]}```
### Type 106 - LOCATION
```{“timestamp”: <timestamp>, “type”: 106, “values”: [lat, lon, accuracy]}```
### Type 201 - WIFI
```{“timestamp”: <timestamp>, “type”: 201, “bssid”: <bssid>, “rssi”: <rssi>}```
### Type 202 - BLE
```{“timestamp”: <timestamp>, “type”: 202, “bssid”: <bssid>, “rssi”: <rssi>}```
### Type 203 - BEACON
```{“timestamp”: <timestamp>, “type”: 203, “bssid”: <bssid>, “rssi”: <rssi>, “power”: <power>}```
### Type 204 - EDDYSTONE
```{“timestamp”: <timestamp>, “type”: 204, “bssid”: <bssid>, “rssi”: <rssi>, “power”: <power>}```
### Type 205 - WIFI RTT
```{“timestamp”: <timestamp>, “type”: 205, “bssid”: <bssid>, “distance”: <distance>, “stddev”: <stddev>}```
### Type 300 - NMEA
```{“timestamp”: <timestamp>, “type”: 300, “sentence_number”: <sentence_number>, “num_sats”: <num_sats>}```
### Type 900 - Comment
```{“timestamp”: <timestamp>, “type”: 900, “comment”: <comment>}```
================================================
FILE: include/navigine/navigation-core/barriers_geometry_builder.h
================================================
/** barriers_geometry_builder.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H
#define NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H
#include "level_geometry.h"
namespace navigine {
namespace navigation_core {
LevelGeometry getGeometry(const std::list<Polygon>& allowedArea);
} } // namespace geometry::navigine::navigation_core
#endif // NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H
================================================
FILE: include/navigine/navigation-core/boost_geometry_adaptation.h
================================================
#pragma once
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <navigine/navigation-core/point.h>
#include "xy_particle.h"
BOOST_GEOMETRY_REGISTER_POINT_2D(navigine::navigation_core::XYPoint, double, boost::geometry::cs::cartesian, x, y);
BOOST_GEOMETRY_REGISTER_POINT_2D(navigine::navigation_core::XYParticle, double, boost::geometry::cs::cartesian, x, y);
namespace navigine {
namespace navigation_core {
using Point = boost::geometry::model::d2::point_xy<double>;
using Box = boost::geometry::model::box<Point>;
using Ring = boost::geometry::model::ring<Point>;
using Polygon = boost::geometry::model::polygon<Point>;
using Multipolygon = boost::geometry::model::multi_polygon<Polygon>;
using Linestring = boost::geometry::model::linestring<Point>;
using Multilinestring = boost::geometry::model::multi_linestring<Linestring>;
using BoxIdx = std::pair<Box, std::size_t>;
using RTree = boost::geometry::index::rtree<BoxIdx, boost::geometry::index::rstar<16, 4>>;
inline std::pair<double, double> getWidthHeightPair(const Box& box)
{
double width = box.max_corner().x() - box.min_corner().x();
double height = box.max_corner().y() - box.min_corner().y();
return std::make_pair(width, height);
}
} } //namespace navigine::navigation_core
================================================
FILE: include/navigine/navigation-core/declare_identifier.h
================================================
#pragma once
#include <string>
#define DECLARE_IDENTIFIER(IdentifierType) \
struct IdentifierType { \
std::string value; \
inline IdentifierType() = default; \
inline explicit IdentifierType(const std::string& value): value(value) {} \
inline bool isValid() const { return !value.empty(); } \
inline bool operator< (const IdentifierType& _id)const { return value < _id.value; } \
inline bool operator!= (const IdentifierType& _id)const { return value != _id.value; } \
inline bool operator== (const IdentifierType& _id)const { return value == _id.value; } \
}; \
\
struct Hasher##IdentifierType \
{ \
std::size_t operator()(const IdentifierType& k) const \
{ \
return std::hash<std::string>()(k.value); \
} \
};
================================================
FILE: include/navigine/navigation-core/geolevel.h
================================================
/** geolevel.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_GEOLEVEL_H
#define NAVIGINE_GEOLEVEL_H
#include <string>
#include <vector>
#include <map>
#include <unordered_map>
#include "level.h"
#include "level_geometry.h"
#include "boost_geometry_adaptation.h"
#include "graph.h"
#include "reference_point.h"
#include "transmitter.h"
namespace navigine {
namespace navigation_core {
struct GeoLevel
{
LevelId id;
Geo3DTransmitters transmitters;
GeoReferencePoints referencePoints;
LevelGeometry geometry;
Graph<GeoPoint> graph;
double altitude;
};
typedef std::vector<std::shared_ptr<GeoLevel> > GeoLevels;
} } // namespace navigine::navigation_core
#endif // NAVIGINE_GEOLEVEL_H
================================================
FILE: include/navigine/navigation-core/graph.h
================================================
/** graph.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_GRAPH_H
#define NAVIGINE_GRAPH_H
#include <vector>
#include <map>
#include <set>
namespace navigine {
namespace navigation_core {
template <typename Point>
class Graph
{
public:
struct Vertex
{
int id;
Point point;
};
public:
typedef typename std::vector<Vertex>::const_iterator VertexIterator;
typedef std::set<std::pair<int, int> >::const_iterator EdgeIterator;
public:
Graph() {}
bool hasVertex(int id) const
{
return id >= 0 && id < static_cast<int>(mVertices.size());
}
const Vertex& vertex(int id) const
{
return mVertices.at(id);
}
const Vertex getVertex(int id) const
{
return mVertices.at(id);
}
const VertexIterator vertexBegin() const
{
return mVertices.begin();
}
const VertexIterator vertexEnd() const
{
return mVertices.end();
}
const EdgeIterator edgesBegin() const
{
return mEdges.begin();
}
const EdgeIterator edgesEnd() const
{
return mEdges.end();
}
Vertex addVertex(const Point& p)
{
Vertex v = {static_cast<int>(mVertices.size()), p};
mVertices.push_back(v);
return v;
}
void addEdge(int v1, int v2)
{
if (!hasVertex(v1) || !hasVertex(v2)) {
throw std::logic_error("vertex does not exist!");
}
std::pair<int, int> p = v1 < v2 ? std::make_pair(v1, v2) : std::make_pair(v2, v1);
mEdges.insert(std::move(p));
}
private:
std::vector<Vertex> mVertices;
std::set<std::pair<int, int>> mEdges; // contains indices of elements in vertices vector
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_GRAPH_H
================================================
FILE: include/navigine/navigation-core/graph_particle.h
================================================
/** graph_particle.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef NAVIGINE_GRAPH_PARTICLE_H
#define NAVIGINE_GRAPH_PARTICLE_H
#include <navigine/navigation-core/xy_particle.h>
namespace navigine {
namespace navigation_core {
struct GraphParticle : XYParticle
{
GraphParticle()
: edgeId(0)
, fromVertex(0)
, toVertex(0)
, percent(0)
{ }
GraphParticle(int _edge, int _fromVertex, int _toVertex,
double _percent, double _x, double _y, double _angle)
: XYParticle(_x, _y, _angle)
, edgeId(_edge)
, fromVertex(_fromVertex)
, toVertex(_toVertex)
, percent(_percent)
{ }
int edgeId;
int fromVertex;
int toVertex;
double percent;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_GRAPH_PARTICLE_H
================================================
FILE: include/navigine/navigation-core/level.h
================================================
/** level.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_LEVEL_H
#define NAVIGINE_LEVEL_H
#include <string>
#include <vector>
#include <map>
#include <unordered_map>
#include "boost_geometry_adaptation.h"
#include "level_geometry.h"
#include "graph.h"
#include "radiomap.h"
#include "reference_point.h"
#include "transmitter.h"
#include "navigation_input.h"
namespace navigine {
namespace navigation_core {
DECLARE_IDENTIFIER(LevelId)
class Level
{
public:
Level(const LevelId& id, const GeoPoint& bindingPoint)
: mId (id)
, mBindingPoint (bindingPoint)
{}
void addTransmitters(const XYZTransmitters& transmitters);
void setReferencePoints(const XYReferencePoints& referencePoints);
void setGraph(const Graph<XYPoint>& graph);
void setGeometry(const LevelGeometry &levelGeometry);
const LevelId& id() const;
const GeoPoint& bindingPoint() const;
bool containsTransmitter(const TransmitterId& txId) const;
const Transmitter<XYZPoint>& transmitter(const TransmitterId& txId) const;
const Radiomap& radiomap() const;
const Graph<XYPoint>& graph() const;
const LevelGeometry& geometry() const;
private:
const LevelId mId;
const GeoPoint mBindingPoint;
std::unordered_map<TransmitterId, Transmitter<XYZPoint>, HasherTransmitterId> mTransmitters;
Radiomap mRadiomap;
LevelGeometry mGeometry;
Graph<XYPoint> mGraph;
};
typedef std::vector<std::unique_ptr<Level> >::const_iterator LevelIterator;
typedef std::vector<std::unique_ptr<Level> > Levels;
RadioMeasurementsData getLevelRadioMeasurements(
const Level& level,
const RadioMeasurementsData& radioMsr);
} } // namespace navigine::navigation_core
#endif // NAVIGINE_LEVEL_H
================================================
FILE: include/navigine/navigation-core/level_collector.h
================================================
/** level_collector.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_LEVEL_COLLECTOR_H
#define NAVIGINE_LEVEL_COLLECTOR_H
#include <string>
#include <vector>
#include <map>
#include <unordered_map>
#include <boost/optional.hpp>
#include "boost_geometry_adaptation.h"
#include "graph.h"
#include <navigine/navigation-core/level.h>
#include "geolevel.h"
#include "reference_point.h"
#include "transmitter.h"
namespace navigine {
namespace navigation_core {
class LevelCollector
{
public:
LevelCollector() {}
bool hasLevel(const LevelId& levelId) const;
const Level& level(const LevelId& levelId) const;
void addGeoLevel(const GeoLevel& geoLevel);
const std::vector<Level>& levels() const;
boost::optional<LevelId> findLevelByTransmitterId(const TransmitterId& transmitterId) const;
private:
std::vector<Level> mLevels;
std::unordered_map<LevelId, std::size_t, HasherLevelId> mLevelsIndices;
};
std::shared_ptr<LevelCollector> createLevelCollector();
} } // namespace navigine::navigation_core
#endif // NAVIGINE_LEVEL_COLLECTOR_H
================================================
FILE: include/navigine/navigation-core/level_geometry.h
================================================
/** level.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_LEVEL_GEOMETRY_H
#define NAVIGINE_LEVEL_GEOMETRY_H
#include "boost_geometry_adaptation.h"
#include <vector>
namespace navigine {
namespace navigation_core {
class LevelGeometry
{
public:
LevelGeometry() {};
LevelGeometry(const Multipolygon& allowedArea, const Box& box);
bool isPathTouchesBarriers(const Linestring& path) const;
const Multipolygon& allowedArea() const;
const Box& boundingBox() const;
private:
Multipolygon mAllowedArea;
Box mBoundingBox;
RTree mTree;
std::vector<Ring> mInnerBarriers;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_LEVEL_GEOMETRY_H
================================================
FILE: include/navigine/navigation-core/location_point.h
================================================
/** location_point.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef NAVIGINE_LOCATION_POINT_H
#define NAVIGINE_LOCATION_POINT_H
#include <vector>
#include <navigine/navigation-core/level.h>
namespace navigine {
namespace navigation_core {
struct LocationPoint
{
LevelId level;
double x;
double y;
};
typedef std::vector<LocationPoint> PolyLine;
} } // namespace navigine::navigation_core
#endif
================================================
FILE: include/navigine/navigation-core/logger.h
================================================
/** logger.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_LOGGER_H
#define NAVIGINE_LOGGER_H
#include <string>
#include <map>
#define LOG_MESSAGE(lvl, fmt, ...) \
do { \
navigine::navigation_core::_Logger.print(lvl, __FILE__, __LINE__, fmt, ##__VA_ARGS__); \
} while (false)
namespace navigine {
namespace navigation_core {
class Logger
{
public:
Logger() = default;
// Function is used for specifying a log file.
// If @file parameter is null, function sets a default log file.
// Otherwise, function sets a separate log file for the corresponding module.
void setLogFile(const std::string& logFile, const char* file = 0);
// Function is used for specifying a debug level.
// If @file parameter is null, function sets a default debug level.
// Otherwise, function sets a separate debug level for the corresponding module.
void setDebugLevel(int level, const char* file = 0);
void printDebugLevels(FILE* fp = stdout);
void printLogFiles(FILE* fp = stdout);
void setOutStream(FILE* os);
// Function prints a format message in printf-like way.
// @level - message debug level. If @level is greater than the debug level
// specified earlier, the message will be ignored.
// @file - module name, from where Logger has been called.
// @line - line number where Logger has been called.
// @fmt - message format string
void print(int level, const char* file, int line, const char* fmt, ...) const;
private:
FILE * mOutStream = stdout;
std::map<std::string, int> mDebugLevels = {};
std::map<std::string, std::string> mLogFiles = {};
};
extern Logger _Logger;
extern int GetLocalTime (int unixTime,
int* year, int* month, int* day,
int* hour, int* minute, int* second);
extern long long GetCurrentTimeMillis();
} } // namespace navigine::navigation_core
#endif
================================================
FILE: include/navigine/navigation-core/motion_info.h
================================================
/** motion_info.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef NAVIGINE_MOTION_INFO_H
#define NAVIGINE_MOTION_INFO_H
#include <cstddef>
namespace navigine {
namespace navigation_core {
struct MotionInfo
{
MotionInfo()
: isStepDetected ( false )
, dtSec ( 0.0 )
, deltaAngle ( 0.0 )
, distance ( 0.0 )
, stepCounter ( 0 )
, heading ( 0.0 )
, azimuth ( 0.0 )
, lastMotionTs ( 0 )
, isAzimuthValid (false)
, gyroHeading ( 0.0 )
{}
bool isStepDetected;
double dtSec;
double deltaAngle;
double distance;
size_t stepCounter;
double heading;
double azimuth;
long long lastMotionTs;
bool isAzimuthValid;
double gyroHeading;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_MOTION_INFO_H
================================================
FILE: include/navigine/navigation-core/navigation_client.h
================================================
#pragma once
#include "navigation_input.h"
#include "navigation_output.h"
#include "navigation_state.h"
#include "navigation_settings.h"
#include "level_collector.h"
namespace navigine {
namespace navigation_core {
class NavigationClient
{
public:
// Return navigation state of this client
virtual std::vector<NavigationState> getStates() const = 0;
// Main navigation function. Calculates current position based on incoming measurements
virtual std::vector<NavigationOutput> navigate(const std::vector<Measurement>& navInput) = 0;
virtual ~NavigationClient() {}
};
std::shared_ptr<NavigationClient> createNavigationClient(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
} } // namespace navigine::navigation_core
================================================
FILE: include/navigine/navigation-core/navigation_input.h
================================================
/** navigation_input.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef NAVIGINE_NAVIGATION_INPUT_H
#define NAVIGINE_NAVIGATION_INPUT_H
#define _CRTDBG_MAP_ALLOC
#include <string>
#include <vector>
#include "boost/variant.hpp"
#include "vector3d.h"
#include "transmitter.h"
namespace navigine {
namespace navigation_core {
template <typename MeasurementData>
struct MeasurementDataWithTimeStamp
{
MeasurementData data;
long long ts;
};
struct RadioMeasurementData
{
enum class Type {
WIFI,
BEACON,
BLUETOOTH,
EDDYSTONE,
WIFI_RTT
};
Type type;
TransmitterId id;
double rssi = 0;
double power = 0;
double distance = 0;
double stddev = 0;
};
struct SensorMeasurementData
{
enum class Type {
ACCELEROMETER,
MAGNETOMETER,
GYROSCOPE,
BAROMETER,
LOCATION,
ORIENTATION
};
Type type;
Vector3d values;
};
struct NmeaMeasurementData
{
int sentenceNumber;
int satellitesNumber;
};
using RadioMeasurementsData = std::vector<RadioMeasurementData>;
using MeasurementData = boost::variant<RadioMeasurementData, SensorMeasurementData, NmeaMeasurementData>;
using Measurement = MeasurementDataWithTimeStamp<MeasurementData>;
} } // namespace navigine::navigation_core
#endif
================================================
FILE: include/navigine/navigation-core/navigation_output.h
================================================
#ifndef NAVIGINE_NAVIGATION_OUTPUT_H
#define NAVIGINE_NAVIGATION_OUTPUT_H
#include <string>
#include <vector>
#include <navigine/navigation-core/level.h>
namespace navigine {
namespace navigation_core {
enum NavigationStatus : int
{
OK = 0,
NAVIGATION_ERROR = -1,
NO_SOLUTION = 4,
NO_RPS = 7,
NO_LEVEL = 30,
PF_MUTATION_FAILED = 32,
PF_SAMPLING_FAILED = 33,
};
enum class Provider { GNSS, INDOOR, FUSED, NONE };
struct NavigationOutput
{
NavigationStatus status;
LevelId posLevel; // Level identifier
double posLatitude = 0.0;
double posLongitude = 0.0;
double posAltitude = 0.0;
double posR; // Output radius (meters)
double posOrientation; // Output orientation in radians (clockwise)
Provider provider;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_NAVIGATION_OUTPUT_H
================================================
FILE: include/navigine/navigation-core/navigation_settings.h
================================================
#ifndef NAVIGINE_NAVIGATION_SETTINGS_H
#define NAVIGINE_NAVIGATION_SETTINGS_H
#include <map>
#include <vector>
#include <memory>
#include <sstream>
#include <navigine/navigation-core/level.h>
namespace navigine {
namespace navigation_core {
struct CommonSettings {
enum class UseAlgorithm { PF_PDR, PF_NOPDR, ZONES, GRAPH, KNN }; //PF_PDR, PF_NOPDR, ZONES, GRAPH, KNN
enum class MeasurementType { RSSI, DISTANCE_V1, DISTANCE_V2 }; // rssi, distance_21, distance_v2
enum class SignalsToUse { BLE, WIFI, BOTH }; //ble, wifi, both
UseAlgorithm useAlgorithm = UseAlgorithm::PF_PDR; // "Use_algorithm"
double lklCorrectionNoise = 10.0; // "Correction_noise"
bool useEnuAzimuth = false; // "Use_enu_azimuth"
bool useDifferentionMode = false; // "Differention_mode"
bool useTxPower = false; // "Use_beacon_power"
bool useRadiomap = false; // "Use_radiomap"
bool useStops = false; // "Use_stops"
bool useTracking = false; // "Use_tracking"
double stopDetectionTime = 3.0; // "Stop_detection_time"
double minDeviationM = 1.0; // "Min_deviation_m"
double averageMovSpeed = 1.0; // "Average_speed"
MeasurementType measurementType = MeasurementType::RSSI; // "Measurement_type"
bool useBestRefPointLevel = false; // "Use_best_ref_point_level"
bool useBarometer = false; // "Use_barometer"
bool useUnknownTransmitters = false; // "Use_unknown_beacons"
double sigCutOffRss = -100; // "Cutoff_rss"
bool useClosestAps = false; // "Use_closest_aps"
unsigned int numClosestAps = 1; // "N_closest_aps"
double sigAveragingTime = 1.0; // "Sig_averaging_time"
double sigWindowShift = 0.0; // "Sig_window_shift"
SignalsToUse signalsToUse = SignalsToUse::BLE; // "Use_Signals"
unsigned int numParticles = 1000; // "N_particles"
unsigned int minMsrNumForMutation = 3; // "Min_msr_num_for_mutation"
double resamplingThreshold = 600.0; // "Resampling_threshold_num", Level
unsigned int minNumParticlesAlive = 100; // "Min_num_of_particles", Level
double gyroNoiseDegree = 4.0; // "Gyro_noise_degree"
double stepNoiseDeviationMeters = 0.4; // "Step_noise_deviation_meters"
bool useProjectionToWhiteArea = false; // "Use_projection_to_white_area"
unsigned int numParticlesAroundClosestAp = 10; // "N_particles_around_closest_AP"
unsigned int numParticlesToMutate = 50; // "N_particles_to_mutate"
unsigned int numSamplingAttempts = 60; // "Num_sampling_attempts"
unsigned int numInitializationAttempts = 500; // "Num_initialization_attempts"
bool useUniformSampling = false; // "Use_uniform_sampling"
unsigned int kNeighbors = 8; // "K_neighbors"
bool useTriangles = true; // "Use_triangles"
bool useDiffMode = true; // "Use_diff_mode"
unsigned int minNumOfMeasurementsForPositionCalculation = 4; // "Min_num_of_measurements_for_position_calculation"
double smoothing = 0.5; // "Smoothing"
double deadReckoningTime = 5.0; // "Dead_reckon_time"
double stopUpdateTime = 4.0; // "Stop_update_time"
double graphProjDist = 5.0; // "Graph_prog_distance"
double useStopsDistanceThresholdM = 10.0; // "Stopped_distance_threshold_m"
// if distance between stopped position and current position
// is more than the threshold - update stopped position
bool useSmoothing = true; // "Use_smoothing"
bool useGraphProjection = false; // "Use_graph_projection"
bool useAccuracyForSmoothing = false; // "Use_accuracy_for_smoothing"
bool useSpeedConstraint = true; // "Use_speed_constraint"
bool useBarriers = false; // "Use_barriers"
bool useTimeSmoothing = false; // "Use_time_smoothing"
bool useAbFilter = true; // "Use_ab_filter"
bool useAltitude = false; // "Use_altitude"
bool useGps = false; // "Use_gps"
bool useInstantGpsPosition = false; // "Use_instant_gps_position"
bool preferIndoorSolution = false; // "Prefer_indoor_solution"
bool fuseGps = false; // "Fuse_gps"
bool useGpsOutsideMap = false; // "Use_gps_outside_map"
bool useGpsSigmaFilter = false; // "Use_gps_sigma_filter"
unsigned int sigmaFilterWindow = 1; // "Sigma_filter_window"
double sigmaFilterDelta = 0.0; // "Sigma_filter_delta"
unsigned int minNumSats = 20; // "Min_num_sats"
double priorDeviation = 50.0; // "Prior_deviation"
double minGpsDeviation = 5.0; // "Gps_deviation"
double maxGpsDeviation = std::numeric_limits<double>::infinity(); // Maximum_to_accept_gps_measurement"
double fuseGpsBorderM = 10; // "Fuse_gps_border"
long long noSignalTimeThreshold = 35; // "No_signal_time_threshold"
long long noActionTimeThreshold = 20; // "No_action_time_threshold"
double positionIsTooOldSec = 20.0; // "Position_old_for_fusing_sec"
double gpsValidTimeWindowSec = 1.0; // "Gps_valid_time_window_sec"
bool useCalculatedAzimuth = false; // "Use_calculated_azimuth"
double deviceAzimuthLifetimeSeconds = 10.0; // "Device_azimuth_lifetime_seconds"
double stepMultiplier = 1.0; // "Step_multiplier"
bool hasAccelerometer = true; // "Has_accelerometer"
bool hasGyroscope = true; // "Has_gyroscope"
double wiFiRttOffset = -1.5; // "WiFi_RTT_Offset"
};
struct LevelSettings {
double normalModeA = -82.0; // "GP_Normal_Mode", "A", Level
double normalModeB = 3.0; // "GP_Normal_Mode", "B", Level
};
struct NavigationSettings
{
CommonSettings commonSettings;
std::map<LevelId, LevelSettings> levelsSettings;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_NAVIGATION_SETTINGS_H
================================================
FILE: include/navigine/navigation-core/navigation_state.h
================================================
/** navigation_state.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef NAVIGINE_NAVIGATION_STATE_H
#define NAVIGINE_NAVIGATION_STATE_H
#include <vector>
#include "navigation_input.h"
#include <navigine/navigation-core/point.h>
#include "xy_particle.h"
namespace navigine {
namespace navigation_core {
class NavigationState
{
public:
NavigationState() { }
NavigationState(const std::vector<XYParticle>& particles,
const std::vector<double>& weights,
const RadioMeasurementsData& radioMsr,
const std::vector<XYParticle>& likelihoodPoints,
const std::vector<double>& likelihoodWeights,
double heading = 0,
double stepLen = 0,
int stepCounter = 0)
: mParticles ( particles )
, mParticleWeights ( weights )
, mMeasurements ( radioMsr )
, mLikelihoodMapPoints ( likelihoodPoints )
, mLikelihoodMapWeights ( likelihoodWeights )
, mHeading ( heading )
, mStepLen ( stepLen )
, mStepCounter ( stepCounter )
{ }
void setStepCounter (double stepCounter) { mStepCounter = stepCounter; }
void setStepLen (double stepLen) { mStepLen = stepLen; }
void setIndoorPosition (const XYPoint& pos) { mIndoorPosition = pos; }
void setOutdoorPosition (const XYPoint& pos) { mOutdoorPosition = pos; }
XYPoint getIndoorPosition ( void ) const { return mIndoorPosition; }
XYPoint getOutdoorPosition ( void ) const { return mOutdoorPosition; }
int getStepCounter ( void ) const { return mStepCounter; }
double getHeading ( void ) const { return mHeading; }
double getStepLen ( void ) const { return mStepLen; }
std::vector<XYParticle> getParticles ( void ) const { return mParticles; }
std::vector<double> getParticleWeights ( void ) const { return mParticleWeights; }
RadioMeasurementsData getLevelRadioMeasurements ( void ) const { return mMeasurements; }
std::vector<XYParticle> getLikelihoodMapPoints ( void ) const { return mLikelihoodMapPoints; }
std::vector<double> getLikelihoodMapWeights ( void ) const { return mLikelihoodMapWeights; }
private:
std::vector<XYParticle> mParticles;
std::vector<double> mParticleWeights;
RadioMeasurementsData mMeasurements;
std::vector<XYParticle> mLikelihoodMapPoints;
std::vector<double> mLikelihoodMapWeights;
double mHeading;
double mStepLen;
int mStepCounter;
XYPoint mIndoorPosition;
XYPoint mOutdoorPosition;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_NAVIGATION_STATE_H
================================================
FILE: include/navigine/navigation-core/point.h
================================================
/** point.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef NAVIGINE_POINT_H
#define NAVIGINE_POINT_H
namespace navigine {
namespace navigation_core {
struct GeoPoint
{
GeoPoint()
: latitude ( 0.0 )
, longitude( 0.0 )
{ }
GeoPoint(double _latitude, double _longitude)
: latitude ( _latitude )
, longitude( _longitude )
{ }
double latitude;
double longitude;
};
struct GeoPoint3D
{
GeoPoint3D()
: latitude ( 0.0 )
, longitude( 0.0 )
, altitude ( 0.0 )
{ }
GeoPoint3D(double _latitude, double _longitude, double _altitude)
: latitude ( _latitude )
, longitude( _longitude )
, altitude ( _altitude )
{ }
double latitude;
double longitude;
double altitude;
};
struct XYPoint
{
XYPoint()
: x( 0.0 )
, y( 0.0 )
{ }
XYPoint(double _x, double _y)
: x( _x )
, y( _y )
{ }
double x;
double y;
};
struct XYZPoint
{
XYZPoint()
: x( 0.0 )
, y( 0.0 )
, z( 0.0 )
{ }
XYZPoint(double _x, double _y, double _z)
: x( _x )
, y( _y )
, z( _z )
{ }
double x;
double y;
double z;
};
XYPoint gpsToLocal(const GeoPoint& point, const GeoPoint& bindPoint);
GeoPoint localToGps(const XYPoint& localPoint, const GeoPoint& bindPoint);
} } // namespace navigine::navigation_core
#endif // NAVIGINE_POINT
================================================
FILE: include/navigine/navigation-core/radiomap.h
================================================
/** radiomap.h
*
* Copyright (c) 2019 Navigine.
*
*/
#ifndef NAVIGINE_RADIOMAP_H
#define NAVIGINE_RADIOMAP_H
#include <map>
#include "reference_point.h"
#include "transmitter.h"
namespace navigine {
namespace navigation_core {
class Radiomap
{
public:
Radiomap(const XYReferencePoints& referencePoints = {});
const XYReferencePoints& referencePoints() const;
ReferencePoint<XYPoint> getReferencePoint(const ReferencePointId& refPointId) const;
bool hasReferencePoint(const ReferencePointId& refPointId) const;
private:
XYReferencePoints mReferencePoints;
std::map<ReferencePointId, size_t> mReferencePointsIndex; // {id -> index}
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_RADIOMAP_H
================================================
FILE: include/navigine/navigation-core/reference_point.h
================================================
/** reference_point.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef NAVIGINE_REFERENCE_POINT_H
#define NAVIGINE_REFERENCE_POINT_H
#include <ctime>
#include <string>
#include <map>
#include "transmitter.h"
namespace navigine {
namespace navigation_core {
struct SignalStatistics
{
TransmitterType type;
double mean;
double variance;
size_t nMeasurements;
void addMeasurement(double meas);
};
DECLARE_IDENTIFIER(ReferencePointId)
template <typename Point>
struct ReferencePoint
{
ReferencePoint(
ReferencePointId _id,
Point _point,
const std::map<TransmitterId, SignalStatistics>& _fingerprints)
: id(_id)
, point(_point)
, fingerprints(_fingerprints)
{}
ReferencePointId id;
Point point;
std::map<TransmitterId, SignalStatistics> fingerprints;
};
typedef std::vector<ReferencePoint<XYPoint>> XYReferencePoints;
typedef std::vector<ReferencePoint<GeoPoint>> GeoReferencePoints;
} } // namespace navigine::navigation_core
#endif // NAVIGINE_REFERENCE_POINT_H
================================================
FILE: include/navigine/navigation-core/transmitter.h
================================================
#pragma once
#include <string>
#include <vector>
#include <cmath>
#include "declare_identifier.h"
#include <navigine/navigation-core/point.h>
namespace navigine {
namespace navigation_core {
struct PathlossModel
{
double A;
double B;
double power;
inline double operator()(double r) const {
static constexpr double MIN_DISTANCE = 0.01;
return A - B * std::log(std::max(r, MIN_DISTANCE));
}
};
enum class TransmitterType { WIFI, BEACON, BLUETOOTH, UNKNOWN, EDDYSTONE, LOCATOR };
DECLARE_IDENTIFIER(TransmitterId)
template <typename Point>
struct Transmitter
{
Transmitter() = default;
Transmitter(TransmitterId _id, Point _p, PathlossModel _model, TransmitterType _type)
: id ( _id )
, point ( _p )
, pathlossModel ( _model )
, type ( _type )
{ }
TransmitterId id;
Point point;
PathlossModel pathlossModel;
TransmitterType type;
bool operator== (const Transmitter& tx)const { return id == tx.id; }
bool operator< (const Transmitter& tx)const { return id < tx.id; }
};
typedef std::vector<Transmitter<XYZPoint>> XYZTransmitters;
typedef std::vector<Transmitter<GeoPoint3D>> Geo3DTransmitters;
} } // namespace navigine::navigation_core
================================================
FILE: include/navigine/navigation-core/vector3d.h
================================================
/** vector3d.h
*
** Copyv2 (c) 2018 Navigine. All v2s reserved.
*
*/
#ifndef NAVIGINE_VECTOR3D_H
#define NAVIGINE_VECTOR3D_H
#include <cmath>
namespace navigine {
namespace navigation_core {
struct Vector3d
{
double x;
double y;
double z;
Vector3d(): x(0.0), y(0.0), z(0.0) { }
Vector3d(double _x, double _y, double _z);
double magnitude() const;
Vector3d normalized() const;
Vector3d& normalize();
bool isNull() const;
static Vector3d crossProduct(const Vector3d& v1, const Vector3d& v2);
static double dotProduct (const Vector3d& v1, const Vector3d& v2);
bool operator==(const Vector3d& v2);
bool operator!=(const Vector3d& v2);
Vector3d& operator+=(const Vector3d& v2);
Vector3d& operator-=(const Vector3d& v2);
Vector3d& operator*=(double multiplier);
Vector3d& operator/=(double divisor);
};
inline Vector3d operator*(double multiplier, const Vector3d& v)
{
return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);
}
inline Vector3d operator*(const Vector3d& v, double multiplier)
{
return Vector3d(v.x * multiplier, v.y * multiplier, v.z * multiplier);
}
inline Vector3d operator+(const Vector3d& v1, const Vector3d& v2)
{
return Vector3d(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z);
}
inline Vector3d operator-(const Vector3d& v1, const Vector3d& v2)
{
return Vector3d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}
inline Vector3d operator-(const Vector3d& v)
{
return Vector3d(-v.x, -v.y, -v.z);
}
inline Vector3d operator/(const Vector3d& v1, double divisor)
{
return Vector3d(v1.x / divisor, v1.y / divisor, v1.z / divisor);
}
} } // namespace navigine::navigation_core
#endif // NAVIGINE_VECTOR3D_H
================================================
FILE: include/navigine/navigation-core/version.h
================================================
#ifndef NAVIGINE_VERSION_H
#define NAVIGINE_VERSION_H
#define NAVIGATION_LIBRARY_VERSION "2.0" // 09 July 2019
#endif
================================================
FILE: include/navigine/navigation-core/xy_particle.h
================================================
/** xy_particle.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef NAVIGINE_PARTICLE_H
#define NAVIGINE_PARTICLE_H
#include <navigine/navigation-core/point.h>
namespace navigine {
namespace navigation_core {
struct XYParticle : XYPoint
{
XYParticle()
: XYPoint ( 0.0, 0.0 )
, angle ( 0.0 )
{ }
XYParticle(double _x, double _y, double _angle)
: XYPoint( _x, _y )
, angle ( _angle )
{ }
double angle;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_PARTICLE
================================================
FILE: src/README
================================================
the implementations of navigation algorithms developed by Navigine
================================================
FILE: src/barriers_geometry_builder.cpp
================================================
/** barriers_geometry_builder.cpp
*
* Copyright (c) 2019 Navigine.
*
*/
#include <iostream>
#include <navigine/navigation-core/barriers_geometry_builder.h>
namespace navigine {
namespace navigation_core {
//TODO test for empty barrier list
LevelGeometry getGeometry(const std::list<Polygon>& allowedArea)
{
Multipolygon allowedFullArea;
for (const auto& area: allowedArea)
{
Polygon correctedArea = area;
boost::geometry::correct(correctedArea);
allowedFullArea.push_back(correctedArea);
}
return {allowedFullArea, boost::geometry::return_envelope<Box>(allowedFullArea)};
}
} } // namespace navigine::navigation_core
================================================
FILE: src/device_properties.h
================================================
#ifndef NAVIGINE_DEVICE_PROPERTIES_H
#define NAVIGINE_DEVICE_PROPERTIES_H
#include <string>
namespace navigine {
namespace navigation_core {
class DeviceProperties
{
public:
DeviceProperties() { }
DeviceProperties(const std::string& _id, const std::string& _model,
bool _hasAcc = false,
bool _hasGyro = false,
bool _hasMagn = false,
bool _hasBaro = false)
: mId ( _id )
, mModel ( _model )
, mHasAccel ( _hasAcc )
, mHasGyro ( _hasGyro )
, mHasMagn ( _hasMagn )
, mHasBaro ( _hasBaro )
{ }
public:
std::string getId ( void ) const { return mId; }
std::string getModel ( void ) const { return mModel; }
bool hasAccel ( void ) const { return mHasAccel; }
bool hasGyro ( void ) const { return mHasGyro; }
bool hasMagn ( void ) const { return mHasMagn; }
bool hasBaro ( void ) const { return mHasBaro; }
private:
std::string mId = ""; // Client ID
std::string mModel = ""; // Telephone model
bool mHasAccel = false;
bool mHasGyro = false;
bool mHasMagn = false;
bool mHasBaro = false;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_DEVICE_PROPERTIES_H
================================================
FILE: src/geometry.cpp
================================================
#include <cmath>
#include "geometry.h"
namespace navigine {
namespace navigation_core {
// Get distance from A to B
double GetDist(double ax, double ay, double bx, double by)
{
return std::sqrt((ax - bx) * (ax - bx) + (ay - by) * (ay - by));
}
// Get distance from A to B
double GetDist(const LocationPoint& A, const LocationPoint& B)
{
return A.level == B.level ? GetDist(A.x, A.y, B.x, B.y) : NAN;
}
// Get distance from segment to point
double SegmentPointDist(const XYPoint& p1, const XYPoint& p2, const XYPoint& p)
{
double v1x = p.x - p1.x;
double v1y = p.y - p1.y;
double v2x = p2.x - p1.x;
double v2y = p2.y - p1.y;
double len = v2x * v2x + v2y * v2y;
if (len < GEOMETRY_DOUBLE_EPSILON)
return GetDist(p1.x, p1.y, p.x, p.y);
double t = (v1x * v2x + v1y * v2y) / len;
t = std::min(std::max(t, 0.0), 1.0);
double sx = p1.x + t * (p2.x - p1.x);
double sy = p1.y + t * (p2.y - p1.y);
double dist = std::sqrt((sx - p.x) * (sx - p.x) + (sy - p.y) * (sy - p.y));
return dist;
}
// Get distance from C to line (AB)
double GetDist(const LocationPoint& A, const LocationPoint& B, const LocationPoint& C)
{
if (A.level != B.level || A.level != C.level)
return NAN;
double d = GetDist(A, B);
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;
}
// Calculate the oriented area of triangle (ABC).
extern double GetArea(const LocationPoint& A,
const LocationPoint& B,
const LocationPoint& C)
{
if (A.level != B.level || A.level != C.level)
return NAN;
return GetArea(A.x, A.y, B.x, B.y, C.x, C.y);
}
// Calculate the oriented area of triangle (ABC).
extern double GetArea(double ax, double ay,
double bx, double by,
double cx, double cy)
{
return Determinant(ax - bx, ay - by, ax - cx, ay - cy) / 2;
}
// Calculate determinant of the 2x2 matrix:
// a11 a12
// a21 a22
double Determinant(double a11, double a12,
double a21, double a22)
{
return a11 * a22 - a12 * a21;
}
// Calculate determinant of the 3x3 matrix:
// a11 a12 a13
// a21 a22 a23
// a31 a32 a33
double Determinant(double a11, double a12, double a13,
double a21, double a22, double a23,
double a31, double a32, double a33)
{
return (a11 * a22 * a33 + a12 * a23 * a31 + a21 * a32 * a13) -
(a21 * a12 * a33 + a11 * a23 * a32 + a13 * a22 * a31);
}
// Get projection from (ox,oy) to segment [(ax,ay), (bx,by)].
double GetProjection(double ax, double ay,
double bx, double by,
double ox, double oy,
double* px, double* py)
{
// Find such point P, that vector OP is orthogonal to the vector AB
// Decompose vector OP for basis (OA, OB):
// OP = (1-k) * OA + k * OB
// Return value: k
double ab = ((bx - ax) * (bx - ax) + (by - ay) * (by - ay));
double ao = ((ox - ax) * (bx - ax) + (oy - ay) * (by - ay));
double k = ab > GEOMETRY_DOUBLE_EPSILON ? ao / ab : 0.0;
if (px) { *px = ax + k * (bx - ax); }
if (py) { *py = ay + k * (by - ay); }
return k;
}
double GetIntersection(double px, double py,
double qx, double qy,
double ax, double ay,
double bx, double by,
double* ox, double* oy)
{
double pqx = qx - px;
double pqy = qy - py;
double abx = bx - ax;
double aby = by - ay;
double pax = ax - px;
double pay = ay - py;
double det = Determinant(pqx, abx, pqy, aby);
if (std::fabs(det) < GEOMETRY_DOUBLE_EPSILON)
return NAN;
double k = Determinant(pax, abx, pay, aby) / det;
double l = -Determinant(pqx, pax, pqy, pay) / det;
if (k < 0 || k > 1.0 || l < 0 || l > 1.0)
return NAN;
if (ox) { *ox = px + k * pqx; }
if (oy) { *oy = py + k * pqy; }
return k;
}
// Check if segments [AB] and [CD] has intersection
bool CheckIntersection(double ax, double ay,
double bx, double by,
double cx, double cy,
double dx, double dy)
{
auto boundBoxCheck = [](double a, double b, double c, double d) -> bool
{
if (a > b) std::swap(a, b);
if (c > d) std::swap(c, d);
return std::max(a,c) < std::min(b,d);
};
return boundBoxCheck(ax, bx, cx, dx) &&
boundBoxCheck(ay, by, cy, dy) &&
GetArea(ax, ay, bx, by, cx, cy) * GetArea(ax, ay, bx, by, dx, dy) < 0 &&
GetArea(cx, cy, dx, dy, ax, ay) * GetArea(cx, cy, dx, dy, bx, by) < 0;
}
bool PointOnLine(double x, double y, double x1, double y1, double x2, double y2)
{
const double Ax = x1 - x;
const double Ay = y1 - y;
const double Bx = x2 - x;
const double By = y2 - y;
return (std::fabs(Ax * By - Ay * Bx) < GEOMETRY_DOUBLE_EPSILON &&
Ax * Bx + Ay * By < GEOMETRY_DOUBLE_EPSILON);
}
bool XRayIntersectsLine(double x, double y, double x1, double y1, double x2, double y2)
{
static const double DELTA = std::sqrt(GEOMETRY_DOUBLE_EPSILON);
if (std::fabs(y - y1) < GEOMETRY_DOUBLE_EPSILON)
y1 += DELTA;
if (std::fabs(y - y2) < GEOMETRY_DOUBLE_EPSILON)
y2 += DELTA;
if (y1 < y && y < y2)
return Determinant(x2, y2, 1, x, y, 1, x1, y1, 1) + GEOMETRY_DOUBLE_EPSILON > 0;
if (y2 < y && y < y1)
return Determinant(x1, y1, 1, x, y, 1, x2, y2, 1) + GEOMETRY_DOUBLE_EPSILON > 0;
return false;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/geometry.h
================================================
/** geometry.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef NAVIGINE_GEOMETRY_H
#define NAVIGINE_GEOMETRY_H
#include <navigine/navigation-core/location_point.h>
namespace navigine {
namespace navigation_core {
static const double GEOMETRY_DOUBLE_EPSILON = 1e-8;
// Get distance from segment to point
extern double SegmentPointDist(const XYPoint &p1,
const XYPoint &p2,
const XYPoint &p);
// Get distance from A to B
extern double GetDist(const LocationPoint& A, const LocationPoint& B);
extern double GetDist(double ax, double ay, double bx, double by);
// Get distance from C to line (AB)
extern double GetDist(const LocationPoint& A,
const LocationPoint& B,
const LocationPoint& C);
// Calculate the oriented area of triangle (ABC).
extern double GetArea(const LocationPoint& A,
const LocationPoint& B,
const LocationPoint& C);
// Calculate the oriented area of triangle (ABC).
extern double GetArea(double ax, double ay,
double bx, double by,
double cx, double cy);
// Calculate determinant of the 2x2 matrix:
// a11 a12
// a21 a22
extern double Determinant(double a11, double a12,
double a21, double a22);
// Calculate determinant of the 3x3 matrix:
// a11 a12 a13
// a21 a22 a23
// a31 a32 a33
extern double Determinant(double a11, double a12, double a13,
double a21, double a22, double a23,
double a31, double a32, double a33);
// Get distance from O to segment [AB].
// Px - is the closest point on [AB].
extern double GetProjection(double ax, double ay,
double bx, double by,
double ox, double oy,
double* px, double* py);
// Get intersection of vector PQ and segment [AB].
extern double GetIntersection(double px, double py,
double qx, double qy,
double ax, double ay,
double bx, double by,
double* x, double* y);
// Check if segments [AB] and [CD] has intersection
extern bool CheckIntersection(double ax, double ay,
double bx, double by,
double cx, double cy,
double dx, double dy);
extern bool PointOnLine ( double x, double y, double x1, double y1, double x2, double y2 );
extern bool XRayIntersectsLine ( double x, double y, double x1, double y1, double x2, double y2 );
} } // namespace navigine::navigation_core
#endif
================================================
FILE: src/knn/knn_utils.cpp
================================================
/** knn_utils.cpp
*
* Copyright (c) 2018 Navigine.
*
*/
#include <iterator>
#include <fstream>
#include <tuple>
#include "knn_utils.h"
namespace navigine {
namespace navigation_core {
namespace {
static constexpr double MIN_WEIGHT_EPSILON = 1e-8;
static constexpr bool USE_AP_WEIGHTS = true;
static constexpr bool USE_TRIANGULATION = true;
static constexpr int MIN_NUM_VISIBLE_BS_IN_RP = 3; // should be more than 0!!!
static constexpr double RSSI_ON_METER = -10.0;
static constexpr double MIN_RP_WEIGHT = 1e-6; // we skip RPs with 0 weights because they are most improbable,
// but if all signals coincide to map, we should add small additive
static constexpr double GAMMA = 3.0;
static constexpr double G_WEIGHT_COEF = 1.8; // Coef to find weight of AP (See Accuracy of RSS-Based Centroid Localization
// Algorithms in an Indoor Environment
} //namespace
std::map<LevelId, std::vector<RefPointsTriangle>> triangulate(const std::shared_ptr<LevelCollector>& levelCollector)
{
std::map<LevelId, std::vector<RefPointsTriangle>> levelTriangles;
#ifdef DEBUG_OUTPUT_TRIANGULATION
std::ofstream debugOs;
std::string fileName = DEBUG_OUTPUT_TRIANGULATION;
debugOs.open(fileName);
debugOs << "level ax ay bx by cx cy " << std::endl;
debugOs.close();
#endif
for (const Level& level : levelCollector->levels())
{
const XYReferencePoints& referencePoints = level.radiomap().referencePoints();
if (referencePoints.size() > 3)
{
std::vector<TriangleVertex> vertices;
for(const ReferencePoint<XYPoint>& rp: referencePoints)
{
TriangleVertex v = TriangleVertex(rp.point.x, rp.point.y, rp.id);
vertices.push_back(v);
}
auto widthHeightPair = getWidthHeightPair(level.geometry().boundingBox());
double maxedge = std::max(widthHeightPair.first, widthHeightPair.second);
std::vector<Triangle> triangles = TriangulateVertices(vertices, maxedge);
if (triangles.empty())
{
// triangulation failed!
return levelTriangles;
}
#ifdef DEBUG_OUTPUT_TRIANGULATION
std::ofstream debugOs;
std::string fileName = DEBUG_OUTPUT_TRIANGULATION;
debugOs.open(fileName, std::ios_base::app);
#endif
std::vector<RefPointsTriangle> refPointsTriangles;
for (auto it = triangles.begin(); it != triangles.end(); it++)
{
#ifdef DEBUG_OUTPUT_TRIANGULATION
Triangle& t = *it;
debugOs << level.id << " "
<< t.getA().x << " " << t.getA().y << " "
<< t.getB().x << " " << t.getB().y << " "
<< t.getC().x << " " << t.getC().y << std::endl;
#endif
RefPointsTriangle rpTriangle = {it->getA().id, it->getB().id, it->getC().id};
refPointsTriangles.push_back(rpTriangle);
}
#ifdef DEBUG_OUTPUT_TRIANGULATION
debugOs.close();
#endif
levelTriangles.insert(std::make_pair(level.id(), refPointsTriangles));
}
}
return levelTriangles;
}
std::map<LevelId,
std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>>>
getLevelsRadiomaps(const std::shared_ptr<LevelCollector> &levelCollector)
{
std::map<LevelId,
std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>>>
levelsRadiomaps;
for (const Level& level : levelCollector->levels())
{
std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>> bssidRpToSigstatMap;
const auto& levelId = level.id();
for (const ReferencePoint<XYPoint>& rp : level.radiomap().referencePoints())
{
const std::map<TransmitterId, SignalStatistics>& rpFingerprint = rp.fingerprints;
for (const auto& bssidSigStatPair: rpFingerprint)
{
bssidRpToSigstatMap.insert({bssidSigStatPair.first,
std::make_pair(rp.id, bssidSigStatPair.second)});
}
}
levelsRadiomaps[levelId] = bssidRpToSigstatMap;
}
return levelsRadiomaps;
}
std::map<LevelId, std::set<TransmitterId>> getLevelReferenceTransmittersMap(const std::shared_ptr<LevelCollector> &levelCollector)
{
std::map<LevelId, std::set<TransmitterId>> levelTransmitters;
for (const auto& level : levelCollector->levels())
{
const LevelId levelId = level.id();
for (const ReferencePoint<XYPoint>& rp : level.radiomap().referencePoints())
{
const std::map<TransmitterId, SignalStatistics>& rpFingerprint = rp.fingerprints;
std::set<TransmitterId> rpTransmittersIds;
std::transform(rpFingerprint.begin(), rpFingerprint.end(),
std::inserter(rpTransmittersIds, rpTransmittersIds.end()),
[](const auto& p){ return p.first; });
if (rpTransmittersIds.empty())
continue;
if (levelTransmitters.find(levelId) == levelTransmitters.end())
levelTransmitters.insert({levelId, rpTransmittersIds});
else
levelTransmitters[levelId].insert(rpTransmittersIds.begin(), rpTransmittersIds.end());
}
}
return levelTransmitters;
}
/**
* @brief calculate weight of the RadioSignal
*
* The function weight the RadioSignal from AP based on this RSSI value.
* The stronger the signal the bigger weight it will get.
* The RSSI at some distance d is: RSSI = A + B * gamma * lg (d / d0)
* RSSI1 - RSSI2 = B * gamma * lg (d1 / d2) and d1 can be found when d2 = 1 meter.
* The rssi at 1 meter distance from the transmitter assumed to be defined in parameter.
* Gamma is the pass loss exponent, or attenuation coefficient.
* For G_WEIGHT_COEF see Accuracy of RSS-Based Centroid Localization Algorithms in an Indoor Environment
*
* @param msr RadioMeasurement either from the access point
*
* @return @warning unnormalized weight of the measurement.
*/
static double calculateTransmitterTrustworthiness(const RadioMeasurementData& msr)
{
double B = 10 * GAMMA;
double txPower = RSSI_ON_METER;
double gCoef = G_WEIGHT_COEF;
txPower = msr.power < 0.00001 ? RSSI_ON_METER : msr.power;
double distance = pow(10, (txPower - msr.rssi) / B); //double apWeight = exp((aps[i].rssi - txPower) / B);
double apWeight = 1 / pow(distance, gCoef);
return apWeight;
}
static std::map<TransmitterId, double> calcTransmittersWeights(
const RadioMeasurementsData& msr)
{
// Calculate AP weights in the basis of signal strength
// We should also take into account number of incoming measurements during weights calculation!!!!!!!!!!!!
// We call this function after averageDuplicateMeasurements
std::map<TransmitterId, double> trWeightMap;
double trWeightSum = 0.0;
for (const auto& m : msr)
{
const TransmitterId& trId = m.id;
double trWeight = USE_AP_WEIGHTS ? calculateTransmitterTrustworthiness(m) : 1.0;
trWeightMap[trId] = trWeight;
trWeightSum += trWeight;
}
for (auto& tp : trWeightMap)
{
tp.second /= trWeightSum;
}
return trWeightMap;
}
/**
* This function calculates weights of RPs on the basis of RSS measurements and
* radio map of the building.
* The radiomap is multimap of transmitter as keys
* and pairs of reference points ids and corresponding signal statistics as values
*/
std::map<ReferencePointId, double> calcRpWeightsKnn(
const std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>>& radiomap,
const RadioMeasurementsData& msr,
bool useDiffMode)
{
struct SigStat { TransmitterId txId; double rssiMeasured; double rssiDatabase;};
std::map<ReferencePointId, std::vector<SigStat>> rpsStatistics;
for (const auto& curMsr : msr)
{
auto range = radiomap.equal_range(curMsr.id);
for (auto it = range.first; it != range.second; it++)
{
const std::pair<ReferencePointId, SignalStatistics>& refPointsSsPair = it->second;
const ReferencePointId& rpId = refPointsSsPair.first;
if (rpsStatistics.find(rpId) == rpsStatistics.end())
rpsStatistics[rpId] = std::vector<SigStat>();
SigStat sigStat = {curMsr.id, curMsr.rssi, refPointsSsPair.second.mean};
rpsStatistics[rpId].push_back(sigStat);
}
}
std::map<ReferencePointId, double> rpWeights;
std::map<ReferencePointId, double> rpNormalize;
std::map<TransmitterId, double> trWeights = calcTransmittersWeights(msr);
for (const auto& pair : rpsStatistics)
{
const auto& rpId = pair.first;
const auto& vecSigStat = pair.second;
if (vecSigStat.size() < MIN_NUM_VISIBLE_BS_IN_RP)
continue;
for (auto it1 = vecSigStat.begin(); it1 != vecSigStat.end(); it1 ++)
{
if (useDiffMode)
{
for (auto it2 = vecSigStat.begin(); it2 != it1; it2++)
{
double measDiff = it1->rssiMeasured - it2->rssiMeasured;
double mapDiff = it1->rssiDatabase - it2->rssiDatabase;
// We sum with rp Penalties, to make sum of apWeight = 1 after normalization)
double apW = trWeights[it1->txId] * trWeights[it2->txId];
rpNormalize[rpId] += apW;
double weight = std::max(fabs(mapDiff - measDiff) * apW, MIN_RP_WEIGHT);
rpWeights[rpId] += weight;
}
}
else
{
double apW = trWeights[it1->txId];
rpNormalize[rpId] += apW;
double weight = std::max(fabs(it1->rssiDatabase + it1->rssiMeasured) * apW, MIN_RP_WEIGHT);
rpWeights[rpId] += weight;
}
}
}
for (auto& pair : rpWeights)
{
const ReferencePointId& rpId = pair.first;
double& rpWeight = pair.second;
if (rpWeight > MIN_WEIGHT_EPSILON)
{
// normalize in order to make sum of transmitter weights == 1,
// because we summed transmitter weights on all APs, not only visible
if (USE_AP_WEIGHTS)
{
auto rpDivider = rpNormalize[rpId];
if (rpDivider > MIN_WEIGHT_EPSILON)
rpWeight /= rpDivider;
}
// do sqrt because this norm is better (it was shown empirically)
// this norm also takes into account
// the number of visible APS, due to its asymptotic is 1 / sqrt(N)
rpWeight = std::sqrt(rpWeight);
// Divide by number of visible APs because it differs from point to point
// rely more on RP where there are more visible tx from measurements vector
// for KNN the more penalty the more probability
auto visibleApNum = rpsStatistics[rpId].size();
if (visibleApNum > 0)
rpWeight /= visibleApNum;
// Revert weights in KNN (to transform weights to probabilities)
rpWeight = 1.0 / rpWeight;
}
}
return rpWeights;
}
static double getTriangleWeight(const RefPointsTriangle& triangle,
const std::map<ReferencePointId, double>& rpWeights)
{
double w1, w2, w3;
std::tie(w1, w2, w3) = getValueForEachKey(rpWeights, 0.0,
triangle.rp1,
triangle.rp2,
triangle.rp3);
return w1 + w2 + w3;
}
XYPoint calcPositionInTriangle(const std::vector<RefPointsTriangle>& triangles,
const Radiomap& radiomap,
const std::map<ReferencePointId, double>& rpWeights)
{
auto maxWeightTriangle = std::max_element(triangles.begin(), triangles.end(),
[&rpWeights]
(const RefPointsTriangle& lhs, const RefPointsTriangle& rhs)
{
return getTriangleWeight(lhs, rpWeights) < getTriangleWeight(rhs, rpWeights);
});
double w1, w2, w3;
std::tie(w1, w2, w3) = getValueForEachKey(rpWeights, 0.0,
maxWeightTriangle->rp1,
maxWeightTriangle->rp2,
maxWeightTriangle->rp3);
double norm = w1 + w2 + w3;
if (norm < std::numeric_limits<double>::epsilon())
{
//TODO send error code
return XYPoint(0.0, 0.0);
}
if (!radiomap.hasReferencePoint(maxWeightTriangle->rp1) ||
!radiomap.hasReferencePoint(maxWeightTriangle->rp2) ||
!radiomap.hasReferencePoint(maxWeightTriangle->rp3))
{
//TODO send error code
return XYPoint(0.0, 0.0);
}
ReferencePoint<XYPoint> rp1 = radiomap.getReferencePoint(maxWeightTriangle->rp1);
ReferencePoint<XYPoint> rp2 = radiomap.getReferencePoint(maxWeightTriangle->rp2);
ReferencePoint<XYPoint> rp3 = radiomap.getReferencePoint(maxWeightTriangle->rp3);
double x = (w1 * rp1.point.x + w2 * rp2.point.x + w3 * rp3.point.x) / norm;
double y = (w1 * rp1.point.y + w2 * rp2.point.y + w3 * rp3.point.y) / norm;
return XYPoint(x, y);
}
XYPoint calcKHeaviestRefPointsAverage(const Radiomap& radiomap,
const std::map<ReferencePointId, double>& rpToWeight,
size_t k)
{
k = std::min(k, rpToWeight.size());
std::vector<std::pair<ReferencePointId, double>> kHeaviestPoints;
kHeaviestPoints.reserve(k);
std::partial_sort_copy(
rpToWeight.begin(), rpToWeight.end(),
kHeaviestPoints.begin(), kHeaviestPoints.end(),
[](std::pair<ReferencePointId, double> const& l,
std::pair<ReferencePointId, double> const& r)
{
return l.second > r.second;
});
double x = 0.0;
double y = 0.0;
double norm = 0.0;
for (auto it = kHeaviestPoints.cbegin(); it != kHeaviestPoints.cend(); it++)
{
// valid, due to heaviest point build structure
ReferencePoint<XYPoint> rp = radiomap.getReferencePoint(it->first);
double w = it->second;
x += w * rp.point.x;
y += w * rp.point.y;
norm += w;
}
if (norm < std::numeric_limits<double>::epsilon())
{
//TODO check if norm can be zero and process error
return XYPoint(0.0, 0.0);
}
x /= norm;
y /= norm;
return XYPoint(x, y);
}
RadioMeasurementsData getWhitelistedMeasurements(
const std::set<TransmitterId>& wl,
const RadioMeasurementsData msrs)
{
RadioMeasurementsData filtered;
std::copy_if(msrs.begin(), msrs.end(), std::back_inserter(filtered),
[&wl](const RadioMeasurementData& msr) {return wl.find(msr.id) != wl.end(); });
return filtered;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/knn/knn_utils.h
================================================
/** knn_utils.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef KNN_UTILS_H
#define KNN_UTILS_H
#include <navigine/navigation-core/navigation_output.h>
#include <navigine/navigation-core/point.h>
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/level_collector.h>
#include <set>
#include "../triangulation.h"
namespace navigine {
namespace navigation_core {
struct RefPointsTriangle
{
ReferencePointId rp1;
ReferencePointId rp2;
ReferencePointId rp3;
};
struct TriangleVerticesWeights
{
double w1;
double w2;
double w3;
};
std::map<LevelId, std::vector<RefPointsTriangle>> triangulate(const std::shared_ptr<LevelCollector>& levelCollector);
std::map<LevelId,
std::multimap<TransmitterId, std::pair<ReferencePointId, SignalStatistics>>>
getLevelsRadiomaps(const std::shared_ptr<LevelCollector>& levelCollector);
std::map<LevelId, std::set<TransmitterId>> getLevelReferenceTransmittersMap(
const std::shared_ptr<LevelCollector>& levelCollector);
std::map<ReferencePointId, double> calcRpWeightsKnn(
const std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics> >& radiomap,
const RadioMeasurementsData& msr,
bool useDiffMode = true);
XYPoint calcPositionInTriangle(const std::vector<RefPointsTriangle>& triangles,
const Radiomap& rps,
const std::map<ReferencePointId, double>& rpToWeight);
XYPoint calcKHeaviestRefPointsAverage(const Radiomap& radiomap,
const std::map<ReferencePointId, double>& rpToWeight,
size_t k);
RadioMeasurementsData getWhitelistedMeasurements(
const std::set<TransmitterId>& wl,
const RadioMeasurementsData msrs);
template<typename KeyType, typename ValType>
ValType func_helper(const std::map<KeyType, ValType>& map, KeyType key, ValType defVal) {
if (map.find(key) == map.end())
return defVal;
else
return map.find(key)->second;
}
template<typename ...Keys, typename ValType, typename KeyType>
auto getValueForEachKey(const std::map<KeyType, ValType>& map, ValType defVal,
Keys... keys) -> decltype(std::make_tuple(func_helper(map, keys, defVal)...)) {
return std::make_tuple(func_helper(map, keys, defVal)...);
}
} } // namespace navigine::navigation_core
#endif // KNN_UTILS_H
================================================
FILE: src/level.cpp
================================================
/** level.cpp
*
* Copyright (c) 2019 Navigine.
*
*/
#include <navigine/navigation-core/level.h>
namespace navigine {
namespace navigation_core {
const GeoPoint& Level::bindingPoint() const
{
return mBindingPoint;
}
const LevelId& Level::id() const
{
return mId;
}
const Radiomap &Level::radiomap() const
{
return mRadiomap;
}
const Graph<XYPoint>& Level::graph() const
{
return mGraph;
}
const LevelGeometry& Level::geometry() const
{
return mGeometry;
}
bool Level::containsTransmitter(const TransmitterId& txId) const
{
return mTransmitters.find(txId) != mTransmitters.end();
}
const Transmitter<XYZPoint>& Level::transmitter(const TransmitterId& txId) const
{
return mTransmitters.at(txId);
}
void Level::addTransmitters(const XYZTransmitters& transmitters)
{
for (const Transmitter<XYZPoint>& tx: transmitters)
mTransmitters.insert({tx.id, tx});
}
//TODO after second call should reference points be added to existed or replaced?
void Level::setReferencePoints(const XYReferencePoints& referencePoints)
{
mRadiomap = Radiomap(referencePoints);
}
void Level::setGraph(const Graph<XYPoint>& graph)
{
mGraph = graph;
}
void Level::setGeometry(const LevelGeometry& levelGeometry)
{
mGeometry = levelGeometry;
}
RadioMeasurementsData getLevelRadioMeasurements(
const Level& level,
const RadioMeasurementsData& radioMsr)
{
RadioMeasurementsData levelRadioMeasurements;
for (const RadioMeasurementData& msr: radioMsr)
{
if (level.containsTransmitter(msr.id))
levelRadioMeasurements.push_back(msr);
}
return levelRadioMeasurements;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/level_collector.cpp
================================================
/** level_collector.cpp
*
* Copyright (c) 2019 Navigine.
*
*/
#include <navigine/navigation-core/level_collector.h>
namespace navigine {
namespace navigation_core {
namespace {
XYZTransmitters getTransmittersInLocalCoordinates(
const GeoPoint& binding,
const std::vector<Transmitter<GeoPoint3D>>& transmitters,
const double& altitude)
{
XYZTransmitters localTransmitters;
for (const Transmitter<GeoPoint3D>& t: transmitters)
{
XYPoint point2D = gpsToLocal(GeoPoint(t.point.latitude, t.point.longitude), binding);
localTransmitters.emplace_back(
TransmitterId(t.id.value),
XYZPoint(point2D.x, point2D.y, altitude + t.point.altitude),
t.pathlossModel,
t.type);
}
return localTransmitters;
}
XYReferencePoints getRefPointsInLocalCoordinates(
const GeoPoint& binding,
const std::vector<ReferencePoint<GeoPoint>>& referencePoints)
{
XYReferencePoints localReferencePoints;
for (const ReferencePoint<GeoPoint>& rp: referencePoints)
{
localReferencePoints.emplace_back(
ReferencePointId(rp.id.value),
gpsToLocal(rp.point, binding),
rp.fingerprints);
}
return localReferencePoints;
}
Polygon getPolygonInLocalCoordinates(
const GeoPoint& binding,
const Polygon& geoPolygon)
{
Polygon result;
for (const auto& p : geoPolygon.outer())
{
XYPoint pxy = gpsToLocal(GeoPoint(p.x(), p.y()), binding);
boost::geometry::append(result.outer(), Point(pxy.x, pxy.y));
}
for (const auto& innerRing : geoPolygon.inners())
{
result.inners().push_back({});
for (const auto& p : innerRing)
{
XYPoint pxy = gpsToLocal(GeoPoint(p.x(), p.y()), binding);
boost::geometry::append(result.inners().back(), Point(pxy.x, pxy.y));
}
}
boost::geometry::correct(result);
return result;
}
Box getBBoxInLocalCoordinates(const GeoPoint& binding, const Box& geoBox)
{
XYPoint pmin = gpsToLocal(GeoPoint(geoBox.min_corner().x(), geoBox.min_corner().y()), binding);
XYPoint pmax = gpsToLocal(GeoPoint(geoBox.max_corner().x(), geoBox.max_corner().y()), binding);
return Box(Point(pmin.x, pmin.y), Point(pmax.x, pmax.y));
}
LevelGeometry getGeometryInLocalCoordinates(
const GeoPoint& binding,
const LevelGeometry& geolevelGeometry)
{
Multipolygon localArea;
for (const auto& poly : geolevelGeometry.allowedArea())
{
localArea.push_back(getPolygonInLocalCoordinates(binding, poly));
}
boost::geometry::correct(localArea);
Box localBox = getBBoxInLocalCoordinates(binding, geolevelGeometry.boundingBox());
boost::geometry::correct(localBox);
return LevelGeometry{localArea, localBox};
}
Graph<XYPoint> getGraphInLocalCoordinates(
const GeoPoint& binding,
const Graph<GeoPoint>& geoGraph)
{
Graph<XYPoint> xyGraph;
std::map<int, int> geoToXyVertexIds;
for (Graph<GeoPoint>::VertexIterator it = geoGraph.vertexBegin(); it != geoGraph.vertexEnd(); it++)
{
Graph<GeoPoint>::Vertex geoVertex = *it;
XYPoint p = gpsToLocal(geoVertex.point, binding);
Graph<XYPoint>::Vertex xyVertex = xyGraph.addVertex(p);
geoToXyVertexIds[geoVertex.id] = xyVertex.id;
}
for (Graph<GeoPoint>::EdgeIterator it = geoGraph.edgesBegin(); it != geoGraph.edgesEnd(); it++)
{
Graph<GeoPoint>::Vertex geoV1 = geoGraph.getVertex(it->first);
Graph<GeoPoint>::Vertex geoV2 = geoGraph.getVertex(it->second);
int xyV1 = geoToXyVertexIds[geoV1.id];
int xyV2 = geoToXyVertexIds[geoV2.id];
xyGraph.addEdge(xyV1, xyV2);
}
return xyGraph;
}
} //namespace
std::shared_ptr<LevelCollector> createLevelCollector()
{
return std::make_shared<LevelCollector>();
}
boost::optional<LevelId> LevelCollector::findLevelByTransmitterId(const TransmitterId &transmitterId) const
{
for (const Level& level: mLevels)
{
if (level.containsTransmitter(transmitterId))
{
return level.id();
}
}
return boost::none;
}
const std::vector<Level>& LevelCollector::levels() const
{
return mLevels;
}
bool LevelCollector::hasLevel(const LevelId& levelId) const
{
return mLevelsIndices.find(levelId) != mLevelsIndices.end();
}
const Level& LevelCollector::level(const LevelId& levelId) const
{
return mLevels.at(mLevelsIndices.at(levelId));
}
GeoPoint getBinding(const GeoLevel& geoLevel)
{
auto geoBox = geoLevel.geometry.boundingBox();
boost::geometry::correct(geoBox);
double minLatitude = geoBox.min_corner().x();
double minLongitude = geoBox.min_corner().y();
return GeoPoint(minLatitude, minLongitude);
}
void LevelCollector::addGeoLevel(const GeoLevel& geoLevel)
{
LevelId levelId = LevelId(geoLevel.id);
GeoPoint binding = getBinding(geoLevel);
Level level(levelId, binding);
level.addTransmitters(getTransmittersInLocalCoordinates(binding, geoLevel.transmitters, geoLevel.altitude));
level.setGeometry(getGeometryInLocalCoordinates(binding, geoLevel.geometry));
level.setReferencePoints(getRefPointsInLocalCoordinates(binding, geoLevel.referencePoints));
level.setGraph(getGraphInLocalCoordinates(binding, geoLevel.graph));
mLevels.push_back(std::move(level));
mLevelsIndices.insert({levelId, mLevels.size() - 1});
}
} } // namespace navigine::navigation_core
================================================
FILE: src/level_estimator/barometer.cpp
================================================
#include <navigine/navigation-core/navigation_input.h>
#include <cmath>
#include <numeric>
#include <vector>
#include "barometer.h"
namespace navigine {
namespace navigation_core {
#ifdef DEBUG_OUTPUT_BAROMETER
#include <iostream>
#include <fstream>
#endif
static const double BAROMETER_R = 8.31; // [J / (mol * K)]
static const double BAROMETER_G = 9.8; // [m / s^2]
static const double BAROMETER_T = 288.0; // [K]
static const double BAROMETER_M = 0.0289; // [kg / mol]
static const double BAROMETER_PASCAL_TO_METERS = -BAROMETER_R * BAROMETER_T / (BAROMETER_G * BAROMETER_M);
static const double BAROMETER_MBAR_TO_PASCAL = 100.0;
static const double BAROMETER_MIN_FLOOR_HEIGHT_M = 2.0; // [m]
static const size_t SIZE_OF_CHUNK = 100;
static double calcMean(const std::deque<double>& measurements)
{
if (measurements.empty())
{
//TODO process error
return 0;
}
return std::accumulate(measurements.begin(), measurements.end(), 0.0) / measurements.size();
}
//TODO test if there is no barometer in the phone
//TODO try to use ring buffer with fixed size
//TODO take into account vertical speed in Pa/sec
// - the floor is changed only if the vertical displacement is more than distance between floors (i.e. 3m)
LevelId Barometer::checkLevelChange(
const LevelId& levelId,
const SensorMeasurement& sensorMsr)
{
if (!mIsInitialized)
{
mIsInitialized = true;
return levelId;
}
updateBuffers(sensorMsr);
updateLevel(levelId);
return mLastKnownLevel;
}
void Barometer::updateBuffers(const SensorMeasurement& sensorMsr)
{
#ifdef DEBUG_OUTPUT_BAROMETER
std::ofstream debugOutputFile;
debugOutputFile.open(DEBUG_OUTPUT_BAROMETER, std::ios_base::app);
if (sensorMsr.data.type == SENSOR_ENTRY_BAROMETER)
debugOutputFile << m.ts << " " << m.values.x << std::endl;
debugOutputFile.close();
#endif
if (sensorMsr.data.type == SensorMeasurementData::Type::BAROMETER)
{
if (mStoredMeasurements.size() > SIZE_OF_CHUNK)
mStoredMeasurements.pop_front();
if (mNewMeasurements.size() > SIZE_OF_CHUNK)
{
mStoredMeasurements.push_back(mNewMeasurements.front());
mNewMeasurements.pop_front();
}
mNewMeasurements.push_back(sensorMsr.data.values.x);
}
}
//TODO take into account pressure change speed - i.e. a person can move slowly upstairs
void Barometer::updateLevel(LevelId levelId)
{
if ((mNewMeasurements.size() > SIZE_OF_CHUNK)
&& (mStoredMeasurements.size() >= SIZE_OF_CHUNK))
{
if (mLastKnownLevel != levelId)
{
double curMean = calcMean(mNewMeasurements);
double lastMean = calcMean(mStoredMeasurements);
if (curMean <= std::numeric_limits<double>::epsilon()
|| lastMean <= std::numeric_limits<double>::epsilon())
{
//TODO: process error: incorrect average pressure!
return;
}
double p2Pa = BAROMETER_MBAR_TO_PASCAL * curMean;
double p1Pa = BAROMETER_MBAR_TO_PASCAL * lastMean;
double deltaHeightM = BAROMETER_PASCAL_TO_METERS * std::log(p2Pa / p1Pa);
bool levelHasChanged = std::abs(deltaHeightM) > BAROMETER_MIN_FLOOR_HEIGHT_M;
if (levelHasChanged)
mLastKnownLevel = levelId;
}
}
else
mLastKnownLevel = levelId;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/level_estimator/barometer.h
================================================
/** barometer.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef BAROMETER_H
#define BAROMETER_H
#include <navigine/navigation-core/navigation_input.h>
#include <deque>
#include <navigine/navigation-core/level.h>
#include "../measurement_types.h"
namespace navigine {
namespace navigation_core {
class Barometer
{
public:
Barometer() : mIsInitialized ( false ), mLastKnownLevel ( LevelId("")) {}
LevelId checkLevelChange(
const LevelId &levelId,
const SensorMeasurement& sensorMsr);
private:
void updateBuffers(const SensorMeasurement& sensorMsr);
void updateLevel(LevelId levelId);
std::deque<double> mNewMeasurements = {};
std::deque<double> mStoredMeasurements = {};
bool mIsInitialized;
LevelId mLastKnownLevel;
};
} } // namespace navigine::navigation_core
#endif // BAROMETER_H
================================================
FILE: src/level_estimator/level_estimator.cpp
================================================
/** level_estimator.cpp
*
** Copyright (c) 2017 Navigine.
*
*/
#include <navigine/navigation-core/navigation_settings.h>
#include "level_estimator.h"
namespace navigine {
namespace navigation_core {
LevelEstimator::LevelEstimator(const NavigationSettings& navProps)
: mUseBarometer(navProps.commonSettings.useBarometer)
{
}
/**
* function detects id of current level according to highest average RSSI for
* placed transmitters. In a case of any error returns -1
*/
LevelId LevelEstimator::calculateLevel(
const RadioMeasurementsData& radioMsr,
const SensorMeasurement& sensorMsr)
{
LevelId detectedLevelId = detectCurrentLevel(radioMsr);
LevelId levelIdWithHistory = mHistory.detectLevelUsingHistory(detectedLevelId);
LevelId levelId = mUseBarometer ? mBarometer.checkLevelChange(levelIdWithHistory, sensorMsr)
: levelIdWithHistory;
return levelId;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/level_estimator/level_estimator.h
================================================
/** level_estimator.h
*
** Copyright (c) 2017 Navigine.
*
*/
#ifndef LEVEL_ESTIMATOR_H
#define LEVEL_ESTIMATOR_H
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/navigation_settings.h>
#include <navigine/navigation-core/level_collector.h>
#include "level_history.h"
#include "barometer.h"
#include "../measurement_types.h"
namespace navigine {
namespace navigation_core {
class LevelEstimator
{
public:
LevelEstimator(const NavigationSettings& navProps);
virtual ~LevelEstimator() {}
LevelId calculateLevel(
const RadioMeasurementsData& radioMsr,
const SensorMeasurement& sensorMsr);
protected:
virtual LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) = 0;
private:
LevelHistory mHistory;
Barometer mBarometer;
const bool mUseBarometer;
};
} } // namespace navigine::navigation_core
#endif // LEVEL_ESTIMATOR_H
================================================
FILE: src/level_estimator/level_estimator_radiomap.cpp
================================================
/** level_estimator_radiomap.cpp
*
** Copyright (c) 2017 Navigine.
*
*/
#include <cmath>
#include <navigine/navigation-core/navigation_settings.h>
#include "level_estimator_radiomap.h"
#include "../knn/knn_utils.h"
namespace navigine {
namespace navigation_core {
LevelEstimatorRadiomap::LevelEstimatorRadiomap(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps)
: LevelEstimator(navProps)
, mLevelIndex(levelCollector)
, mUseBestRp(navProps.commonSettings.useBestRefPointLevel)
{
mLevelsRadiomaps = getLevelsRadiomaps(levelCollector);
}
LevelId LevelEstimatorRadiomap::detectCurrentLevel(const RadioMeasurementsData& radioMsr)
{
LevelId bestLevelId = LevelId("");
double bestLevelWeight = 0.0;
for (const Level& level : mLevelIndex->levels())
{
const std::multimap<TransmitterId, fingerprint>& radiomap = mLevelsRadiomaps.at(level.id());
double levelWeight = mUseBestRp ? calcWeightBestRp(radioMsr, radiomap)
: calcWeight(radioMsr, radiomap);
if (levelWeight > bestLevelWeight)
{
bestLevelWeight = levelWeight;
bestLevelId = level.id();
}
}
return bestLevelId;
}
/** level weight is calculated based of difference between measured and reference signals */
double LevelEstimatorRadiomap::calcWeight(
const RadioMeasurementsData& radioMsr,
const std::multimap<TransmitterId, fingerprint> &fingerprints)
{
int nUniqueTransmitters = 0;
double levelWeight = 0.0;
for (const RadioMeasurementData& meas : radioMsr)
{
//TODO try to use lower weight for wifi signal since it is not reliable for level identification
double signalTypeWeight = 1.0;//(meas.type == SIGNAL_ENTRY_WIFI) ? 0.01 : 1.0;
double measWeight = 0.0;
int numOfOccurrences = 0;
auto range = fingerprints.equal_range(meas.id);
for (auto it = range.first; it != range.second; ++it)
{
const fingerprint& refPointsSsPair = it->second;
const SignalStatistics& sigStat = refPointsSsPair.second;
// It is possible that positive rssi values are used in measure.xml
// and negative rssi values are used in measurements - so both abs are necessary here:
double difference = std::fabs(std::fabs(meas.rssi) - std::fabs(sigStat.mean));
measWeight += signalTypeWeight * 1.0 / (difference + 0.1);
numOfOccurrences ++;
}
if (numOfOccurrences > 0)
levelWeight += measWeight / numOfOccurrences;
if (std::distance(range.first, range.second) > 0)
nUniqueTransmitters++;
}
levelWeight *= static_cast<double>(nUniqueTransmitters);
return levelWeight;
}
/** level weight is given equal to weight of best reference point */
double LevelEstimatorRadiomap::calcWeightBestRp(
const RadioMeasurementsData& radioMsr,
const std::multimap<TransmitterId, fingerprint>& radiomap)
{
std::map<ReferencePointId, double> rpToWeight = calcRpWeightsKnn(radiomap, radioMsr, true);
double bestWeight = 0.0;
if (!rpToWeight.empty())
{
ReferencePointId bestRp = rpToWeight.begin()->first;
for (auto const& rpWeightPair : rpToWeight)
{
if (rpWeightPair.second > bestWeight)
{
bestRp = rpWeightPair.first;
bestWeight = rpWeightPair.second;
}
}
}
return bestWeight;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/level_estimator/level_estimator_radiomap.h
================================================
/** level_estimator_radiomap.h
*
** Copyright (c) 2017 Navigine.
*
*/
#ifndef LEVEL_ESTIMATOR_RADIOMAP_H
#define LEVEL_ESTIMATOR_RADIOMAP_H
#include "level_estimator.h"
namespace navigine {
namespace navigation_core {
class LevelEstimatorRadiomap : public LevelEstimator
{
public:
LevelEstimatorRadiomap(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
protected:
LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) override;
private:
using fingerprint = std::pair<ReferencePointId, SignalStatistics>;
double calcWeight(
const RadioMeasurementsData& radioMsr,
const std::multimap<TransmitterId, fingerprint> &fingerprints);
double calcWeightBestRp(
const RadioMeasurementsData& radioMsr,
const std::multimap<TransmitterId, fingerprint> &radiomap);
std::shared_ptr<LevelCollector> mLevelIndex;
std::map<LevelId,
std::multimap<TransmitterId, fingerprint>> mLevelsRadiomaps;
const bool mUseBestRp;
};
} } // namespace navigine::navigation_core
#endif // LEVEL_ESTIMATOR_RADIOMAP_H
================================================
FILE: src/level_estimator/level_estimator_transmitters.cpp
================================================
/** level_estimator_transmitters.cpp
*
** Copyright (c) 2019 Navigine.
*
*/
#include "level_estimator_transmitters.h"
namespace navigine {
namespace navigation_core {
LevelEstimatorTransmitters::LevelEstimatorTransmitters(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps)
: LevelEstimator(navProps)
, mLevelIndex(levelCollector)
, mUseTxPower(navProps.commonSettings.useTxPower)
{
}
static double getAverageRssi(const RadioMeasurementsData& radioMsr)
{
double averageRssi = 0.0;
for(int i = 0; i < (int)radioMsr.size(); ++i)
averageRssi += radioMsr[i].rssi;
averageRssi /= std::max((int)radioMsr.size(), 1);
return averageRssi;
}
static double getAverageA(const Level& level, const RadioMeasurementsData& radioMsr)
{
double averageA = 0.0;
for(int i = 0; i < (int)radioMsr.size(); ++i)
{
const Transmitter<XYZPoint> transmitter = level.transmitter(radioMsr[i].id);
const PathlossModel pathLossModel = transmitter.pathlossModel;
averageA += pathLossModel.A;
}
averageA /= std::max((int)radioMsr.size(), 1);
return averageA;
}
LevelId LevelEstimatorTransmitters::detectCurrentLevel(const RadioMeasurementsData& radioMsr)
{
LevelId bestLevelId("");
double bestAverCost = -1000.0;
for (const Level& level : mLevelIndex->levels())
{
RadioMeasurementsData levelMsr = getLevelRadioMeasurements(level, radioMsr);
if (!levelMsr.empty())
{
double averageRssi = getAverageRssi(levelMsr);
double averageA;
if (mUseTxPower)
averageA = getAverageA(level, levelMsr);
else
averageA = 0;
double averageCost = averageRssi + averageA;
// Using weighted average. Weight depends on number of visible Transmitters
double weight = 1.0 + 0.1 * levelMsr.size() / radioMsr.size();
averageCost /= weight;
if (averageCost > bestAverCost)
{
bestAverCost = averageCost;
bestLevelId = level.id();
}
}
}
return bestLevelId;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/level_estimator/level_estimator_transmitters.h
================================================
/** level_estimator_transmitters.h
*
** Copyright (c) 2019 Navigine.
*
*/
#ifndef LEVEL_ESTIMATOR_TRANSMITTERS_H
#define LEVEL_ESTIMATOR_TRANSMITTERS_H
#include "level_estimator.h"
namespace navigine {
namespace navigation_core {
class LevelEstimatorTransmitters : public LevelEstimator
{
public:
LevelEstimatorTransmitters(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
protected:
LevelId detectCurrentLevel(const RadioMeasurementsData& radioMsr) override;
private:
std::shared_ptr<LevelCollector> mLevelIndex;
const bool mUseTxPower;
};
} } // namespace navigine::navigation_core
#endif // LEVEL_ESTIMATOR_TRANSMITTERS_H
================================================
FILE: src/level_estimator/level_history.cpp
================================================
/** level_history.cpp
*
* Copyright (c) 2017 Navigine.
*
*/
#include "level_history.h"
namespace navigine {
namespace navigation_core {
static const int MAX_LEVEL_ID_OCCUR = 4;
LevelHistory::LevelHistory()
: mMaxLevelIdOccurrence(MAX_LEVEL_ID_OCCUR)
, mIsInitialized(false)
, mCurrentLevelId(LevelId(""))
{
}
LevelId LevelHistory::detectLevelUsingHistory(const LevelId& detectedLevelId)
{
//TODO change to boost optional!!!
if (detectedLevelId.value == "")
return mCurrentLevelId;
if (!mIsInitialized)
{
mIsInitialized = true;
mCurrentLevelId = detectedLevelId;
}
if (++mFloorCnt[detectedLevelId] > MAX_LEVEL_ID_OCCUR)
{
mFloorCnt.clear();
mFloorCnt[detectedLevelId] = MAX_LEVEL_ID_OCCUR;
mCurrentLevelId = detectedLevelId;
}
return mCurrentLevelId;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/level_estimator/level_history.h
================================================
/** level_history.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef LEVEL_HISTORY_H
#define LEVEL_HISTORY_H
#include <vector>
#include <map>
#include <navigine/navigation-core/level.h>
namespace navigine {
namespace navigation_core {
class LevelHistory
{
public:
LevelHistory();
LevelId detectLevelUsingHistory(const LevelId &detectedLevelId);
private:
const int mMaxLevelIdOccurrence;
LevelId mCurrentLevelId;
bool mIsInitialized;
std::map<LevelId, int> mFloorCnt;
};
} } // namespace navigine::navigation_core
#endif // LEVEL_HISTORY_H
================================================
FILE: src/level_geometry.cpp
================================================
#include <navigine/navigation-core/level_collector.h>
namespace navigine {
namespace navigation_core {
namespace { }
LevelGeometry::LevelGeometry(const Multipolygon& allowedArea, const Box& box)
: mAllowedArea(allowedArea)
, mBoundingBox(box)
{
for (const auto& polygon : mAllowedArea)
for (const auto& inner : polygon.inners())
mInnerBarriers.push_back(inner);
std::vector<BoxIdx> boxIndices;
for (std::size_t i = 0; i < mInnerBarriers.size(); i++)
{
boost::geometry::correct(mInnerBarriers[i]);
boxIndices.push_back({boost::geometry::return_envelope<Box>(mInnerBarriers[i]), i});
}
mTree = RTree(boxIndices);
}
bool LevelGeometry::isPathTouchesBarriers(const Linestring& path) const
{
bool isInsideAllowedArea = false;
for (const auto& allowedZone : mAllowedArea)
if (boost::geometry::covered_by(path, allowedZone.outer()))
isInsideAllowedArea = true;
if (!isInsideAllowedArea)
return true;
std::vector<BoxIdx> intersectedGeometryBoxes;
mTree.query(boost::geometry::index::intersects(path), std::back_inserter(intersectedGeometryBoxes));
for (const auto& boxIdxPair : intersectedGeometryBoxes)
{
if (boost::geometry::intersects(path, mInnerBarriers[boxIdxPair.second]))
return true;
}
return false;
}
const Multipolygon& LevelGeometry::allowedArea() const
{
return mAllowedArea;
}
const Box& LevelGeometry::boundingBox() const
{
return mBoundingBox;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/likelihood/likelihood.cpp
================================================
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/level_collector.h>
#include <navigine/navigation-core/transmitter.h>
#include <navigine/navigation-core/level.h>
#include <navigine/navigation-core/point.h>
#include "likelihood.h"
namespace navigine {
namespace navigation_core {
bool isParticleInIntersectionArea(
const Level& level,
const RadioMeasurementsData& radioMsr,
double x, double y)
{
for (const auto& msr : radioMsr)
{
const Transmitter<XYZPoint> tx = level.transmitter(msr.id);
const double circleEps = -1e-7;
const double dx = tx.point.x - x;
const double dy = tx.point.y - y;
const double modelValue = std::sqrt(dx * dx + dy * dy);
const double delta = msr.distance - modelValue;
if (delta < circleEps)
return false;
}
return true;
};
}
}
================================================
FILE: src/likelihood/likelihood.h
================================================
#ifndef LIKELIHOOD_H
#define LIKELIHOOD_H
#include <navigine/navigation-core/xy_particle.h>
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/navigation_settings.h>
#include <navigine/navigation-core/level_collector.h>
#include "../position.h"
namespace navigine {
namespace navigation_core {
const constexpr double MIN_WEIGHT_SUM_DOUBLE_EPSILON = 1e-8;
enum class MeasurementLikelihoodModel {DISTANCE_V1, DISTANCE_V2, RSSI, RADIOMAP};
class Likelihood
{
public:
virtual ~Likelihood() {}
virtual std::vector<double> calculateWeights(
const Level& levelCollector,
const RadioMeasurementsData& msr,
const std::vector<XYParticle>& particles) = 0;
/**
* optimization for mutation and sampling - first we perform all calculations necessary for likelihood
* and then use this likelihood to get particle weights in method getWeight()
*/
virtual void doPreliminaryCalculations(
const Level& level,
const RadioMeasurementsData& radioMsr) = 0;
/**
* return position weight base on information calculated during Likelihood::compute() method
*/
virtual double getWeight(
const Level& level,
const RadioMeasurementsData& radioMsr,
double x, double y) const = 0;
virtual bool hasSufficientMeasurementsForUpdate(
const Level& level,
const RadioMeasurementsData& radioMsr) const = 0;
virtual bool hasSufficientMeasurementsForMutation(
const Level& level,
const RadioMeasurementsData& radioMsr) const = 0;
/**
* Heuristic to improve particle filter robustness.
* The method returns points for sampling particles around them.
* Depending on implementation it can be transmitter positions with best signals,
* or reference points with best weighs
*/
virtual std::vector<XYPoint> calculateSamplingCenters(
const Level& level,
const RadioMeasurementsData& radioMsr) = 0;
virtual MeasurementLikelihoodModel getLikelihoodModel(void) = 0;
virtual RadioMeasurementsData getOnlyCorrectlyIntersectedMeasurements(
const Level& level,
const RadioMeasurementsData& radioMsr) const = 0;
};
bool isParticleInIntersectionArea(
const Level& level,
const RadioMeasurementsData& radioMsr,
double x, double y);
std::shared_ptr<Likelihood> createLikelihoodLogModel(const std::shared_ptr<LevelCollector>& levelCollector, const NavigationSettings& navProps, MeasurementLikelihoodModel likelihoodModel);
} } // namespace navigine::navigation_core
#endif // LIKELIHOOD_H
================================================
FILE: src/likelihood/likelihood_radiomap.cpp
================================================
#include "likelihood_radiomap.h"
#include "../knn/knn_utils.h"
namespace navigine {
namespace navigation_core {
#ifdef NAVIGATION_VISUALIZATION
XYPoint LikelihoodRadiomap::debugTrianglePoint = XYPoint(0, 0);
std::map<ReferencePointId, double>
LikelihoodRadiomap::debugRefPointWeights = std::map<ReferencePointId, double>();
#endif
LikelihoodRadiomap::LikelihoodRadiomap(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps)
: mK(navProps.commonSettings.kNeighbors)
, mMinMsrNumForMutation(navProps.commonSettings.minMsrNumForMutation)
, mMinMsrNumForPositionCalculation(navProps.commonSettings.minNumOfMeasurementsForPositionCalculation)
, mUseTriangles(navProps.commonSettings.useTriangles)
, mUseDiffMode(navProps.commonSettings.useDiffMode)
, mLikelihoodPoint(boost::none)
{
if (mUseTriangles)
mLevelTriangles = triangulate(levelCollector);
mLevelReferenceTransmittersMap = getLevelReferenceTransmittersMap(levelCollector);
mLevelsRadiomaps = getLevelsRadiomaps(levelCollector);
}
bool LikelihoodRadiomap::hasSufficientMeasurementsForUpdate(const Level& level,
const RadioMeasurementsData& radioMsr) const
{
const std::set<TransmitterId>& txIds = mLevelReferenceTransmittersMap.at(level.id());
auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);
return validMsr.size() >= mMinMsrNumForPositionCalculation;
}
bool LikelihoodRadiomap::hasSufficientMeasurementsForMutation(
const Level& level,
const RadioMeasurementsData& radioMsr) const
{
const std::set<TransmitterId>& txIds = mLevelReferenceTransmittersMap.at(level.id());
auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);
return validMsr.size() >= mMinMsrNumForMutation;
}
double calcParticleWeight(
XYParticle particle,
XYPoint trianglePosition)
{
double sqrDist = (particle.x - trianglePosition.x) * (particle.x - trianglePosition.x)
+ (particle.y - trianglePosition.y) * (particle.y - trianglePosition.y);
return 1.0 / (0.1 + sqrt(sqrDist));
}
std::vector<double> LikelihoodRadiomap::calculateWeights(const Level& level,
const RadioMeasurementsData& radioMsr,
const std::vector<XYParticle>& particles)
{
const std::set<TransmitterId>& txIds = mLevelReferenceTransmittersMap.at(level.id());
auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);
std::vector<double> weightsMult(particles.size(), 1.0 / particles.size());
if (validMsr.size() < mMinMsrNumForPositionCalculation)
{
//TODO check if error is possible and process error
return weightsMult;
}
const std::multimap<TransmitterId, std::pair<ReferencePointId, SignalStatistics>>&
radiomap = mLevelsRadiomaps.at(level.id());
std::map<ReferencePointId, double> rpToWeight = calcRpWeightsKnn(radiomap, validMsr, mUseDiffMode);
boost::optional<XYPoint> p = calculateRadiomapPoint(level.id(), level.radiomap(), rpToWeight);
#ifdef NAVIGATION_VISUALIZATION
std::map<ReferencePointId, double> weights;
for (const auto &rpPair: rpToWeight)
weights.insert(rpPair);
debugRefPointWeights = weights;
if (p.is_initialized())
debugTrianglePoint = p.get();
#endif
double sum = 0.0;
for (size_t i = 0; i < particles.size(); i++)
{
double w = p.is_initialized() ? calcParticleWeight(particles[i], p.get()) : 0.0;
weightsMult[i] = w;
sum += w;
}
if (sum > MIN_WEIGHT_SUM_DOUBLE_EPSILON)
{
for (size_t i = 0; i < particles.size(); i++)
{
weightsMult[i] /= sum;
}
}
return weightsMult;
}
boost::optional<XYPoint> LikelihoodRadiomap::calculateRadiomapPoint(
const LevelId& levelId,
const Radiomap& radiomap,
const std::map<ReferencePointId, double>& rpToWeight)
{
if (!mUseTriangles)
{
return calcKHeaviestRefPointsAverage(radiomap, rpToWeight, mK);
}
else
{
auto trianglesIt = mLevelTriangles.find(levelId);
if (trianglesIt == mLevelTriangles.end())
{
return boost::none;
}
const std::vector<RefPointsTriangle>& triangles = trianglesIt->second;
if (triangles.empty())
{
return boost::none;
}
return calcPositionInTriangle(triangles, radiomap, rpToWeight);
}
}
std::vector<XYPoint> LikelihoodRadiomap::calculateSamplingCenters(const Level& level,
const RadioMeasurementsData& radioMsr)
{
this->doPreliminaryCalculations(level, radioMsr);
std::vector<XYPoint> samplingCenters;
if (mLikelihoodPoint.is_initialized())
samplingCenters.push_back(mLikelihoodPoint.get());
return samplingCenters;
}
void LikelihoodRadiomap::doPreliminaryCalculations(
const Level& level,
const RadioMeasurementsData& radioMsr)
{
const std::set<TransmitterId>& txIds = mLevelReferenceTransmittersMap.at(level.id());
auto validMsr = getWhitelistedMeasurements(txIds, radioMsr);
if (validMsr.size() >= mMinMsrNumForPositionCalculation)
{
const Radiomap& refPointsMap = level.radiomap();
const std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>>& signalsMap =
mLevelsRadiomaps.at(level.id());
std::map<ReferencePointId, double> rpToWeight =
calcRpWeightsKnn(signalsMap, validMsr, mUseDiffMode);
mLikelihoodPoint = calculateRadiomapPoint(level.id(), refPointsMap, rpToWeight);
}
}
double LikelihoodRadiomap::getWeight(const Level&,
const RadioMeasurementsData&,
double x,
double y) const
{
return mLikelihoodPoint.is_initialized()
? calcParticleWeight(XYParticle{x, y, 0}, mLikelihoodPoint.get())
: 0.0;
}
RadioMeasurementsData LikelihoodRadiomap::getOnlyCorrectlyIntersectedMeasurements(
const Level& level,
const RadioMeasurementsData& radioMsr) const
{
return radioMsr;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/likelihood/likelihood_radiomap.h
================================================
#ifndef LIKELIHOOD_TRIANGULATION_H
#define LIKELIHOOD_TRIANGULATION_H
#include <boost/optional.hpp>
#include <navigine/navigation-core/navigation_settings.h>
#include "likelihood.h"
#include "../knn/knn_utils.h"
namespace navigine {
namespace navigation_core {
class LikelihoodRadiomap : public Likelihood
{
public:
LikelihoodRadiomap(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
std::vector<double> calculateWeights(
const Level& level,
const RadioMeasurementsData& radioMsr,
const std::vector<XYParticle>& particles
) override;
void doPreliminaryCalculations(const Level& level,
const RadioMeasurementsData& radioMsr
) override;
double getWeight(
const Level& level,
const RadioMeasurementsData& radioMsr,
double x, double y
) const override;
std::vector<XYPoint> calculateSamplingCenters(
const Level& level,
const RadioMeasurementsData& radioMsr
) override;
bool hasSufficientMeasurementsForUpdate(
const Level& level,
const RadioMeasurementsData& radioMsr
) const override;
bool hasSufficientMeasurementsForMutation(const Level& level,
const RadioMeasurementsData& radioMsr
) const override;
MeasurementLikelihoodModel getLikelihoodModel(void) override{return mLikelihoodModel;}
RadioMeasurementsData getOnlyCorrectlyIntersectedMeasurements(
const Level& level,
const RadioMeasurementsData& radioMsr
) const override;
#ifdef NAVIGATION_VISUALIZATION
static XYPoint debugTrianglePoint;
static std::map<ReferencePointId, double> debugRefPointWeights;
#endif
private:
boost::optional<XYPoint> calculateRadiomapPoint(
const LevelId& levelId,
const Radiomap& radiomap,
const std::map<ReferencePointId, double>& rpToWeight);
const MeasurementLikelihoodModel mLikelihoodModel = MeasurementLikelihoodModel::RADIOMAP;
const int mK;
const size_t mMinMsrNumForMutation;
const size_t mMinMsrNumForPositionCalculation;
const bool mUseTriangles;
const bool mUseDiffMode;
//TODO take into account probability of such point!
boost::optional<XYPoint> mLikelihoodPoint; // point for particle mutation and particle sampling
std::map<LevelId, std::vector<RefPointsTriangle>> mLevelTriangles;
std::map<LevelId, std::set<TransmitterId>> mLevelReferenceTransmittersMap;
std::map<LevelId,
std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>>>
mLevelsRadiomaps;
};
} } // namespace navigine::navigation_core
#endif // LIKELIHOOD_TRIANGULATION_H
================================================
FILE: src/measurement_types.h
================================================
#pragma once
#include <navigine/navigation-core/navigation_input.h>
namespace navigine {
namespace navigation_core {
using SensorMeasurement = MeasurementDataWithTimeStamp<SensorMeasurementData>; // TODO: m.doroshenko rename me
using NmeaMeasurement = MeasurementDataWithTimeStamp<NmeaMeasurementData>; // TODO: m.doroshenko rename me
using RadioMeasurement = MeasurementDataWithTimeStamp<RadioMeasurementData>; // TODO: m.doroshenko rename me
} } // namespace navigine::navigation_core
================================================
FILE: src/measurements/measurement_preprocessor.cpp
================================================
/** measurement_preprocessor.cpp
*
* Copyright (c) 2017 Navigine.
*
*/
#include <algorithm>
#include <numeric>
#include "measurement_preprocessor.h"
#if defined (DEBUG_OUTPUT_MSR_PREPROCESSOR)
#include <iostream>
#include <fstream>
namespace navigine {
namespace navigation_core {
static const std::string PREPROCESSED_MSR_DEBUG_OUTPUT_FILE = DEBUG_OUTPUT_MSR_PREPROCESSOR;
void clearDebugOutput();
void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsData& entries);
void printExtractedMeasurements(RadioMeasurementsData& radioMeasurements);
} } // namespace navigine::navigation_core
#endif
namespace navigine {
namespace navigation_core {
namespace {
const constexpr bool DEFAULT_USE_WIFI = false;
const constexpr bool DEFAULT_USE_BLE = true;
bool checkUseWifi(const NavigationSettings& navProps) {
const auto signalsToUse = navProps.commonSettings.signalsToUse;
if (signalsToUse == CommonSettings::SignalsToUse::WIFI || signalsToUse == CommonSettings::SignalsToUse::BOTH) {
return true;
} else if (signalsToUse == CommonSettings::SignalsToUse::BLE) {
return false;
}
return DEFAULT_USE_WIFI;
}
bool checkUseBle(const NavigationSettings& navProps) {
const auto signalsToUse = navProps.commonSettings.signalsToUse;
if (signalsToUse == CommonSettings::SignalsToUse::BLE || signalsToUse == CommonSettings::SignalsToUse::BOTH) {
return true;
} else if (signalsToUse == CommonSettings::SignalsToUse::WIFI) {
return false;
}
return DEFAULT_USE_BLE;
}
RadioMeasurementBuffer createRadioMeasurementBuffer(
const NavigationSettings& navProps) {
long long sigAverageTime = (long long)(1000 * navProps.commonSettings.sigAveragingTime);
long long sigWindowShift = (long long)(1000 * navProps.commonSettings.sigWindowShift);
long long stopDetectTime = (long long)(1000 * navProps.commonSettings.stopDetectionTime);
bool useStops = navProps.commonSettings.useStops;
// TO DO: why RadioMeasurementBuffer is not initialized here
if (sigWindowShift == 0 || sigWindowShift > sigAverageTime)
sigWindowShift = sigAverageTime;
long long keepRadioTimeMs = sigAverageTime - sigWindowShift;
double rssiBias = 0;
return RadioMeasurementBuffer(keepRadioTimeMs, sigWindowShift, stopDetectTime, rssiBias, useStops);
}
}
MeasurementsPreprocessor::MeasurementsPreprocessor(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps)
: mLevelIndex(levelCollector)
, mWiFiRttOffset(navProps.commonSettings.wiFiRttOffset)
, mCutOffRssi(navProps.commonSettings.sigCutOffRss)
, mUseClosestAps(navProps.commonSettings.useClosestAps)
, mNumClosestAps(navProps.commonSettings.numClosestAps)
, mUseUnknownTxs(navProps.commonSettings.useUnknownTransmitters)
, mMeasurementType(navProps.commonSettings.measurementType)
, mUseWifi(checkUseWifi(navProps))
, mUseBle(checkUseBle(navProps))
, mIsTracking(navProps.commonSettings.useTracking)
, mRadiosBuffer(createRadioMeasurementBuffer(navProps))
, mCurrentTs(0)
{ }
void MeasurementsPreprocessor::updateMeasurements(const Measurement& navMsg)
{
mCurrentTs = navMsg.ts;
boost::apply_visitor(*this, navMsg.data);
}
void MeasurementsPreprocessor::operator()(const RadioMeasurementData& measurement)
{
RadioMeasurementData signal = measurement;
boost::optional<RadioMeasurementData> validSignalData = getValidSignalEntry(signal);
if (validSignalData.is_initialized())
{
RadioMeasurement validSignal;
validSignal.data = validSignalData.get();
validSignal.ts = mCurrentTs;
mRadiosBuffer.addMeasurement(validSignal);
mLastSignalTs = mCurrentTs;
}
}
void MeasurementsPreprocessor::operator()(const SensorMeasurementData& measurement)
{
mSensor.data = measurement;
mSensor.ts = mCurrentTs;
}
void MeasurementsPreprocessor::operator()(const NmeaMeasurementData& measurement)
{
mNmea.data = measurement;
mNmea.ts = mCurrentTs;
}
RadioMeasurementsData MeasurementsPreprocessor::extractRadioMeasurements(long long lastStepTime)
{
RadioMeasurementsData radioMeasurements = mRadiosBuffer.extractMeasurements(lastStepTime);
if (mUseClosestAps)
{
std::sort(radioMeasurements.rbegin(), radioMeasurements.rend(),
[](const RadioMeasurementData& lhs, const RadioMeasurementData& rhs)
{
return lhs.rssi < rhs.rssi;
});
if (radioMeasurements.size() > (unsigned int) mNumClosestAps)
radioMeasurements.resize(mNumClosestAps);
}
for (auto& msr : radioMeasurements)
{
if (msr.type != RadioMeasurementData::Type::WIFI_RTT)
{
boost::optional<LevelId> levelId = mLevelIndex->findLevelByTransmitterId(msr.id);
if (levelId.is_initialized())
{
const Level& level = mLevelIndex->level(levelId.get());
Transmitter<XYZPoint> transmitter = level.transmitter(msr.id);
const PathlossModel pathLossModel = transmitter.pathlossModel;
msr.distance = std::exp((pathLossModel.A - msr.rssi) / pathLossModel.B);
}
continue;
}
msr.distance -= mWiFiRttOffset;
}
return radioMeasurements;
}
long long MeasurementsPreprocessor::getCurrentTs() const
{
return mCurrentTs;
}
long long MeasurementsPreprocessor::getLastSignalTs() const
{
return mLastSignalTs;
}
boost::optional<RadioMeasurementData> MeasurementsPreprocessor::getValidSignalEntry(
const RadioMeasurementData& entry) const
{
bool isTransmitterValid = mUseUnknownTxs
|| (mLevelIndex->findLevelByTransmitterId(entry.id).is_initialized()
&& isTransmitterSupported(entry.id));
if (mMeasurementType != CommonSettings::MeasurementType::RSSI && entry.type != RadioMeasurementData::Type::WIFI_RTT)
isTransmitterValid = false;
if (mMeasurementType == CommonSettings::MeasurementType::RSSI && !isRssiValid(entry.rssi))
{
isTransmitterValid = false;
}
if (isTransmitterValid && isSignalTypeSupported(entry.type))
return entry;
else
return boost::none;
}
bool MeasurementsPreprocessor::isSignalTypeSupported(RadioMeasurementData::Type signalType) const
{
return (signalType == RadioMeasurementData::Type::WIFI && mUseWifi) ||
(signalType == RadioMeasurementData::Type::BEACON && mUseBle) ||
(signalType == RadioMeasurementData::Type::BLUETOOTH && mUseBle) ||
(signalType == RadioMeasurementData::Type::EDDYSTONE && mUseBle) ||
(signalType == RadioMeasurementData::Type::WIFI_RTT && mUseWifi);
}
bool MeasurementsPreprocessor::isRssiValid(double rssi) const
{
return rssi < 0.0 && rssi > mCutOffRssi;
}
bool MeasurementsPreprocessor::isTransmitterSupported(const TransmitterId& transmitterId) const
{
boost::optional<LevelId> levelId = mLevelIndex->findLevelByTransmitterId(transmitterId);
if (!levelId.is_initialized())
return false;
if (!mLevelIndex->hasLevel(levelId.get()))
return false;
const Level& level = mLevelIndex->level(levelId.get());
if (!level.containsTransmitter(transmitterId))
return false;
const Transmitter<XYZPoint>& transmitter = level.transmitter(transmitterId);
return mIsTracking ? transmitter.type == TransmitterType::LOCATOR
: transmitter.type != TransmitterType::LOCATOR;
}
RadioMeasurementBuffer::RadioMeasurementBuffer(
long long radioKeepPeriodMs,
long long sigWindowShiftMs,
long long stopDetectionTime,
double rssiBias,
bool useStops)
: mRadioKeepPeriodMs(radioKeepPeriodMs)
, mSignalWindowShiftMs(sigWindowShiftMs)
, mStopDetectionTimeMs(stopDetectionTime)
, mRssiBias(rssiBias)
, mUseStops(useStops)
, mLastExtractionTs(0)
, mCurrentTs(0)
, mLastStepTs(0)
{ }
void RadioMeasurementBuffer::addMeasurement(const RadioMeasurement& msr)
{
if (mCurrentTs == 0)
{
mCurrentTs = msr.ts;
mLastExtractionTs = msr.ts;
}
RadioMeasurement radioMsr = msr;
radioMsr.data.rssi = msr.data.rssi + mRssiBias;
mMeasurements.push_back(radioMsr);
mCurrentTs = msr.ts;
}
RadioMeasurementsData RadioMeasurementBuffer::extractMeasurements(long long lastStepTs)
{
RadioMeasurementsData measurements;
if (lastStepTs > 0)
mLastStepTs = lastStepTs;
if (mCurrentTs - mLastExtractionTs < mSignalWindowShiftMs)
{
// if not enough time passed
return measurements;
}
mLastExtractionTs = mCurrentTs;
// Averaging measurements from same Transmitters
std::sort(mMeasurements.begin(), mMeasurements.end(),
[](const RadioMeasurement& lhs, const RadioMeasurement& rhs)
{
return lhs.data.id < rhs.data.id;
});
auto sameIdFirst = mMeasurements.cbegin();
while (sameIdFirst != mMeasurements.cend())
{
auto sameIdLast = std::upper_bound(sameIdFirst, mMeasurements.cend(), *sameIdFirst,
[](const RadioMeasurement& lhs, const RadioMeasurement& rhs)
{
return lhs.data.id < rhs.data.id;
});
double nSignals = std::distance(sameIdFirst, sameIdLast);
double sumRssi = std::accumulate(sameIdFirst, sameIdLast, 0.0,
[](double sum, const RadioMeasurement& m)
{
return sum + m.data.rssi;
});
double averRssi = sumRssi / nSignals;
double sumDistances = std::accumulate(sameIdFirst, sameIdLast, 0.0,
[](double sum, const RadioMeasurement& m)
{
return sum + m.data.distance;
});
double averDistance = sumDistances / nSignals;
RadioMeasurement radioMsr = *sameIdFirst;
radioMsr.data.rssi = averRssi;
radioMsr.data.distance = averDistance;
measurements.push_back(radioMsr.data);
sameIdFirst = sameIdLast;
}
// In case we are standing still we don't erase old measurements
if (mUseStops & (mLastStepTs > 0) && (mCurrentTs - mLastStepTs > mStopDetectionTimeMs))
return measurements;
// Erasing old measurements
if (mRadioKeepPeriodMs == 0)
{
mMeasurements.clear();
}
else
{
long long keepFromTs = mCurrentTs - mRadioKeepPeriodMs;
std::sort(mMeasurements.begin(), mMeasurements.end(),
[](const RadioMeasurement& lhs, const RadioMeasurement& rhs)
{
return lhs.ts < rhs.ts;
});
auto eraseUpperBound = std::find_if(mMeasurements.begin(),
mMeasurements.end(),
[keepFromTs](const RadioMeasurement& rm)
{
return rm.ts >= keepFromTs;
});
mMeasurements.erase(mMeasurements.begin(), eraseUpperBound);
}
return measurements;
}
SensorMeasurement MeasurementsPreprocessor::getValidSensor() const
{
return mSensor;
}
NmeaMeasurement MeasurementsPreprocessor::getCurrentNmea() const
{
return mNmea;
}
#if defined (DEBUG_OUTPUT_MSR_PREPROCESSOR)
void clearDebugOutput()
{
cout << "print debug output to " << PREPROCESSED_MSR_DEBUG_OUTPUT_FILE << endl;
std::ofstream debugOutputFile;
debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE);
debugOutputFile << "type id ts rssi" << endl;
debugOutputFile.close();
}
void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsData& entries)
{
ofstream debugOutputFile;
debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE, ofstream::out | ofstream::app);
for (RadioMeasurement& entry: entries)
{
long long wsTsSec = navMsg->tmUnixTime;
long long wsTsMs = wsTsSec * 1000 - entry.offset;
debugOutputFile << "INPUT "<< entry.bssid << " " << wsTsMs << " " << entry.data.rssi << endl;
}
debugOutputFile.close();
}
void printExtractedMeasurements(RadioMeasurementsData& radioMeasurements)
{
ofstream debugOutputFile;
debugOutputFile.open(PREPROCESSED_MSR_DEBUG_OUTPUT_FILE, ofstream::out | ofstream::app);
for (RadioMeasurement msr: radioMeasurements)
{
debugOutputFile << "OUTPUT "<< msr.data.id << " " << msr.ts << " " << msr.data.rssi << endl;
}
debugOutputFile.close();
}
#endif
} } // namespace navigine::navigation_core
================================================
FILE: src/measurements/measurement_preprocessor.h
================================================
#ifndef MEASUREMENT_PREPROCESSOR_H
#define MEASUREMENT_PREPROCESSOR_H
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/navigation_settings.h>
#include <navigine/navigation-core/level_collector.h>
#include <functional>
#include <vector>
#include <map>
#include <string>
#include <boost/optional.hpp>
#include "../measurement_types.h"
namespace navigine {
namespace navigation_core {
class RadioMeasurementBuffer
{
public:
RadioMeasurementBuffer(const RadioMeasurementBuffer&) = default;
RadioMeasurementBuffer(long long radioKeepPeriodMs,
long long sigWindowShiftMs,
long long stopDetectionTime,
double rssiBias,
bool useStops);
RadioMeasurementsData extractMeasurements(long long lastStepTime);
void addMeasurement(const RadioMeasurement& msr);
private:
const long long mRadioKeepPeriodMs;
const long long mSignalWindowShiftMs;
const long long mStopDetectionTimeMs;
const double mRssiBias;
const bool mUseStops;
long long mLastExtractionTs;
long long mCurrentTs;
long long mLastStepTs;
std::vector<RadioMeasurement> mMeasurements = {};
};
class MeasurementsPreprocessor: public boost::static_visitor<>
{
public:
MeasurementsPreprocessor(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
void updateMeasurements(const Measurement& navMsg);
long long getCurrentTs() const;
long long getLastSignalTs() const;
RadioMeasurementsData extractRadioMeasurements(long long lastStepTime);
SensorMeasurement getValidSensor() const;
NmeaMeasurement getCurrentNmea() const;
void operator()(const RadioMeasurementData& measurement);
void operator()(const SensorMeasurementData& measurement);
void operator()(const NmeaMeasurementData& measurement);
private:
boost::optional<RadioMeasurementData> getValidSignalEntry(const RadioMeasurementData& entry) const;
bool isSignalTypeSupported(RadioMeasurementData::Type signalType) const;
bool isRssiValid(double rssi) const;
std::pair<bool, LevelId> transmitterLevelId(const TransmitterId& transmitterId) const;
bool isTransmitterSupported(const TransmitterId& transmitterId) const;
private:
const std::shared_ptr<LevelCollector> mLevelIndex;
const double mWiFiRttOffset;
const double mCutOffRssi;
const bool mUseClosestAps;
const int mNumClosestAps;
const bool mUseUnknownTxs;
const CommonSettings::MeasurementType mMeasurementType;
const bool mUseWifi;
const bool mUseBle;
const bool mIsTracking;
long long mLastSignalTs;
RadioMeasurementBuffer mRadiosBuffer;
long long mCurrentTs;
SensorMeasurement mSensor;
NmeaMeasurement mNmea;
};
} } // namespace navigine::navigation_core
#endif // MEASUREMENT_PREPROCESSOR_H
================================================
FILE: src/navigation_client_impl.cpp
================================================
#include <iostream>
#include <navigine/navigation-core/navigation_settings.h>
#include "navigation_client_impl.h"
#include "position_estimator/position_estimator_outdoor.h"
#include "position_estimator/position_estimator_zone.h"
#include "position_estimator/position_estimator_knn.h"
#include "level_estimator/level_estimator_radiomap.h"
#include "level_estimator/level_estimator_transmitters.h"
#include "trilateration.h"
namespace navigine {
namespace navigation_core {
NavigationClientImpl::NavigationClientImpl(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps)
: mUseEnuAzimuth(navProps.commonSettings.useEnuAzimuth)
, mNoSignalTimeThreshold((long long)(1000 * navProps.commonSettings.noSignalTimeThreshold))
, mNoActionTimeThreshold((long long)(1000 * navProps.commonSettings.noActionTimeThreshold))
, mLevelIndex(levelCollector)
, mLevelEstimator(createLevelEstimator(levelCollector, navProps))
, mPositionEstimatorIndoor(createPostitionEstimator(levelCollector, navProps))
, mMsrPreprocessor(std::make_unique<MeasurementsPreprocessor>(levelCollector, navProps))
, mPositionEstimatorOutdoor(std::make_unique<PositionEstimatorOutdoor>(levelCollector, navProps))
, mPositionPostprocessor(std::make_unique<PositionPostprocessor>(navProps))
, mSensorFusion(std::make_unique<SensorFusion>(navProps))
, mFlagIndoorPos(false)
, mPrevFusedPosHeading(boost::none)
, mPrevGyroHeading(boost::none)
, mUseAltitude(navProps.commonSettings.useAltitude)
{ }
std::vector<NavigationOutput> NavigationClientImpl::navigate(const std::vector<Measurement>& navBatchInput)
{
std::vector<NavigationOutput> navBatchOutput;
if (navBatchInput.size() == 0)
return navBatchOutput;
mNavigationStates.clear();
long long prevTs = navBatchInput[0].ts;
for (const auto &navInput: navBatchInput)
{
if (prevTs > navInput.ts)
{
std::cerr << "prevTs: " << prevTs << ", t.ts: " << navInput.ts << std::endl;
continue;
}
else
{
prevTs = navInput.ts;
}
NavigationStatus retStatus = NavigationStatus::OK;
mMsrPreprocessor->updateMeasurements(navInput);
const long long curTs = mMsrPreprocessor->getCurrentTs();
const long long lastSignalTs = mMsrPreprocessor->getLastSignalTs();
const SensorMeasurement sensorMsr = mMsrPreprocessor->getValidSensor();
const NmeaMeasurement nmeaMsr = mMsrPreprocessor->getCurrentNmea();
MotionInfo motionInfo = mSensorFusion->calculateDisplacement(sensorMsr, curTs);
const auto radioMsr = mMsrPreprocessor->extractRadioMeasurements(motionInfo.lastMotionTs);
LevelId levelId = mLevelEstimator->calculateLevel(radioMsr, sensorMsr);
const Position outdoorPos = mPositionEstimatorOutdoor->calculatePosition(curTs, sensorMsr, nmeaMsr, mFlagIndoorPos);
mFlagIndoorPos = false; // TODO refactoring needed
if (curTs - lastSignalTs >= mNoSignalTimeThreshold)
{
mPositionEstimatorIndoor->reInit();
levelId = LevelId();
}
if (curTs - lastSignalTs >= mNoActionTimeThreshold && curTs - motionInfo.lastMotionTs >= mNoActionTimeThreshold && motionInfo.lastMotionTs > 0)
{
mPositionEstimatorIndoor->reInit();
levelId = LevelId();
}
if (mLevelIndex->hasLevel(levelId))
{
const Level& level = mLevelIndex->level(levelId);
const Position indoorPos = mPositionEstimatorIndoor->calculatePosition(level, curTs, radioMsr, motionInfo, retStatus);
if (!indoorPos.isEmpty)
mFlagIndoorPos = true;
const Position fusedPos = mPositionPostprocessor->fusePositions(curTs, indoorPos, outdoorPos, retStatus);
if (!fusedPos.isEmpty && mPrevFusedPosHeading.is_initialized() && (fusedPos.heading != mPrevFusedPosHeading.get()))
{
mPrevGyroHeading = motionInfo.gyroHeading;
motionInfo.isAzimuthValid = false;
}
else if (mPrevGyroHeading.is_initialized())
{
double deltaGyroHeading = motionInfo.gyroHeading - mPrevGyroHeading.get();
motionInfo.azimuth = - (mPrevFusedPosHeading.get() - deltaGyroHeading - M_PI_2); // NED
motionInfo.isAzimuthValid = true;
}
if (!fusedPos.isEmpty)
mPrevFusedPosHeading = fusedPos.heading;
mPosition = mPositionPostprocessor->getProcessedPosition(fusedPos, curTs, motionInfo, level);
if (!motionInfo.isAzimuthValid) {
motionInfo.azimuth = - (fusedPos.heading - M_PI_2); // convert from ENU to NED
}
if (mUseEnuAzimuth)
motionInfo.azimuth = -motionInfo.azimuth + M_PI_2; // convert from NED to ENU
}
else if (!outdoorPos.isEmpty)
{
mPosition = outdoorPos;
}
else
{
retStatus = NavigationStatus::NO_LEVEL;
}
// Filling navigationSate for Debug
auto navigationState = mPositionEstimatorIndoor->getParticleFilterState();
navigationState.setStepCounter(motionInfo.stepCounter);
navigationState.setStepLen(motionInfo.distance);
navigationState.setIndoorPosition(XYPoint(mPositionPostprocessor->getIndoorPosition().x,
mPositionPostprocessor->getIndoorPosition().y));
navigationState.setOutdoorPosition(XYPoint(mPositionPostprocessor->getOutdoorPosition().x,
mPositionPostprocessor->getOutdoorPosition().y));
mNavigationStates.push_back(navigationState);
// Filling navOutput
NavigationOutput navOutput;
navOutput.status = retStatus;
// TODO: optimize NavClient state variables (mPosition, mRetStatus, etc.)
// https://redmine.navigine.com/issues/2554
if (mPosition.isEmpty)
{
if (retStatus == NavigationStatus::OK)
{
navOutput.status = NavigationStatus::NAVIGATION_ERROR;
//TODO ret rid of empty position status
}
navBatchOutput.emplace_back(navOutput);
continue;
}
else
{
navOutput.status = NavigationStatus::OK;
}
navOutput.posLevel = mPosition.levelId;
if (navOutput.posLevel.isValid())
{
const Level& level = mLevelIndex->level(navOutput.posLevel);
const GeoPoint gpsPosition = localToGps(XYPoint(mPosition.x, mPosition.y), level.bindingPoint());
navOutput.posLatitude = gpsPosition.latitude;
navOutput.posLongitude = gpsPosition.longitude;
if (mUseAltitude)
{
Trilateration altitudeWorker;
boost::optional<double> altitude = altitudeWorker.calculateAltitude(level, radioMsr);
if (altitude.is_initialized())
navOutput.posAltitude = altitude.get();
}
}
navOutput.posR = mPosition.deviationM;
navOutput.provider = mPosition.provider;
navOutput.posOrientation = to_minus_Pi_Pi(motionInfo.azimuth);
navBatchOutput.emplace_back(navOutput);
}
return navBatchOutput;
}
std::unique_ptr<LevelEstimator> NavigationClientImpl::createLevelEstimator(
const std::shared_ptr<navigine::navigation_core::LevelCollector>& levelCollector,
const NavigationSettings& navProps)
{
const bool useRadiomap = navProps.commonSettings.useRadiomap;
// the following three lines are used for navigation.xml backcompatibility
const CommonSettings::UseAlgorithm algName = navProps.commonSettings.useAlgorithm;
const bool useKnn = (algName == CommonSettings::UseAlgorithm::KNN);
if (useRadiomap || useKnn)
return std::make_unique<LevelEstimatorRadiomap>(levelCollector, navProps);
return std::make_unique<LevelEstimatorTransmitters>(levelCollector, navProps);
}
std::unique_ptr<PositionEstimator> NavigationClientImpl::createPostitionEstimator(
const std::shared_ptr<navigine::navigation_core::LevelCollector>& levelCollector,
const NavigationSettings& navProps)
{
const CommonSettings::UseAlgorithm algName = navProps.commonSettings.useAlgorithm;
bool usePDR;
if (algName == CommonSettings::UseAlgorithm::KNN) return std::make_unique<PositionEstimatorKnn>(levelCollector, navProps);
return std::make_unique<PositionEstimatorZone>(levelCollector, navProps);
}
std::vector<NavigationState> NavigationClientImpl::getStates() const
{
return mNavigationStates;
}
std::shared_ptr<NavigationClient> createNavigationClient(
const std::shared_ptr<navigine::navigation_core::LevelCollector>& levelCollector,
const NavigationSettings& navProps)
{
return std::make_shared<NavigationClientImpl>(levelCollector, navProps);
}
} } // namespace navigine::navigation_core
================================================
FILE: src/navigation_client_impl.h
================================================
#pragma once
#include <navigine/navigation-core/navigation_client.h>
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/navigation_output.h>
#include <navigine/navigation-core/navigation_state.h>
#include <navigine/navigation-core/navigation_settings.h>
#include <navigine/navigation-core/level_collector.h>
#include "measurements/measurement_preprocessor.h"
#include "position_estimator/position_estimator.h"
#include "position_estimator/position_estimator_outdoor.h"
#include "position_postprocessor/position_postprocessor.h"
#include "sensor_fusion/sensor_fusion.h"
#include "level_estimator/level_estimator.h"
namespace navigine {
namespace navigation_core {
class NavigationClientImpl : public NavigationClient
{
public:
NavigationClientImpl(
const std::shared_ptr<navigine::navigation_core::LevelCollector>& levelCollector,
const NavigationSettings& navProps);
// Return navigation state of this client
std::vector<NavigationState> getStates() const override;
// Main navigation function. Calculates current position based on incoming measurements
std::vector<NavigationOutput> navigate(const std::vector<Measurement>& navBatchInput) override;
private:
NavigationClientImpl(const NavigationClientImpl&) = delete;
void operator=(const NavigationClientImpl&) = delete;
std::unique_ptr<LevelEstimator> createLevelEstimator(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
std::unique_ptr<PositionEstimator> createPostitionEstimator(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
private:
const bool mUseEnuAzimuth;
const long long mNoSignalTimeThreshold;
const long long mNoActionTimeThreshold;
std::shared_ptr<LevelCollector> mLevelIndex;
std::unique_ptr<LevelEstimator> mLevelEstimator;
std::unique_ptr<PositionEstimator> mPositionEstimatorIndoor;
std::unique_ptr<MeasurementsPreprocessor> mMsrPreprocessor;
std::unique_ptr<PositionEstimatorOutdoor> mPositionEstimatorOutdoor;
std::unique_ptr<PositionPostprocessor> mPositionPostprocessor;
std::unique_ptr<SensorFusion> mSensorFusion;
std::vector<NavigationState> mNavigationStates = {};
Position mPosition = {};
bool mFlagIndoorPos;
boost::optional<double> mPrevFusedPosHeading;
boost::optional<double> mPrevGyroHeading;
const bool mUseAltitude;
};
} } // namespace navigine::navigation_core
================================================
FILE: src/navigation_error_codes.h
================================================
/** navigation_error_codes.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef NAVIGINE_NAVIGATION_ERROR_CODES_H
#define NAVIGINE_NAVIGATION_ERROR_CODES_H
namespace navigine {
namespace navigation_core {
static const int ERROR_NO_SOLUTION = 4;
static const int MISSING_STRING_CONFIG = 5;
static const int MISSING_LEVELS = 6;
static const int NO_PARAMETERS_IN_JSON_FILE = 21;
static const int ERROR_OPEN_FILE = 25;
static const int INVALID_LOC_ID_IN_JSON_FILE = 26;
static const int INVALID_PARAMETER_IN_JSON_FILE = 27;
} } // namespace navigine::navigation_core
#endif // NAVIGINE_NAVIGATION_ERROR_CODES_H
================================================
FILE: src/point.cpp
================================================
/** point.cpp
*
* Copyright (c) 2019 Navigine.
*
*/
#include <navigine/navigation-core/point.h>
#include <cmath>
namespace navigine {
namespace navigation_core {
constexpr double EQUATOR_CIRCUMFERENCE_METERS = 40075160.0;
constexpr double POLE_CIRCUMFERENCE_METERS = 40008000.0;
// Normalize longitude to (-180, 180]
double normalizeLongitude(double lon)
{
if (lon > 180)
{
lon -= 360;
} else if (lon <= -180)
{
lon += 360;
}
return lon;
}
double degToRad(double deg)
{
return deg * M_PI / 180.0;
}
XYPoint gpsToLocal(const GeoPoint& point, const GeoPoint& bindPoint)
{
const double deltaLat = point.latitude - bindPoint.latitude;
const double deltaLon = normalizeLongitude(point.longitude - bindPoint.longitude);
const double latitudeCircumference =
EQUATOR_CIRCUMFERENCE_METERS * std::cos(degToRad(bindPoint.latitude));
XYPoint localPoint;
localPoint.x = deltaLon * (latitudeCircumference / 360.0);
localPoint.y = deltaLat * (POLE_CIRCUMFERENCE_METERS / 360.0);
return localPoint;
}
GeoPoint localToGps(const XYPoint& localPoint, const GeoPoint& bindPoint)
{
const double latitudeCircumference = EQUATOR_CIRCUMFERENCE_METERS * std::cos(degToRad(bindPoint.latitude));
const double lat = (localPoint.y * (360.0 / POLE_CIRCUMFERENCE_METERS)) + bindPoint.latitude;
const double lon = normalizeLongitude(localPoint.x * 360.0 / latitudeCircumference + bindPoint.longitude);
return GeoPoint(lat, lon);
}
} } // namespace navigine::navigation_core
================================================
FILE: src/position.h
================================================
/** position.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef NAVIGINE_POSITION_H
#define NAVIGINE_POSITION_H
#include <string>
#include <navigine/navigation-core/point.h>
#include <navigine/navigation-core/navigation_output.h>
#include <navigine/navigation-core/level.h>
namespace navigine {
namespace navigation_core {
struct Position : XYPoint
{
Position()
: XYPoint(0.0, 0.0)
, levelId(LevelId(""))
, deviationM(0)
, ts(0)
, isEmpty(true)
, provider(Provider::NONE)
, heading(0.0)
{ }
Position(double _x, double _y, LevelId _levelId,
double _deviationM, long long _ts, bool _isEmpty,
const Provider& _provider,
double _heading)
: XYPoint(_x, _y)
, levelId(_levelId)
, deviationM(_deviationM)
, ts(_ts)
, isEmpty(_isEmpty)
, provider(_provider)
, heading(_heading)
{ }
LevelId levelId;
double deviationM;
long long ts;
bool isEmpty = true; //TODO use boost optional for empty?
Provider provider = Provider::NONE;
double heading;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_POSITION_H
================================================
FILE: src/position_estimator/Readme.md
================================================
================================================
FILE: src/position_estimator/docs/KNN.md
================================================
# K-Nearest Neighbours Positioning Algorithm
Note:: The algorithm requires special radiomap data measurement that can beperformed using procedure called Radiomap Measurement.
For detailed information of data collection for algorithm usage refer to [https://docs.navigine.com/Measuring_Radiomap](https://docs.navigine.com/Measuring_Radiomap)
Radiomap 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.
*Requirements*:
* infrastructure deployed
* maps (locations) implemented
* [radiomap data](https://docs.navigine.com/Measuring_Radiomap) collected
*Our notation*:
* RSSI
* KNN
* transmismitter
* beacon
* BLE
* radio map
## Measurements & context
Algorithm 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.
All 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.
<!-- The positions of beacons are not known to the program. -->
<!--
Radio Map can give us information on our position based on measurements similarity. -->
<!-- weighted centroid approach -->
<!-- The measuring process was repeated 30 times for each of
the 68 test point locations resulting in a total amount of
24 480 RSS values collected-->
<!-- /The achieved measurement re-
peatability -->
The 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.
### What means **access point** is KNN positioning context
We measure incoming RSSI in several points of building for a long enough time.
After 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.
The "access point" is the landmark, point with known coordinates and a history of observations from each beacon in visible range.
### KNN logfile and map contents
On figure above we see GeoJSON visualization containing some access points (circles).
The structure of data used for this algorithm is as follows:
```json
{
"type": "Feature",
"properties": {
"name": null,
"type": "reference_point",
"level": "5179",
"id": "1",
"uuid": "eff14dbc-b33a-11e8-945d-7c7a91af7e27",
"entries": [
{
"value": [
{
"rssi": 83,
"count": 6
},
{
...
},
],
"bssid": "20:18:00:00:04:32",
"type": "WIFI"
},
{
...
}
]
},
"geometry": {
"type": "Point",
"coordinates": [
-1.6307104042,
47.225793733499998
]
}
}
```
For each entity of "type": "reference_point" we have many observations for each detected beacon in area.
Approximately we can say:
The example log will consist of 20-100 access point locations, 10000 RSSI observations from 20-50 detected beacons for a single location.
We 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.
## KNN algorithm setup
The settings we use for program configuration:
```json
"Use_algorithm": "KNN",
"Use_triangles": true,
"Min_num_of_measurements_for_position_calculation": 6,
"Use_diff_mode": true,
"K": 3,
```
We have two mode options for this algorithm called tringle and differential modes respectively.
<!-- Averaging signal for a long time we reduce measurement noise. -->
<!--
we skip RPs with 0 weights because they are most improbable
but if all signals coincide to map, we should add small additive
TODO:: Coefficient to find weight of AP (See Accuracy of RSS-Based Centroid Localization -->
## The algorithm
### KNN algorithm math and general concept
KNN approach is based on a radio-channel propagation model:
<p align="center"><img alt="RSS(d) = RSS(d_0)-10\mu \log \frac{d}{d_0} + w " src="https://render.githubusercontent.com/render/math?math=RSS%28d%29%20%3D%20RSS%28d_0%29-10%5Cmu%20%5Clog%20%5Cfrac%7Bd%7D%7Bd_0%7D%20%2B%20w%20"/></p>
The goal of an RSS-based localization algorithm is to provide estimate <img alt="\hat p = (\hat x, \hat y)" src="https://render.githubusercontent.com/render/math?math=%5Chat%20p%20%3D%20%28%5Chat%20x%2C%20%5Chat%20y%29" style="transform: translateY(20%);" />
of position <img alt="p" src="https://render.githubusercontent.com/render/math?math=p" style="transform: translateY(20%);" /> given the vector <img alt="[RSS_1, RSS_2, . . . , RSS_n]" src="https://render.githubusercontent.com/render/math?math=%5BRSS_1%2C%20RSS_2%2C%20.%20.%20.%20%2C%20%20RSS_n%5D" style="transform: translateY(20%);" />.
There exist several algorithms that can be used to determine
the position of a target through RSS measurements: Some
of them are geometric methods, such as lateration or a
minimum—maximum method (min—max), whereas some
others are based on statistical approaches, such as maximum
likelihood.
The algorithms for localization can be classified as either statistical of geometrical.
When the target node communicates with all the anchors, i.e., all anchors are visible, the centroid results the center of the anchors coordinates.
If 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.
Anchors - beacons transmitting the signal.
<p align="center"><img alt="\hat p = \frac{1}{m}\cdot \sum_{i=1}^m {a_i} " src="https://render.githubusercontent.com/render/math?math=%5Chat%20p%20%3D%20%5Cfrac%7B1%7D%7Bm%7D%5Ccdot%20%5Csum_%7Bi%3D1%7D%5Em%20%7Ba_i%7D%20"/></p>
If we have <img alt="m" src="https://render.githubusercontent.com/render/math?math=m" style="transform: translateY(20%);" /> different anchor points in our collected data - cardinality of visible subset. (under assumption that all the visible anchors are equally close to target node).
### Implementation
#### Initialization step
* collect measurements
* construct building radio map
Firstly we organise the recorded data, write set of detected anchors to their respective sublocations.
We 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.
#### RP weights estimation step
* get measurements
* get reference points for the respective location
* filter outliers
* stop until enough measurements collected
* calculate weights of RPs based on RSS measurements and building radio map
* calculate position based on known RPs weights and positions (see below)
We select the level / sublocation for localization.
For 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.
We assume that reference points with the most similar signal are also the closest one, because of our signal prpagation model.
#### Position calculation
The pose estimation from weighted set of closest reference points can be performed in different ways: geometrically, statistically, etc.
Based on our settings, we have two options:
##### Weighted centroid approach
The position is the weighted mean of closest reference points with respect to their likelihood / signal similarity.
<p align="center"><img alt="\hat x = \sum_{i=1}^{n}{x_i \cdot w_i};\\
\hat y = \sum_{i=1}^{n}{y_i \cdot w_i}
" src="https://render.githubusercontent.com/render/math?math=%5Chat%20x%20%3D%20%5Csum_%7Bi%3D1%7D%5E%7Bn%7D%7Bx_i%20%5Ccdot%20w_i%7D%3B%5C%5C%0A%20%5Chat%20y%20%3D%20%5Csum_%7Bi%3D1%7D%5E%7Bn%7D%7By_i%20%5Ccdot%20w_i%7D%0A"/></p>
Where <img alt="n" src="https://render.githubusercontent.com/render/math?math=n" style="transform: translateY(20%);" /> is the cardinality of closest reference points set.
##### Triangulation
* Triangulate our reference points set.
* Select triange with maximum cumulative weight: <img alt="w_i = w_1 + w_2 + w_3" src="https://render.githubusercontent.com/render/math?math=w_i%20%3D%20w_1%20%2B%20w_2%20%2B%20w_3" style="transform: translateY(20%);" />
From 3 closest reference points we select the triangle; Based on our signal model we calculate the position.
<p align="center"><img alt="\hat x = \sum_{i=1}^{3}{x_i \cdot w_i};\\
\hat y = \sum_{i=1}^{3}{y_i \cdot w_i}
" src="https://render.githubusercontent.com/render/math?math=%5Chat%20x%20%3D%20%5Csum_%7Bi%3D1%7D%5E%7B3%7D%7Bx_i%20%5Ccdot%20w_i%7D%3B%5C%5C%0A%20%5Chat%20y%20%3D%20%5Csum_%7Bi%3D1%7D%5E%7B3%7D%7By_i%20%5Ccdot%20w_i%7D%0A"/></p>
* Pros:: direct calclation with low number of reference points required. Robust solution.
* 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.
##### Discussion
The 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.
If 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.
## Preview & examples
Demo video for KNN in triangulation mode:
https://user-images.githubusercontent.com/68880242/139697439-7cffb9a0-e8cf-4f52-8ba3-f40350acbd14.mov
================================================
FILE: src/position_estimator/position_estimator.h
================================================
#ifndef POSITION_ESTIMATOR_H
#define POSITION_ESTIMATOR_H
#include <navigine/navigation-core/navigation_output.h>
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/navigation_state.h>
#include <navigine/navigation-core/navigation_settings.h>
#include <navigine/navigation-core/level_collector.h>
#include <navigine/navigation-core/motion_info.h>
#include "../position.h"
namespace navigine {
namespace navigation_core {
class PositionEstimator
{
public:
virtual ~PositionEstimator() = default;
PositionEstimator(const std::shared_ptr<LevelCollector>& levelCollector)
: mLevelIndex(levelCollector)
, mPosition(Position())
, mParticleFilterState(NavigationState())
{ }
virtual Position calculatePosition(
const Level& level,
long long ts,
const RadioMeasurementsData& radioMsr,
const MotionInfo& motionInfo,
NavigationStatus& retStatus) = 0;
virtual void reInit() {};
NavigationState getParticleFilterState() const { return mParticleFilterState; }
protected:
std::shared_ptr<LevelCollector> mLevelIndex;
Position mPosition;
NavigationState mParticleFilterState;
};
} } // namespace navigine::navigation_core
#endif // POSITION_ESTIMATOR_H
================================================
FILE: src/position_estimator/position_estimator_knn.cpp
================================================
/** position_estimator_knn.cpp
*
* Copyright (c) 2018 Navigine.
*
*/
#include <algorithm>
#include <numeric>
#include <cmath>
#include <navigine/navigation-core/navigation_settings.h>
#include "position_estimator_knn.h"
namespace navigine {
namespace navigation_core {
#ifdef NAVIGATION_VISUALIZATION
std::map<ReferencePointId, double>
PositionEstimatorKnn::debugRefPointWeights = std::map<ReferencePointId, double>();
#endif
PositionEstimatorKnn::PositionEstimatorKnn(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps)
: PositionEstimator(levelCollector)
, mUseTriangles(navProps.commonSettings.useTriangles)
, mUseDiffMode(navProps.commonSettings.useDiffMode)
, mK(navProps.commonSettings.kNeighbors)
, mMinMsrNumForPositionCalculation(navProps.commonSettings.minNumOfMeasurementsForPositionCalculation)
, mLevelTriangles(triangulate(levelCollector))
, mLevelReferenceTransmittersMap(getLevelReferenceTransmittersMap(levelCollector))
, mLevelsRadiomaps(getLevelsRadiomaps(levelCollector))
{
}
Position PositionEstimatorKnn::calculatePosition(
const Level& level,
long long ts,
const RadioMeasurementsData& radioMsr,
const MotionInfo&,
NavigationStatus &retStatus)
{
if (radioMsr.empty())
{
retStatus = NavigationStatus::NO_SOLUTION;
return mPosition;
}
retStatus = NavigationStatus::OK;
const std::set<TransmitterId>& beaconsIds = mLevelReferenceTransmittersMap.at(level.id());
auto validMsr = getWhitelistedMeasurements(beaconsIds, radioMsr);
if (validMsr.size() < mMinMsrNumForPositionCalculation)
{
retStatus = NavigationStatus::NO_SOLUTION;
return mPosition;
}
const Radiomap& refPointsMap = level.radiomap();
const std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>>& signalMap = mLevelsRadiomaps.at(level.id());
std::map<ReferencePointId, double> rpToWeight = calcRpWeightsKnn(signalMap, validMsr, mUseDiffMode);
#ifdef NAVIGATION_VISUALIZATION
std::map<ReferencePointId, double> weights;
for (std::map<ReferencePointId, double>::iterator it = rpToWeight.begin();
it != rpToWeight.end(); it++)
weights.insert(*it);
debugRefPointWeights = weights;
#endif
XYPoint p = XYPoint(0.0, 0.0);
if (mUseTriangles)
{
if (mLevelTriangles.find(level.id()) == mLevelTriangles.end())
{
retStatus = NavigationStatus::NAVIGATION_ERROR;
return mPosition;
}
const std::vector<RefPointsTriangle>& triangles = mLevelTriangles.find(level.id())->second;
if (triangles.empty())
{
retStatus = NavigationStatus::NO_RPS;
return mPosition;
}
p = calcPositionInTriangle(triangles, refPointsMap, rpToWeight);
}
else
{
p = calcKHeaviestRefPointsAverage(refPointsMap, rpToWeight, mK);
}
bool isEmpty = false;
double precision = 1.0; //TO FIX: calculate position accuracy
mPosition = Position(p.x, p.y, level.id(), precision, ts, isEmpty, Provider::INDOOR, 0.0);
return mPosition;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/position_estimator/position_estimator_knn.h
================================================
/** position_estimator_knn.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef POSITION_ESTIMATOR_KNN_H
#define POSITION_ESTIMATOR_KNN_H
#include <navigine/navigation-core/navigation_output.h>
#include "position_estimator.h"
#include "../knn/knn_utils.h"
namespace navigine {
namespace navigation_core {
class PositionEstimatorKnn: public PositionEstimator
{
public:
PositionEstimatorKnn(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
Position calculatePosition(
const Level& level,
long long ts,
const RadioMeasurementsData& radioMsr,
const MotionInfo& motionInfo,
NavigationStatus& retStatus) override;
#ifdef NAVIGATION_VISUALIZATION
static std::map<ReferencePointId, double> debugRefPointWeights;
#endif
private:
const bool mUseTriangles;
const bool mUseDiffMode;
const size_t mK;
const size_t mMinMsrNumForPositionCalculation;
const std::map<LevelId, std::vector<RefPointsTriangle>> mLevelTriangles;
const std::map<LevelId, std::set<TransmitterId>> mLevelReferenceTransmittersMap;
const std::map<LevelId,
std::multimap<TransmitterId,
std::pair<ReferencePointId, SignalStatistics>>> mLevelsRadiomaps;
};
} } // namespace navigine::navigation_core
#endif // POSITION_ESTIMATOR_KNN_H
================================================
FILE: src/position_estimator/position_estimator_outdoor.cpp
================================================
/** position_estimator_outdoor.cpp
*
* Copyright (c) 2018 Navigine.
*
*/
#include <navigine/navigation-core/navigation_settings.h>
#include "position_estimator_outdoor.h"
namespace navigine {
namespace navigation_core {
bool SigmaFilter::add(const XYPoint& point)
{
bool pntValid = false;
if (mGNSSbuffer.size() == mSigmaWindow) {
double sumX = 0;
double sumY = 0;
for (auto const& pnt : mGNSSbuffer)
{
sumX += pnt.x;
sumY += pnt.y;
}
double meanX = sumX / mSigmaWindow;
double meanY = sumY / mSigmaWindow;
double accumX = 0.0;
double accumY = 0.0;
std::for_each(std::begin(mGNSSbuffer), std::end(mGNSSbuffer), [&](auto const d) {
accumX += (d.x - meanX) * (d.x - meanX);
accumY += (d.y - meanY) * (d.y - meanY);
});
auto deviationX = 3.0 * std::sqrt(accumX / mSigmaWindow) + mSigmaDelta;
auto deviationY = 3.0 * std::sqrt(accumY / mSigmaWindow) + mSigmaDelta;
if (std::abs(meanX - point.x) <= deviationX &&
std::abs(meanY - point.y) <= deviationY)
pntValid = true;
mGNSSbuffer.pop_front();
}
mGNSSbuffer.push_back(point);
return pntValid;
}
void SigmaFilter::clear()
{
mGNSSbuffer.clear();
}
PositionEstimatorOutdoor::PositionEstimatorOutdoor(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps)
: mLevelIndex (levelCollector)
, mSigmaFilter(navProps.commonSettings.sigmaFilterWindow, navProps.commonSettings.sigmaFilterDelta)
, mGpsValidTimeWindowSec(navProps.commonSettings.gpsValidTimeWindowSec)
, mUseGpsOutsideMap(navProps.commonSettings.useGpsOutsideMap)
, mUseSigmaFilter(navProps.commonSettings.useGpsSigmaFilter)
, mMinNumSats(navProps.commonSettings.minNumSats)
, mMinGpsDeviation(navProps.commonSettings.minGpsDeviation)
, mMaxGpsDeviation(navProps.commonSettings.maxGpsDeviation)
{ }
GpsPosition PositionEstimatorOutdoor::extractGpsPosition(long long ts, const SensorMeasurement& sensorMsr)
{
if (sensorMsr.data.type == SensorMeasurementData::Type::LOCATION)
{
double lat = sensorMsr.data.values.x;
double lng = sensorMsr.data.values.y;
double dev = std::max(sensorMsr.data.values.z, mMinGpsDeviation);
GpsPosition pos = GpsPosition(lat, lng, dev, ts);
return pos;
}
else
{
return GpsPosition();
}
}
int PositionEstimatorOutdoor::extractNumberOfSatellites(const NmeaMeasurement& nmeaEntry)
{
int totalNumSats = 0;
if ((nmeaEntry.data.sentenceNumber == 1) && (nmeaEntry.data.satellitesNumber != 0))
totalNumSats += nmeaEntry.data.satellitesNumber;
return totalNumSats;
}
Position PositionEstimatorOutdoor::calculatePosition(
long long ts,
const SensorMeasurement& sensorEntry,
const NmeaMeasurement& nmeaEntry,
const bool flagIndoorPos)
{
GpsPosition gpsPos = extractGpsPosition(ts, sensorEntry);
int totalNumSats = extractNumberOfSatellites(nmeaEntry);
if (flagIndoorPos || (gpsPos.deviation > mMaxGpsDeviation))
{
mSigmaFilter.clear(); //If there is indoor solution then clear sigma filter GNSS buffer.
}
if (gpsPos.isEmpty)
{
double timeSinceLastGpsPosition = (ts - mLastGpsPosition.ts) / 1000.0;
if (timeSinceLastGpsPosition > mGpsValidTimeWindowSec)
return Position(); //TODO error code: gps is empty!
else
return mLastGpsPosition;
}
//TODO find map closest to previous position (at least check if inside)!!!!
for (const Level& level: mLevelIndex->levels())
{
XYPoint point = gpsToLocal(GeoPoint(gpsPos.latitude, gpsPos.longitude), level.bindingPoint());
if ((!std::isnan(point.x) && !std::isnan(point.y)
&& boost::geometry::covered_by(Point(point.x, point.y), level.geometry().boundingBox())) || mUseGpsOutsideMap)
{
Position outPos;
outPos.ts = gpsPos.ts;
outPos.x = point.x;
outPos.y = point.y;
outPos.levelId = level.id();
outPos.isEmpty = false;
outPos.deviationM = gpsPos.deviation;
outPos.provider = Provider::GNSS;
bool isValid = true;
if (outPos.ts != mLastGpsPosition.ts)
{
if (mUseSigmaFilter && !mSigmaFilter.add(point))
isValid = false;
}
if (!isValid || (totalNumSats != 0 && totalNumSats < mMinNumSats))
return Position();
mLastGpsPosition = outPos;
return outPos;
}
}
return Position(); //TODO error code: "can not find map for gps position!"
}
} } // namespace navigine::navigation_core
================================================
FILE: src/position_estimator/position_estimator_outdoor.h
================================================
/** position_estimator_outdoor.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef NAVIGINE_POSITION_ESTIMATOR_OUTDOOR_H
#define NAVIGINE_POSITION_ESTIMATOR_OUTDOOR_H
#include <navigine/navigation-core/navigation_output.h>
#include <navigine/navigation-core/navigation_input.h>
#include <navigine/navigation-core/navigation_settings.h>
#include <navigine/navigation-core/level_collector.h>
#include <deque>
#include "../position.h"
#include "../measurement_types.h"
namespace navigine {
namespace navigation_core {
struct GpsPosition
{
GpsPosition(double _latitude, double _longitude, double _deviation, long long _ts)
: latitude (_latitude)
, longitude (_longitude)
, deviation (_deviation)
, ts (_ts)
{
isEmpty = false;
}
GpsPosition()
{
isEmpty = true;
}
double latitude = NAN;
double longitude = NAN;
double altitude = NAN;
double deviation = NAN;
long long ts = 0;
bool isEmpty = true;
};
class SigmaFilter
{
public:
SigmaFilter(
const unsigned int& sigmaWindow,
const double& sigmaDelta)
: mSigmaWindow(sigmaWindow)
, mSigmaDelta(sigmaDelta)
{}
bool add(const XYPoint& point);
void clear();
private:
const unsigned int mSigmaWindow;
const double mSigmaDelta;
std::deque<XYPoint> mGNSSbuffer;
};
class PositionEstimatorOutdoor
{
public:
PositionEstimatorOutdoor(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
Position calculatePosition(
long long ts,
const SensorMeasurement& sensorMsr,
const NmeaMeasurement& nmeaMsr,
const bool flagIndoorPos);
private:
GpsPosition extractGpsPosition(long long ts, const SensorMeasurement& sensorEntry);
int extractNumberOfSatellites(const NmeaMeasurement& nmeaEntry);
Position mLastGpsPosition;
std::shared_ptr<LevelCollector> mLevelIndex;
SigmaFilter mSigmaFilter;
const double mGpsValidTimeWindowSec;
const bool mUseGpsOutsideMap;
const bool mUseSigmaFilter;
const int mMinNumSats;
const double mMinGpsDeviation;
const double mMaxGpsDeviation;
};
} } // namespace navigine::navigation_core
#endif // POSITION_ESTIMATOR_OUTDOOR_H
================================================
FILE: src/position_estimator/position_estimator_zone.cpp
================================================
/** position_estimator_zone.cpp
*
* Copyright (c) 2017 Navigine.
*
*/
#include <navigine/navigation-core/navigation_settings.h>
#include "position_estimator_zone.h"
namespace navigine {
namespace navigation_core {
PositionEstimatorZone::PositionEstimatorZone(
const std::shared_ptr<LevelCollector> &levelCollector,
const NavigationSettings& navProps)
: PositionEstimator(levelCollector)
{
for (const Level& level: levelCollector->levels())
{
mLogModelParameterA[level.id()] = navProps.levelsSettings.at(level.id()).normalModeA;
mLogModelParameterB[level.id()] = navProps.levelsSettings.at(level.id()).normalModeB;
}
}
// TODO take into account transmitter power!
Position PositionEstimatorZone::calculatePosition(
const Level& level,
long long ts,
const RadioMeasurementsData& radioMsr,
const MotionInfo&,
NavigationStatus &retStatus)
{
if (radioMsr.empty())
{
retStatus = NavigationStatus::NO_SOLUTION;
return mPosition;
}
retStatus = NavigationStatus::OK;
RadioMeasurementsData radioMeasurements = getLevelRadioMeasurements(level, radioMsr);
if (radioMeasurements.empty())
{
retStatus = NavigationStatus::NO_SOLUTION;
return Position();
}
auto nearestTx = std::max_element(radioMeasurements.begin(), radioMeasurements.end(),
[](RadioMeasurementData msr1, RadioMeasurementData msr2) {return msr1.rssi < msr2.rssi; });
TransmitterId nearestTxId = nearestTx->id;
XYZPoint p = XYZPoint(0.0, 0.0, 0.0);
if (level.containsTransmitter(nearestTxId))
{
p = level.transmitter(nearestTxId).point;
}
//TODO rename prop from A to "GP_Normal_Mode_A"
double A = mLogModelParameterA[level.id()];
double B = mLogModelParameterB[level.id()];
double nearestTxRssi = nearestTx->rssi;
double precision = sqrt(exp((A - nearestTxRssi) / B)) + 1.0;
return Position(
p.x,
p.y,
level.id(),
precision,
ts,
false,
Provider::INDOOR,
0.0);
}
} } // namespace navigine::navigation_core
================================================
FILE: src/position_estimator/position_estimator_zone.h
================================================
/** position_estimator_zone.h
*
* Copyright (c) 2017 Navigine.
*
*/
#ifndef POSITION_ESTIMATOR_ZONE_H
#define POSITION_ESTIMATOR_ZONE_H
#include "position_estimator.h"
namespace navigine {
namespace navigation_core {
class PositionEstimatorZone: public PositionEstimator
{
public:
PositionEstimatorZone(
const std::shared_ptr<LevelCollector>& levelCollector,
const NavigationSettings& navProps);
Position calculatePosition(
const Level& level,
long long ts,
const RadioMeasurementsData& radioMsr,
const MotionInfo& motionInfo,
NavigationStatus& retStatus) override;
private:
std::map<LevelId, double> mLogModelParameterA;
std::map<LevelId, double> mLogModelParameterB;
};
} } // namespace navigine::navigation_core
#endif // POSITION_ESTIMATOR_ZONE_H
================================================
FILE: src/position_postprocessor/navigation_time_smoother.cpp
================================================
/** navigation_time_smoother.cpp
*
* Copyright (c) 2018 Navigine.
*
*/
#include "navigation_time_smoother.h"
namespace navigine {
namespace navigation_core {
NavigationTimeSmoother::NavigationTimeSmoother()
: mPrevPosition() // init empty position
, mNextPosition() // init empty position
{
}
/**
* Returns position smoothed in time (due to this navigation is updated every time wsMessage is received).
* Keeps last two positions and calculates intermidiate position between them based on how much time passed since
* last known position was calculated
*/
Position NavigationTimeSmoother::getTimeSmoothedPosition(const Position& lastKnownPosition, long long currentTs)
{
if (currentTs < lastKnownPosition.ts)
return lastKnownPosition;
if (mPrevPosition.levelId != lastKnownPosition.levelId || mNextPosition.levelId != lastKnownPosition.levelId)
{
mPrevPosition = lastKnownPosition;
mNextPosition = lastKnownPosition;
return lastKnownPosition;
}
if (mPrevPosition.isEmpty || mNextPosition.isEmpty || lastKnownPosition.isEmpty)
{
mPrevPosition = mNextPosition;
mNextPosition = lastKnownPosition;
return lastKnownPosition;
}
if (lastKnownPosition.ts > mNextPosition.ts)
{
mPrevPosition = mNextPosition;
mNextPosition = lastKnownPosition;
}
if (mNextPosition.ts == mPrevPosition.ts)
return mNextPosition;
double actualTsDiff = double(currentTs - mNextPosition.ts);
double keptTsDiff = double(mNextPosition.ts - mPrevPosition.ts);
if (actualTsDiff > keptTsDiff) // we don't use extrapolation
return lastKnownPosition;
double timeSmoothedX = (mNextPosition.x * actualTsDiff + mPrevPosition.x * (keptTsDiff - actualTsDiff)) / keptTsDiff;
double timeSmoothedY = (mNextPosition.y * actualTsDiff + mPrevPosition.y * (keptTsDiff - actualTsDiff)) / keptTsDiff;
Position timeSmoothedPos = mNextPosition;
timeSmoothedPos.x = timeSmoothedX;
timeSmoothedPos.y = timeSmoothedY;
timeSmoothedPos.ts = currentTs;
return timeSmoothedPos;
}
} } // namespace navigine
================================================
FILE: src/position_postprocessor/navigation_time_smoother.h
================================================
/** navigation_time_smoother.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef NAVIGINE_NAVIGATION_TIME_SMOOTHER_H
#define NAVIGINE_NAVIGATION_TIME_SMOOTHER_H
#include "../position.h"
namespace navigine {
namespace navigation_core {
class NavigationTimeSmoother
{
public:
NavigationTimeSmoother();
Position getTimeSmoothedPosition(const Position& lastKnownPosition, long long currentTs);
private:
Position mPrevPosition;
Position mNextPosition;
};
} } // namespace navigine::navigation_core
#endif // NAVIGINE_NAVIGATION_TIME_SMOOTHER_H
================================================
FILE: src/position_postprocessor/polynomial_fit.cpp
================================================
/** polynomial_fit.h
*
* Copyright (c) 2018 Navigine.
*
*/
#include "polynomial_fit.h"
namespace navigine {
namespace navigation_core {
PolynomialFit::PolynomialFit(int power, double timeInterval, double timeShift, double newWeight)
: mPower(power)
, mTimeInterval(timeInterval)
, mTimeShift(timeShift)
, mCoeff(mPower + 1, 0.)
, mNewSolutionWeight(newWeight)
, mLastOutput(0)
{
}
void PolynomialFit::addSequencePoint(double x, double y)
{
if (mY.empty())
mCoeff.assign(mPower + 1, 0.);
if (mY.empty() || mY.back() != y)
{
mY.push_back(y);
mX.push_back(x);
removeOldPoints();
if ((int)mY.size() > mPower)
getLeastSquaresCoeff();
}
}
void PolynomialFit::removeOldPoints()
{
while ((mX.back() - mX.front()) > mTimeInterval)
{
mX.pop_front();
mY.pop_front();
}
}
void PolynomialFit::clear()
{
mX.clear();
mY.clear();
}
double PolynomialFit::predict(double x)
{
double out = 0;
double slnCoef = mNewSolutionWeight;
int size = (int)mY.size();
if (size == 0)
{
return -1.0;
}
else if (size == 1 || size < mPower + 1)
{
out = getLagrangeSolution(x);
}
else
{
out = getLeastSquaresSolution(x);
out = out * slnCoef + mLastOutput * (1.0 - slnCoef);
}
mLastOutput = out;
return out;
}
double PolynomialFit::getLagrangeSolution(double curTime)
{
size_t pwr = mY.size();
double out = 0.0;
double mult = 1.0;
double time = curTime - mTimeShift;
for (size_t i = 0; i < pwr; i++)
{
mult = 1.0;
for (size_t j = 0; j < pwr; j++)
{
if (i != j)
mult *= (time - mX[j]) / (mX[i] - mX[j]);
}
out += mY[i] * mult;
}
return out;
}
double PolynomialFit::getLeastSquaresSolution (double curTime)
{
double out = 0.0;
double time = curTime - mTimeShift;
for (int i = 0; i <= mPower; i++)
{
out += mCoeff[i] * pow(time, i);
}
return out;
}
void PolynomialFit::getLeastSquaresCoeff()
{
std::vector <double> A ( (mPower + 1)*(mPower + 1), 0.);
std::vector <double> b ( mPower + 1, 0.);
std::vector <double> sub(2*mPower + 1, 0.);
double sum;
// Initialize sub
for (int i = 0; i < 2 * mPower + 1; i++)
{
sum = 0;
for (size_t j = 0; j < mY.size(); j++)
{
sum += pow(mX[j], i);
}
sub[i] = sum;
}
// Initialize matrix 'A'
for (int i = 0; i < mPower + 1; i++)
for (int j = 0; j < mPower + 1; j++)
A[i + j*(mPower + 1)] = sub[i + j];
// Initialize vector 'b'
for (int i = 0; i < mPower + 1; i++)
{
sum = 0;
for (size_t j = 0; j < mY.size(); j++)
{
sum += pow(mX[j], i) * mY[j];
}
b[i] = sum;
}
solveLinearSystem(A, b, mCoeff, mPower + 1);
return;
}
void solveLinearSystem(const std::vector <double>& A, const std::vector <double>& b,
std::vector <double>& x, const int n)
{
// Rotations method
int i, j, k;
double cosphi, sinphi, norm, temp;
std::vector <double> E(n*(n + 1));
// E initialization. E is a [n x (n+1)] matrix formed as
// E = [A,b] to simplify the calculations
for (int i = 0; i < n; i++)
{
E[i*(n + 1) + n] = b[i];
for (int j = 0; j < n; j++)
E[i*(n + 1) + j] = A[i*n + j];
}
// Forward Gaussian
for (k = 0; k < n; ++k)
for (j = k + 1; j < n; ++j)
{
cosphi = E[k * (n + 1) + k];
sinphi = E[j * (n + 1) + k];
norm = sqrt(cosphi * cosphi + sinphi * sinphi);
cosphi /= norm;
sinphi /= norm;
for (i = k; i < n + 1; ++i)
{
temp = cosphi * E[j * (n + 1) + i] - sinphi * E[k * (n + 1) + i];
E[k * (n + 1) + i] = sinphi * E[j * (n + 1) + i] + cosphi * E[k * (n + 1) + i];
E[j * (n + 1) + i] = temp;
}
}
// Back-Gaussing
for (k = n - 1; k >= 0; --k)
{
x[k] = E[k*(n + 1) + n];
for (i = k + 1; i < n; ++i)
x[k] -= E[k*(n + 1) + i] * x[i];
x[k] /= E[k*(n + 1) + k];
}
return;
}
} } // namespace navigine::navigation_core
================================================
FILE: src/position_postprocessor/polynomial_fit.h
================================================
/** polynomial_fit.h
*
* Copyright (c) 2018 Navigine.
*
*/
#ifndef POLYNOMIAL_FIT_H
#define POLYNOMIAL_FIT_H
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <deque>
#include <vector>
namespace navigine {
namespace navigation_core {
void solveLinearSystem(const std::vector <double>& A, const std::vector <double>& b,
std::vector <double>& x, const int n);
class PolynomialFit
{
public:
PolynomialFit(int power = 1, double timeInterval = 2.0, double timeShift = 0.7, double newWeight = 0.2);
void addSequencePoint(double x, double y);
double predict(double x);
void clear();
private:
const int mPower;
const double mTimeInterval;
const double mTimeShift;
const double mNewSolutionWeight;
double mLastOutput;
std::deque<double> mY;
std::deque<double> mX;
std::vector<double> mCoeff;
void removeOldPoints();
void getLeastSquaresCoeff();
double getLagrangeSolution(const double x);
double getLeastSquaresSolution(const double x);
};
} } // namespace navigine::navigation_core
#endif
================================================
FILE: src/position_postprocessor/position_postprocessor.cpp
================================================
#include <navigine/navigation-core/navigation_settings.h>
#include "../geometry.h"
#include "position_postprocessor.h"
#include "position_smoother_ab.h"
#include "position_smoother_lstsq.h"
namespace navigine {
namespace navigation_core {
PositionPostprocessor::PositionPostprocessor(const NavigationSettings& navProps)
: mUseSmoothing(navProps.commonSettings.useSmoothing)
, mUseStops(navProps.commonSettings.useStops)
, mUseInstantGpsPosition(navProps.commonSettings.useInstantGpsPosition)
, mUseGps(navProps.commonSettings.useGps)
, mFuseGps(navProps.commonSettings.fuseGps)
, mUseGpsOutsideMap(navProps.commonSettings.useGpsOutsideMap)
, mPreferIndoorSolution(navProps.commonSettings.preferIndoorSolution)
, mUseTimeSmoothing(navProps.commonSettings.useTimeSmoothing)
, mUseGraphProjection(navProps.commonSettings.useGraphProjection)
, mDeadReckonTimeMs((long long)(1000 * navProps.commonSettings.deadReckoningTime))
, mStopUpdateTimeMs((long long)(1000 * navProps.commonSettings.stopUpdateTime))
, mStopDetectionTimeMs((long long)(1000 * navProps.commonSettings.stopDetectionTime))
, mGraphProjectionDistance(navProps.commonSettings.graphProjDist)
, mPriorDev(navProps.commonSettings.priorDeviation)
, mFuseGpsBorderM(navProps.commonSettings.fuseGpsBorderM)
, mMotionSpeed(navProps.commonSettings.averageMovSpeed)
, mPosIsTooOldForFusingSec(navProps.commonSettings.positionIsTooOldSec)
, mStopsDistThresholdM(navProps.commonSettings.useStopsDistanceThresholdM)
, mIndoorDev(navProps.commonSettings.priorDeviation)
, mOutdoorDev(navProps.commonSettings.priorDeviation)
, mNavigationTimeSmoother()
, mIndoorPos()
, mOutdoorPos()
, mLastOutPosition()
{
mLastExtractionTs = 0;
bool useAbFilter = navProps.commonSettings.useAbFilter;
if (useAbFilter)
mPositionSmoother = new PositionSmootherAB(navProps);
else
mPositionSmoother = new PositionSmootherLstsq(navProps);
}
PositionPostprocessor::~PositionPostprocessor()
{
if (mPositionSmoother)
delete mPositionSmoother;
}
void PositionPostprocessor::fillIndoorOutdoorPositionStates(
const Position& indoorPos,
const Position& outdoorPos)
{
if (!indoorPos.isEmpty)
{
double priorIndoorDev = mIndoorPos.isEmpty ? mPriorDev : std::min(mPriorDev, mIndoorDev + mMotionSpeed * (indoorPos.ts - mIndoorPos.ts) / 1000.0);
mIndoorPos = indoorPos;
mIndoorDev = (priorIndoorDev + indoorPos.deviationM) / 2.0;
}
if (!outdoorPos.isEmpty)
{
double priorOutdoorDev = mOutdoorPos.isEmpty ? mPriorDev : std::min(mPriorDev, mOutdoorDev + mMotionSpeed * (outdoorPos.ts - mOutdoorPos.ts) / 1000.0);
mOutdoorDev = (priorOutdoorDev + outdoorPos.deviationM) / 2.0;
mOutdoorPos = outdoorPos;
}
}
Position PositionPostprocessor::fusePositions(
long long curTs,
const Position& indoorPos,
const Position& outdoorPos,
const NavigationStatus& navStatus)
{
fillIndoorOutdoorPositionStates(indoorPos, outdoorPos);
// return instant gps position for debugging:
if (mUseInstantGpsPosition && !outdoorPos.isEmpty)
return outdoorPos;
if (mPreferIndoorSolution && !indoorPos.isEmpty and navStatus==0)
return indoorPos;
if (!mUseGps || mOutdoorPos.isEmpty)
return mIndoorPos;
if (mIndoorPos.isEmpty)
return mOutdoorPos;
double indoorPositionDelay = curTs >= mIndoorPos.ts ? (curTs - mIndoorPos.ts) / 1000.0 : 0.0;
double outdoorPositionDelay = curTs >= mOutdoorPos.ts ? (curTs - mOutdoorPos.ts) / 1000.0 : 0.0;
if ((indoorPositionDelay) > mPosIsTooOldForFusingSec)
return mOutdoorPos;
if ((outdoorPositionDelay) > mPosIsTooOldForFusingSec)
return mIndoorPos;
double indoorDev = mMotionSpeed * indoorPositionDelay + mIndoorDev;
double outdoorDev = mMotionSpeed * outdoorPositionDelay + mOutdoorDev;
double positionsDist = GetDist(mIndoorPos.x, mIndoorPos.y, mOutdoorPos.x, mOutdoorPos.y);
double accuracyDist = mIndoorPos.deviationM + mOutdoorPos.deviationM + mFuseGpsBorderM;
if (mFuseGps && (accuracyDist > positionsDist))
{
Position fusedPos = mIndoorPos;
fusedPos.ts = std::max(mIndoorPos.ts, mOutdoorPos.ts);
fusedPos.x = (mIndoorPos.x * outdoorDev + mOutdoorPos.x * indoorDev) / (outdoorDev + indoorDev);
fusedPos.y = (mIndoorPos.y * outdoorDev + mOutdoorPos.y * indoorDev) / (outdoorDev + indoorDev);
fusedPos.deviationM = std::min(indoorDev, outdoorDev);
fusedPos.provider = Provider::FUSED;
return fusedPos;
}
else
return indoorDev < outdoorDev ? mIndoorPos : mOutdoorPos;
}
LocationPoint getProjection(const Graph<XYPoint>& graph, const LocationPoint& P)
{
LocationPoint P0 = P;
double d0 = NAN;
std::pair<int, int> e0;
for (auto edgePair = graph.edgesBegin(); edgePair != graph.edgesEnd(); ++edgePair)
{
const Graph<XYPoint>::Vertex& u = graph.getVertex((*edgePair).first);
const Graph<XYPoint>::Vertex& v = graph.getVertex((*edgePair).second);
LocationPoint P1 = P;
double k = GetProjection(u.point.x, u.point.y, v.point.x, v.point.y, P.x, P.y, &P1.x, &P1.y);
if (k < 0)
P1 = LocationPoint{P.level, u.point.x, u.point.y};
else if (k > 1)
P1 = LocationPoint{P.level, v.point.x, v.point.y};
double d = GetDist(P, P1);
if (std::isnan(d0) || d < d0)
{
gitextract_n3y_rmqw/
├── .gitattributes
├── .github/
│ ├── ISSUE_TEMPLATE/
│ │ ├── bug_report.md
│ │ ├── custom.md
│ │ └── feature_request.md
│ └── workflows/
│ ├── macos.yml
│ └── ubuntu.yml
├── .gitignore
├── CMakeLists.txt
├── CODE_OF_CONDUCT.md
├── CONTRIBUTING.md
├── LICENSE
├── README.md
├── include/
│ └── navigine/
│ └── navigation-core/
│ ├── CHANGELOG.md
│ ├── README.md
│ ├── barriers_geometry_builder.h
│ ├── boost_geometry_adaptation.h
│ ├── declare_identifier.h
│ ├── geolevel.h
│ ├── graph.h
│ ├── graph_particle.h
│ ├── level.h
│ ├── level_collector.h
│ ├── level_geometry.h
│ ├── location_point.h
│ ├── logger.h
│ ├── motion_info.h
│ ├── navigation_client.h
│ ├── navigation_input.h
│ ├── navigation_output.h
│ ├── navigation_settings.h
│ ├── navigation_state.h
│ ├── point.h
│ ├── radiomap.h
│ ├── reference_point.h
│ ├── transmitter.h
│ ├── vector3d.h
│ ├── version.h
│ └── xy_particle.h
├── src/
│ ├── README
│ ├── barriers_geometry_builder.cpp
│ ├── device_properties.h
│ ├── geometry.cpp
│ ├── geometry.h
│ ├── knn/
│ │ ├── knn_utils.cpp
│ │ └── knn_utils.h
│ ├── level.cpp
│ ├── level_collector.cpp
│ ├── level_estimator/
│ │ ├── barometer.cpp
│ │ ├── barometer.h
│ │ ├── level_estimator.cpp
│ │ ├── level_estimator.h
│ │ ├── level_estimator_radiomap.cpp
│ │ ├── level_estimator_radiomap.h
│ │ ├── level_estimator_transmitters.cpp
│ │ ├── level_estimator_transmitters.h
│ │ ├── level_history.cpp
│ │ └── level_history.h
│ ├── level_geometry.cpp
│ ├── likelihood/
│ │ ├── likelihood.cpp
│ │ ├── likelihood.h
│ │ ├── likelihood_radiomap.cpp
│ │ └── likelihood_radiomap.h
│ ├── measurement_types.h
│ ├── measurements/
│ │ ├── measurement_preprocessor.cpp
│ │ └── measurement_preprocessor.h
│ ├── navigation_client_impl.cpp
│ ├── navigation_client_impl.h
│ ├── navigation_error_codes.h
│ ├── point.cpp
│ ├── position.h
│ ├── position_estimator/
│ │ ├── Readme.md
│ │ ├── docs/
│ │ │ └── KNN.md
│ │ ├── position_estimator.h
│ │ ├── position_estimator_knn.cpp
│ │ ├── position_estimator_knn.h
│ │ ├── position_estimator_outdoor.cpp
│ │ ├── position_estimator_outdoor.h
│ │ ├── position_estimator_zone.cpp
│ │ └── position_estimator_zone.h
│ ├── position_postprocessor/
│ │ ├── navigation_time_smoother.cpp
│ │ ├── navigation_time_smoother.h
│ │ ├── polynomial_fit.cpp
│ │ ├── polynomial_fit.h
│ │ ├── position_postprocessor.cpp
│ │ ├── position_postprocessor.h
│ │ ├── position_smoother.h
│ │ ├── position_smoother_ab.cpp
│ │ ├── position_smoother_ab.h
│ │ ├── position_smoother_lstsq.cpp
│ │ └── position_smoother_lstsq.h
│ ├── radiomap.cpp
│ ├── sensor_fusion/
│ │ ├── complementary_filter.cpp
│ │ ├── complementary_filter.h
│ │ ├── pedometer.cpp
│ │ ├── pedometer.h
│ │ ├── quaternion.cpp
│ │ ├── quaternion.h
│ │ ├── sensor_fusion.cpp
│ │ └── sensor_fusion.h
│ ├── triangulation.cpp
│ ├── triangulation.h
│ ├── trilateration.cpp
│ ├── trilateration.h
│ └── vector3d.cpp
├── standalone_algorithms/
│ ├── README.md
│ ├── System description.md
│ ├── complementary_filter/
│ │ ├── CMakeLists.txt
│ │ ├── README.md
│ │ ├── examples/
│ │ │ └── main.cpp
│ │ ├── helpers/
│ │ │ └── plot_angles.py
│ │ ├── include/
│ │ │ ├── complementary_filter.h
│ │ │ ├── quaternion.h
│ │ │ └── vector3d.h
│ │ ├── src/
│ │ │ ├── complementary_filter.cpp
│ │ │ ├── quaternion.cpp
│ │ │ └── vector3d.cpp
│ │ └── tests/
│ │ └── complementary_filter_test.cpp
│ ├── pedometer/
│ │ ├── CMakeLists.txt
│ │ ├── README.md
│ │ ├── examples/
│ │ │ └── main.cpp
│ │ ├── include/
│ │ │ ├── pedometer.h
│ │ │ └── vector3d.h
│ │ ├── src/
│ │ │ ├── pedometer.cpp
│ │ │ └── vector3d.cpp
│ │ └── tests/
│ │ └── pedometer_test.cpp
│ ├── position_estimation/
│ │ ├── CMakeLists.txt
│ │ ├── README.md
│ │ ├── examples/
│ │ │ └── main.cpp
│ │ ├── helpers/
│ │ │ └── plot_positions.py
│ │ ├── include/
│ │ │ ├── measurement_preprocessor.h
│ │ │ ├── navigation_structures.h
│ │ │ ├── nearest_transmitter_estimator.h
│ │ │ ├── position_smoother.h
│ │ │ └── transmitter.h
│ │ ├── logs/
│ │ │ └── transmitters.txt
│ │ └── src/
│ │ ├── measurement_preprocessor.cpp
│ │ ├── nearest_transmitter_estimator.cpp
│ │ └── position_smoother.cpp
│ └── trilateteration/
│ ├── CMakeLists.txt
│ ├── README.md
│ └── src/
│ ├── beacon.cpp
│ ├── beacon.h
│ ├── test_trilateration.cpp
│ ├── test_trilateration.h
│ ├── trilateration.cpp
│ └── trilateration.h
├── tests/
│ └── navigation_test.cpp
└── tools/
└── verification/
├── helpers.cpp
└── helpers.h
SYMBOL INDEX (337 symbols across 112 files)
FILE: include/navigine/navigation-core/barriers_geometry_builder.h
function namespace (line 12) | namespace navigine {
FILE: include/navigine/navigation-core/boost_geometry_adaptation.h
function namespace (line 14) | namespace navigine {
FILE: include/navigine/navigation-core/geolevel.h
function namespace (line 22) | namespace navigine {
FILE: include/navigine/navigation-core/graph.h
function namespace (line 14) | namespace navigine {
FILE: include/navigine/navigation-core/graph_particle.h
function namespace (line 12) | namespace navigine {
FILE: include/navigine/navigation-core/level_collector.h
function namespace (line 23) | namespace navigine {
FILE: include/navigine/navigation-core/level_geometry.h
function namespace (line 13) | namespace navigine {
FILE: include/navigine/navigation-core/location_point.h
function namespace (line 13) | namespace navigine {
FILE: include/navigine/navigation-core/logger.h
function namespace (line 18) | namespace navigine {
FILE: include/navigine/navigation-core/motion_info.h
function namespace (line 12) | namespace navigine {
FILE: include/navigine/navigation-core/navigation_client.h
function namespace (line 9) | namespace navigine {
FILE: include/navigine/navigation-core/navigation_input.h
function namespace (line 18) | namespace navigine {
FILE: include/navigine/navigation-core/navigation_output.h
function namespace (line 10) | namespace navigation_core {
FILE: include/navigine/navigation-core/navigation_settings.h
function namespace (line 12) | namespace navigation_core {
FILE: include/navigine/navigation-core/navigation_state.h
function namespace (line 15) | namespace navigine {
FILE: include/navigine/navigation-core/radiomap.h
function namespace (line 15) | namespace navigine {
FILE: include/navigine/navigation-core/reference_point.h
function namespace (line 15) | namespace navigine {
FILE: include/navigine/navigation-core/transmitter.h
function namespace (line 9) | namespace navigine {
FILE: include/navigine/navigation-core/vector3d.h
function namespace (line 12) | namespace navigine {
FILE: include/navigine/navigation-core/xy_particle.h
function namespace (line 12) | namespace navigine {
FILE: src/barriers_geometry_builder.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function LevelGeometry (line 13) | LevelGeometry getGeometry(const std::list<Polygon>& allowedArea)
FILE: src/device_properties.h
function namespace (line 6) | namespace navigine {
FILE: src/geometry.cpp
type navigine (line 4) | namespace navigine {
type navigation_core (line 5) | namespace navigation_core {
function GetDist (line 8) | double GetDist(double ax, double ay, double bx, double by)
function GetDist (line 14) | double GetDist(const LocationPoint& A, const LocationPoint& B)
function SegmentPointDist (line 20) | double SegmentPointDist(const XYPoint& p1, const XYPoint& p2, const ...
function GetDist (line 44) | double GetDist(const LocationPoint& A, const LocationPoint& B, const...
function GetArea (line 54) | extern double GetArea(const LocationPoint& A,
function GetArea (line 64) | extern double GetArea(double ax, double ay,
function Determinant (line 74) | double Determinant(double a11, double a12,
function Determinant (line 84) | double Determinant(double a11, double a12, double a13,
function GetProjection (line 93) | double GetProjection(double ax, double ay,
function GetIntersection (line 110) | double GetIntersection(double px, double py,
function CheckIntersection (line 139) | bool CheckIntersection(double ax, double ay,
function PointOnLine (line 156) | bool PointOnLine(double x, double y, double x1, double y1, double x2...
function XRayIntersectsLine (line 167) | bool XRayIntersectsLine(double x, double y, double x1, double y1, do...
FILE: src/geometry.h
function namespace (line 12) | namespace navigine {
FILE: src/knn/knn_utils.cpp
type navigine (line 13) | namespace navigine {
type navigation_core (line 14) | namespace navigation_core {
function triangulate (line 29) | std::map<LevelId, std::vector<RefPointsTriangle>> triangulate(const ...
function getLevelReferenceTransmittersMap (line 118) | std::map<LevelId, std::set<TransmitterId>> getLevelReferenceTransmit...
function calculateTransmitterTrustworthiness (line 158) | static double calculateTransmitterTrustworthiness(const RadioMeasure...
function calcTransmittersWeights (line 171) | static std::map<TransmitterId, double> calcTransmittersWeights(
function calcRpWeightsKnn (line 201) | std::map<ReferencePointId, double> calcRpWeightsKnn(
function getTriangleWeight (line 299) | static double getTriangleWeight(const RefPointsTriangle& triangle,
function XYPoint (line 310) | XYPoint calcPositionInTriangle(const std::vector<RefPointsTriangle>&...
function XYPoint (line 352) | XYPoint calcKHeaviestRefPointsAverage(const Radiomap& radiomap,
function RadioMeasurementsData (line 393) | RadioMeasurementsData getWhitelistedMeasurements(
FILE: src/knn/knn_utils.h
function namespace (line 19) | namespace navigine {
FILE: src/level.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function GeoPoint (line 12) | const GeoPoint& Level::bindingPoint() const
function LevelId (line 17) | const LevelId& Level::id() const
function Radiomap (line 22) | const Radiomap &Level::radiomap() const
function LevelGeometry (line 32) | const LevelGeometry& Level::geometry() const
function RadioMeasurementsData (line 69) | RadioMeasurementsData getLevelRadioMeasurements(
FILE: src/level_collector.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function XYZTransmitters (line 14) | XYZTransmitters getTransmittersInLocalCoordinates(
function XYReferencePoints (line 32) | XYReferencePoints getRefPointsInLocalCoordinates(
function Polygon (line 47) | Polygon getPolygonInLocalCoordinates(
function Box (line 70) | Box getBBoxInLocalCoordinates(const GeoPoint& binding, const Box& ge...
function LevelGeometry (line 77) | LevelGeometry getGeometryInLocalCoordinates(
function getGraphInLocalCoordinates (line 94) | Graph<XYPoint> getGraphInLocalCoordinates(
function createLevelCollector (line 121) | std::shared_ptr<LevelCollector> createLevelCollector()
function Level (line 149) | const Level& LevelCollector::level(const LevelId& levelId) const
function GeoPoint (line 154) | GeoPoint getBinding(const GeoLevel& geoLevel)
FILE: src/level_estimator/barometer.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function calcMean (line 27) | static double calcMean(const std::deque<double>& measurements)
function LevelId (line 42) | LevelId Barometer::checkLevelChange(
FILE: src/level_estimator/barometer.h
function namespace (line 17) | namespace navigine {
FILE: src/level_estimator/level_estimator.cpp
type navigine (line 11) | namespace navigine {
type navigation_core (line 12) | namespace navigation_core {
function LevelId (line 23) | LevelId LevelEstimator::calculateLevel(
FILE: src/level_estimator/level_estimator.h
function namespace (line 19) | namespace navigine {
FILE: src/level_estimator/level_estimator_radiomap.cpp
type navigine (line 13) | namespace navigine {
type navigation_core (line 14) | namespace navigation_core {
function LevelId (line 26) | LevelId LevelEstimatorRadiomap::detectCurrentLevel(const RadioMeasur...
FILE: src/level_estimator/level_estimator_radiomap.h
function namespace (line 12) | namespace navigine {
FILE: src/level_estimator/level_estimator_transmitters.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function getAverageRssi (line 21) | static double getAverageRssi(const RadioMeasurementsData& radioMsr)
function getAverageA (line 30) | static double getAverageA(const Level& level, const RadioMeasurement...
function LevelId (line 43) | LevelId LevelEstimatorTransmitters::detectCurrentLevel(const RadioMe...
FILE: src/level_estimator/level_estimator_transmitters.h
function namespace (line 12) | namespace navigine {
FILE: src/level_estimator/level_history.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function LevelId (line 21) | LevelId LevelHistory::detectLevelUsingHistory(const LevelId& detecte...
FILE: src/level_estimator/level_history.h
function namespace (line 14) | namespace navigine {
FILE: src/level_geometry.cpp
type navigine (line 3) | namespace navigine {
type navigation_core (line 4) | namespace navigation_core {
function Multipolygon (line 46) | const Multipolygon& LevelGeometry::allowedArea() const
function Box (line 51) | const Box& LevelGeometry::boundingBox() const
FILE: src/likelihood/likelihood.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function isParticleInIntersectionArea (line 12) | bool isParticleInIntersectionArea(
FILE: src/likelihood/likelihood.h
function namespace (line 12) | namespace navigation_core {
FILE: src/likelihood/likelihood_radiomap.cpp
type navigine (line 4) | namespace navigine {
type navigation_core (line 5) | namespace navigation_core {
function calcParticleWeight (line 48) | double calcParticleWeight(
function RadioMeasurementsData (line 175) | RadioMeasurementsData LikelihoodRadiomap::getOnlyCorrectlyIntersecte...
FILE: src/likelihood/likelihood_radiomap.h
function namespace (line 10) | namespace navigine {
FILE: src/measurement_types.h
function namespace (line 5) | namespace navigine {
FILE: src/measurements/measurement_preprocessor.cpp
type navigine (line 16) | namespace navigine {
type navigation_core (line 17) | namespace navigation_core {
function checkUseWifi (line 35) | bool checkUseWifi(const NavigationSettings& navProps) {
function checkUseBle (line 45) | bool checkUseBle(const NavigationSettings& navProps) {
function RadioMeasurementBuffer (line 55) | RadioMeasurementBuffer createRadioMeasurementBuffer(
function RadioMeasurementsData (line 124) | RadioMeasurementsData MeasurementsPreprocessor::extractRadioMeasurem...
function RadioMeasurementsData (line 252) | RadioMeasurementsData RadioMeasurementBuffer::extractMeasurements(lo...
function SensorMeasurement (line 340) | SensorMeasurement MeasurementsPreprocessor::getValidSensor() const
function NmeaMeasurement (line 345) | NmeaMeasurement MeasurementsPreprocessor::getCurrentNmea() const
function clearDebugOutput (line 351) | void clearDebugOutput()
function printInputSignals (line 360) | void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsD...
function printExtractedMeasurements (line 373) | void printExtractedMeasurements(RadioMeasurementsData& radioMeasurem...
type navigation_core (line 28) | namespace navigation_core {
function checkUseWifi (line 35) | bool checkUseWifi(const NavigationSettings& navProps) {
function checkUseBle (line 45) | bool checkUseBle(const NavigationSettings& navProps) {
function RadioMeasurementBuffer (line 55) | RadioMeasurementBuffer createRadioMeasurementBuffer(
function RadioMeasurementsData (line 124) | RadioMeasurementsData MeasurementsPreprocessor::extractRadioMeasurem...
function RadioMeasurementsData (line 252) | RadioMeasurementsData RadioMeasurementBuffer::extractMeasurements(lo...
function SensorMeasurement (line 340) | SensorMeasurement MeasurementsPreprocessor::getValidSensor() const
function NmeaMeasurement (line 345) | NmeaMeasurement MeasurementsPreprocessor::getCurrentNmea() const
function clearDebugOutput (line 351) | void clearDebugOutput()
function printInputSignals (line 360) | void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsD...
function printExtractedMeasurements (line 373) | void printExtractedMeasurements(RadioMeasurementsData& radioMeasurem...
type navigine (line 27) | namespace navigine {
type navigation_core (line 17) | namespace navigation_core {
function checkUseWifi (line 35) | bool checkUseWifi(const NavigationSettings& navProps) {
function checkUseBle (line 45) | bool checkUseBle(const NavigationSettings& navProps) {
function RadioMeasurementBuffer (line 55) | RadioMeasurementBuffer createRadioMeasurementBuffer(
function RadioMeasurementsData (line 124) | RadioMeasurementsData MeasurementsPreprocessor::extractRadioMeasurem...
function RadioMeasurementsData (line 252) | RadioMeasurementsData RadioMeasurementBuffer::extractMeasurements(lo...
function SensorMeasurement (line 340) | SensorMeasurement MeasurementsPreprocessor::getValidSensor() const
function NmeaMeasurement (line 345) | NmeaMeasurement MeasurementsPreprocessor::getCurrentNmea() const
function clearDebugOutput (line 351) | void clearDebugOutput()
function printInputSignals (line 360) | void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsD...
function printExtractedMeasurements (line 373) | void printExtractedMeasurements(RadioMeasurementsData& radioMeasurem...
type navigation_core (line 28) | namespace navigation_core {
function checkUseWifi (line 35) | bool checkUseWifi(const NavigationSettings& navProps) {
function checkUseBle (line 45) | bool checkUseBle(const NavigationSettings& navProps) {
function RadioMeasurementBuffer (line 55) | RadioMeasurementBuffer createRadioMeasurementBuffer(
function RadioMeasurementsData (line 124) | RadioMeasurementsData MeasurementsPreprocessor::extractRadioMeasurem...
function RadioMeasurementsData (line 252) | RadioMeasurementsData RadioMeasurementBuffer::extractMeasurements(lo...
function SensorMeasurement (line 340) | SensorMeasurement MeasurementsPreprocessor::getValidSensor() const
function NmeaMeasurement (line 345) | NmeaMeasurement MeasurementsPreprocessor::getCurrentNmea() const
function clearDebugOutput (line 351) | void clearDebugOutput()
function printInputSignals (line 360) | void printInputSignals(NavigationMessage* navMsg, RadioMeasurementsD...
function printExtractedMeasurements (line 373) | void printExtractedMeasurements(RadioMeasurementsData& radioMeasurem...
FILE: src/measurements/measurement_preprocessor.h
function namespace (line 16) | namespace navigine {
FILE: src/navigation_client_impl.cpp
type navigine (line 11) | namespace navigine {
type navigation_core (line 12) | namespace navigation_core {
function createNavigationClient (line 209) | std::shared_ptr<NavigationClient> createNavigationClient(
FILE: src/navigation_client_impl.h
function namespace (line 17) | namespace navigine {
FILE: src/navigation_error_codes.h
function namespace (line 10) | namespace navigine {
FILE: src/point.cpp
type navigine (line 10) | namespace navigine {
type navigation_core (line 11) | namespace navigation_core {
function normalizeLongitude (line 17) | double normalizeLongitude(double lon)
function degToRad (line 29) | double degToRad(double deg)
function XYPoint (line 34) | XYPoint gpsToLocal(const GeoPoint& point, const GeoPoint& bindPoint)
function GeoPoint (line 47) | GeoPoint localToGps(const XYPoint& localPoint, const GeoPoint& bindP...
FILE: src/position.h
function namespace (line 15) | namespace navigine {
FILE: src/position_estimator/position_estimator.h
function namespace (line 13) | namespace navigine {
FILE: src/position_estimator/position_estimator_knn.cpp
type navigine (line 14) | namespace navigine {
type navigation_core (line 15) | namespace navigation_core {
function Position (line 36) | Position PositionEstimatorKnn::calculatePosition(
FILE: src/position_estimator/position_estimator_knn.h
function namespace (line 15) | namespace navigine {
FILE: src/position_estimator/position_estimator_outdoor.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function GpsPosition (line 64) | GpsPosition PositionEstimatorOutdoor::extractGpsPosition(long long t...
function Position (line 90) | Position PositionEstimatorOutdoor::calculatePosition(
FILE: src/position_estimator/position_estimator_outdoor.h
function namespace (line 19) | namespace navigine {
FILE: src/position_estimator/position_estimator_zone.cpp
type navigine (line 10) | namespace navigine {
type navigation_core (line 11) | namespace navigation_core {
function Position (line 26) | Position PositionEstimatorZone::calculatePosition(
FILE: src/position_estimator/position_estimator_zone.h
function namespace (line 12) | namespace navigine {
FILE: src/position_postprocessor/navigation_time_smoother.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function Position (line 23) | Position NavigationTimeSmoother::getTimeSmoothedPosition(const Posit...
FILE: src/position_postprocessor/navigation_time_smoother.h
function namespace (line 12) | namespace navigine {
FILE: src/position_postprocessor/polynomial_fit.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function solveLinearSystem (line 146) | void solveLinearSystem(const std::vector <double>& A, const std::vec...
FILE: src/position_postprocessor/polynomial_fit.h
function namespace (line 16) | namespace navigine {
FILE: src/position_postprocessor/position_postprocessor.cpp
type navigine (line 7) | namespace navigine {
type navigation_core (line 8) | namespace navigation_core {
function Position (line 71) | Position PositionPostprocessor::fusePositions(
function LocationPoint (line 121) | LocationPoint getProjection(const Graph<XYPoint>& graph, const Locat...
function Position (line 159) | Position PositionPostprocessor::getProcessedPosition(const Position&...
FILE: src/position_postprocessor/position_postprocessor.h
function namespace (line 12) | namespace navigine {
FILE: src/position_postprocessor/position_smoother.h
function namespace (line 8) | namespace navigine {
FILE: src/position_postprocessor/position_smoother_ab.cpp
type navigine (line 4) | namespace navigine {
type navigation_core (line 5) | namespace navigation_core {
function Position (line 25) | Position PositionSmootherAB::smoothPosition(Position pos, const Leve...
FILE: src/position_postprocessor/position_smoother_ab.h
function namespace (line 6) | namespace navigine {
FILE: src/position_postprocessor/position_smoother_lstsq.cpp
type navigine (line 4) | namespace navigine {
type navigation_core (line 5) | namespace navigation_core {
function Position (line 18) | Position PositionSmootherLstsq::smoothPosition(Position pos, const L...
FILE: src/position_postprocessor/position_smoother_lstsq.h
function namespace (line 7) | namespace navigine {
FILE: src/radiomap.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function XYReferencePoints (line 19) | const XYReferencePoints& Radiomap::referencePoints() const
FILE: src/sensor_fusion/complementary_filter.cpp
type navigine (line 11) | namespace navigine {
type navigation_core (line 12) | namespace navigation_core {
function to_minus_Pi_Pi (line 18) | double to_minus_Pi_Pi(double x)
function Quaternion (line 25) | Quaternion updateQuaternion(const Quaternion& q, const Vector3d& v)
function SensorMeasurement (line 32) | SensorMeasurement lowPassFilter(const SensorMeasurement& prevMsr, co...
function Orientation (line 45) | Orientation ComplementaryFilter::getFusedOrientation() const
FILE: src/sensor_fusion/complementary_filter.h
function namespace (line 18) | namespace navigine {
FILE: src/sensor_fusion/pedometer.cpp
type navigine (line 16) | namespace navigine {
type navigation_core (line 17) | namespace navigation_core {
function Magnitude (line 77) | Magnitude Pedometer::calculateFilteredAccMagnitudes() const
function clearPedometerDebugOutput (line 188) | void clearPedometerDebugOutput()
function printDebugPedometer (line 197) | void printDebugPedometer(const std::string &type, long long ts, doub...
FILE: src/sensor_fusion/pedometer.h
type Magnitude (line 27) | struct Magnitude
FILE: src/sensor_fusion/quaternion.cpp
type navigine (line 10) | namespace navigine {
type navigation_core (line 11) | namespace navigation_core {
function Quaternion (line 25) | Quaternion Quaternion::normalized() const
function Quaternion (line 54) | Quaternion& Quaternion::operator+=(const Quaternion& r_q)
function Quaternion (line 63) | Quaternion& Quaternion::operator-=(const Quaternion& r_q)
function Quaternion (line 72) | Quaternion& Quaternion::operator*=(const double& scale)
function Quaternion (line 81) | Quaternion& Quaternion::operator*=(const Quaternion& other)
function Quaternion (line 91) | Quaternion operator+(Quaternion l_q, const Quaternion& r_q)
function Quaternion (line 96) | Quaternion operator-(Quaternion l_q, const Quaternion& r_q)
function Quaternion (line 101) | Quaternion operator*(Quaternion l_q, const double& scale)
function Quaternion (line 106) | Quaternion operator*(const double& scale, Quaternion l_q)
function Quaternion (line 111) | Quaternion operator*(Quaternion l_q, const Quaternion& r_q)
FILE: src/sensor_fusion/quaternion.h
function namespace (line 17) | namespace navigine {
FILE: src/sensor_fusion/sensor_fusion.cpp
type navigine (line 6) | namespace navigine {
type navigation_core (line 7) | namespace navigation_core {
function MotionInfo (line 17) | MotionInfo SensorFusion::calculateDisplacement(const SensorMeasureme...
FILE: src/sensor_fusion/sensor_fusion.h
function namespace (line 19) | namespace navigine {
FILE: src/triangulation.cpp
type navigine (line 3) | namespace navigine {
type navigation_core (line 4) | namespace navigation_core {
function TriangulateVertices (line 8) | std::vector<Triangle> TriangulateVertices(const std::vector<Triangle...
FILE: src/triangulation.h
function namespace (line 11) | namespace navigine {
FILE: src/trilateration.cpp
type navigine (line 9) | namespace navigine {
type navigation_core (line 10) | namespace navigation_core {
function RadioMeasurementsData (line 12) | RadioMeasurementsData getIntersectedMeasurements(const Level &level,...
FILE: src/trilateration.h
function namespace (line 5) | namespace navigine {
FILE: src/vector3d.cpp
type navigine (line 10) | namespace navigine {
type navigation_core (line 11) | namespace navigation_core {
function Vector3d (line 29) | Vector3d Vector3d::normalized() const
function Vector3d (line 37) | Vector3d& Vector3d::normalize()
function Vector3d (line 63) | Vector3d& Vector3d::operator+=(const Vector3d& v)
function Vector3d (line 71) | Vector3d& Vector3d::operator-=(const Vector3d& v)
function Vector3d (line 79) | Vector3d& Vector3d::operator*=(double multiplier)
function Vector3d (line 87) | Vector3d& Vector3d::operator/=(double divisor)
function Vector3d (line 100) | Vector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2)
FILE: standalone_algorithms/complementary_filter/examples/main.cpp
function simulatePendulumMotion (line 23) | std::tuple<std::vector<double>,
function generateImuPendulum (line 62) | std::tuple<std::vector<SensorMeasurement>,
function main (line 124) | int main()
FILE: standalone_algorithms/complementary_filter/include/complementary_filter.h
function namespace (line 6) | namespace navigine {
FILE: standalone_algorithms/complementary_filter/include/quaternion.h
function namespace (line 9) | namespace navigine {
FILE: standalone_algorithms/complementary_filter/include/vector3d.h
function namespace (line 7) | namespace navigine {
FILE: standalone_algorithms/complementary_filter/src/complementary_filter.cpp
type navigine (line 3) | namespace navigine {
type navigation_core (line 4) | namespace navigation_core {
function to_minus_Pi_Pi (line 10) | double to_minus_Pi_Pi(double x)
function Quaternion (line 17) | Quaternion updateQuaternion(const Quaternion& q, const Vector3d& v)
function SensorMeasurement (line 24) | SensorMeasurement lowPassFilter(const SensorMeasurement& prevMsr, co...
function Orientation (line 38) | Orientation ComplementaryFilter::getFusedOrientation() const
FILE: standalone_algorithms/complementary_filter/src/quaternion.cpp
type navigine (line 3) | namespace navigine {
type navigation_core (line 4) | namespace navigation_core {
function Quaternion (line 18) | Quaternion Quaternion::normalized() const
function Quaternion (line 47) | Quaternion& Quaternion::operator+=(const Quaternion& r_q)
function Quaternion (line 56) | Quaternion& Quaternion::operator-=(const Quaternion& r_q)
function Quaternion (line 65) | Quaternion& Quaternion::operator*=(const double& scale)
function Quaternion (line 74) | Quaternion& Quaternion::operator*=(const Quaternion& other)
function Quaternion (line 84) | Quaternion operator+(Quaternion l_q, const Quaternion& r_q)
function Quaternion (line 89) | Quaternion operator-(Quaternion l_q, const Quaternion& r_q)
function Quaternion (line 94) | Quaternion operator*(Quaternion l_q, const double& scale)
function Quaternion (line 99) | Quaternion operator*(const double& scale, Quaternion l_q)
function Quaternion (line 104) | Quaternion operator*(Quaternion l_q, const Quaternion& r_q)
FILE: standalone_algorithms/complementary_filter/src/vector3d.cpp
type navigine (line 3) | namespace navigine {
type navigation_core (line 4) | namespace navigation_core {
function Vector3d (line 18) | Vector3d Vector3d::normalized() const
function Vector3d (line 25) | Vector3d& Vector3d::normalize()
function Vector3d (line 39) | Vector3d& Vector3d::operator+=(const Vector3d& v)
function Vector3d (line 47) | Vector3d& Vector3d::operator-=(const Vector3d& v)
function Vector3d (line 55) | Vector3d& Vector3d::operator*=(double multiplier)
function Vector3d (line 63) | Vector3d& Vector3d::operator/=(double divisor)
function Vector3d (line 76) | Vector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2)
FILE: standalone_algorithms/complementary_filter/tests/complementary_filter_test.cpp
function getTiltedImuMeasurements (line 17) | std::tuple<std::vector<SensorMeasurement>,
function BOOST_AUTO_TEST_CASE (line 84) | BOOST_AUTO_TEST_CASE(staticTilt)
FILE: standalone_algorithms/pedometer/examples/main.cpp
function parseMeasurements (line 8) | std::vector<SensorMeasurement> parseMeasurements(const std::string& logF...
function main (line 32) | int main(int argc, char *argv[])
FILE: standalone_algorithms/pedometer/include/pedometer.h
type SensorMeasurement (line 10) | struct SensorMeasurement
type Magnitude (line 24) | struct Magnitude
FILE: standalone_algorithms/pedometer/include/vector3d.h
function namespace (line 7) | namespace navigine {
FILE: standalone_algorithms/pedometer/src/pedometer.cpp
type navigine (line 5) | namespace navigine {
type navigation_core (line 6) | namespace navigation_core {
function Magnitude (line 47) | Magnitude Pedometer::calculateFilteredAccMagnitudes() const
FILE: standalone_algorithms/pedometer/src/vector3d.cpp
type navigine (line 3) | namespace navigine {
type navigation_core (line 4) | namespace navigation_core {
function Vector3d (line 18) | Vector3d Vector3d::normalized() const
function Vector3d (line 25) | Vector3d& Vector3d::normalize()
function Vector3d (line 39) | Vector3d& Vector3d::operator+=(const Vector3d& v)
function Vector3d (line 47) | Vector3d& Vector3d::operator-=(const Vector3d& v)
function Vector3d (line 55) | Vector3d& Vector3d::operator*=(double multiplier)
function Vector3d (line 63) | Vector3d& Vector3d::operator/=(double divisor)
function Vector3d (line 76) | Vector3d Vector3d::crossProduct(const Vector3d& v1, const Vector3d& v2)
FILE: standalone_algorithms/pedometer/tests/pedometer_test.cpp
function parseTestData (line 20) | std::pair<std::pair<int, double>, std::vector<SensorMeasurement>> parseT...
function getStepsNumLenPair (line 51) | std::pair<int, double> getStepsNumLenPair(const std::vector<SensorMeasur...
function BOOST_AUTO_TEST_CASE (line 71) | BOOST_AUTO_TEST_CASE(stepsHuaweiLong1)
function BOOST_AUTO_TEST_CASE (line 84) | BOOST_AUTO_TEST_CASE(stepsHuaweiLong2)
function BOOST_AUTO_TEST_CASE (line 97) | BOOST_AUTO_TEST_CASE(stepsIPhoneShort1)
function BOOST_AUTO_TEST_CASE (line 110) | BOOST_AUTO_TEST_CASE(stepsIPhoneShort2)
function BOOST_AUTO_TEST_CASE (line 123) | BOOST_AUTO_TEST_CASE(stepsIPhoneShort3)
function BOOST_AUTO_TEST_CASE (line 136) | BOOST_AUTO_TEST_CASE(stepsIPhoneShort4)
function BOOST_AUTO_TEST_CASE (line 149) | BOOST_AUTO_TEST_CASE(stepsNexusShort1)
function BOOST_AUTO_TEST_CASE (line 162) | BOOST_AUTO_TEST_CASE(stepsNexusShort2)
function BOOST_AUTO_TEST_CASE (line 175) | BOOST_AUTO_TEST_CASE(stepsNexusShort3)
function BOOST_AUTO_TEST_CASE (line 188) | BOOST_AUTO_TEST_CASE(stepsXiaomiLong1)
function BOOST_AUTO_TEST_CASE (line 201) | BOOST_AUTO_TEST_CASE(stepsXiaomiLong2)
function BOOST_AUTO_TEST_CASE (line 214) | BOOST_AUTO_TEST_CASE(stepsXiaomiLong3)
function BOOST_AUTO_TEST_CASE (line 227) | BOOST_AUTO_TEST_CASE(stepsXiaomiLong4)
FILE: standalone_algorithms/position_estimation/examples/main.cpp
type NavigationPoint (line 10) | struct NavigationPoint
method NavigationPoint (line 12) | NavigationPoint(){}
method NavigationPoint (line 14) | NavigationPoint(long long _timeMs, double _x, double _y, double _angle...
function getTransmitters (line 29) | std::vector<Transmitter> getTransmitters(const std::string& transmitters...
function getMeasurements (line 59) | std::vector<RadioMeasurement> getMeasurements(const std::string& msrFile...
function splitToPackets (line 90) | std::vector<RadioMeasurements> splitToPackets(const std::vector<RadioMea...
function main (line 105) | int main()
FILE: standalone_algorithms/position_estimation/include/measurement_preprocessor.h
function namespace (line 10) | namespace navigine {
FILE: standalone_algorithms/position_estimation/include/navigation_structures.h
type Position (line 23) | struct Position
type RadioMeasurement (line 32) | struct RadioMeasurement
type NavigationInput (line 46) | struct NavigationInput
FILE: standalone_algorithms/position_estimation/include/nearest_transmitter_estimator.h
function namespace (line 8) | namespace navigine {
FILE: standalone_algorithms/position_estimation/include/position_smoother.h
function namespace (line 5) | namespace navigine {
FILE: standalone_algorithms/position_estimation/include/transmitter.h
function namespace (line 6) | namespace navigine {
FILE: standalone_algorithms/position_estimation/src/measurement_preprocessor.cpp
type navigine (line 6) | namespace navigine {
type navigation_core (line 7) | namespace navigation_core {
FILE: standalone_algorithms/position_estimation/src/nearest_transmitter_estimator.cpp
type navigine (line 5) | namespace navigine {
type navigation_core (line 6) | namespace navigation_core {
function Position (line 28) | Position NearestTransmitterPositionEstimator::calculatePosition(cons...
FILE: standalone_algorithms/position_estimation/src/position_smoother.cpp
type navigine (line 3) | namespace navigine {
type navigation_core (line 4) | namespace navigation_core {
function Position (line 19) | Position PositionSmoother::smoothPosition(Position pos)
FILE: standalone_algorithms/trilateteration/src/beacon.cpp
function Beacon (line 272) | Beacon* BeaconMeas::getBeaconPtr() const
FILE: standalone_algorithms/trilateteration/src/test_trilateration.cpp
function PrintHelp (line 11) | void PrintHelp()
function main (line 21) | int main( void )
FILE: standalone_algorithms/trilateteration/src/trilateration.cpp
function compareBeaconMeasByName (line 410) | bool compareBeaconMeasByName( BeaconMeas first, BeaconMeas second )
function findBeaconForMeas (line 416) | std::vector<Beacon>::const_iterator findBeaconForMeas( const vector<Beac...
FILE: tests/navigation_test.cpp
function PrintUsage (line 23) | void PrintUsage(const char* program)
function BuildPdrTrack (line 30) | void BuildPdrTrack(const double pdrDistance, const double pdrHeading, do...
function main (line 42) | int main(int argc, char** argv)
FILE: tools/verification/helpers.cpp
type nlohmann (line 19) | namespace nlohmann {
function from_json (line 66) | static void from_json(const nlohmann::json& j, SensorMeasurementData& ...
function from_json (line 75) | static void from_json(const nlohmann::json& j, RadioMeasurementData& r...
function from_json (line 90) | static void from_json(const nlohmann::json& j, NmeaMeasurementData& nm...
function from_json (line 96) | static void from_json(const nlohmann::json& j, Measurement& msr)
function from_json (line 111) | static void from_json(const nlohmann::json& j, CommonSettings& commonS...
function from_json (line 289) | static void from_json(const nlohmann::json& j, LevelSettings& levelSet...
function GetNavMessages (line 316) | std::vector<Measurement> GetNavMessages(const std::string& jsonFile)
function getGeoDist (line 333) | double getGeoDist(const GeoPoint& p1, const GeoPoint& p2)
function GeoPoint (line 349) | GeoPoint removeSame(std::vector<GeoPoint>& allPoints)
function getVertex (line 369) | int getVertex(const GeoPoint& firstPoint, const std::unordered_map<int, ...
function parseGraph (line 382) | std::pair<LevelId, Graph<GeoPoint>> parseGraph(const nlohmann::json& j)
function parseTransmitter (line 460) | std::pair<LevelId, Transmitter<GeoPoint3D>> parseTransmitter(nlohmann::j...
function parseBarriers (line 490) | std::pair<LevelId, std::list<Polygon>> parseBarriers(const nlohmann::jso...
function parseReferencePoint (line 513) | std::pair<LevelId, ReferencePoint<GeoPoint>> parseReferencePoint(const n...
function parseLevel (line 568) | std::pair<LevelId, std::list<Polygon>> parseLevel(const nlohmann::json& j)
function GeoLevels (line 600) | GeoLevels ParseGeojson(const std::string& jsonFile, int& errorCode)
function NavigationSettings (line 698) | NavigationSettings CreateSettingsFromJson(
FILE: tools/verification/helpers.h
type NavigationPoint (line 10) | struct NavigationPoint
Condensed preview — 149 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (454K chars).
[
{
"path": ".gitattributes",
"chars": 483,
"preview": "# Auto detect text files and perform LF normalization\n* text=auto\n\n# Custom for Visual Studio\n*.cs diff=csharp\n*.sln"
},
{
"path": ".github/ISSUE_TEMPLATE/bug_report.md",
"chars": 834,
"preview": "---\nname: Bug report\nabout: Create a report to help us improve\ntitle: ''\nlabels: ''\nassignees: ''\n\n---\n\n**Describe the b"
},
{
"path": ".github/ISSUE_TEMPLATE/custom.md",
"chars": 126,
"preview": "---\nname: Custom issue template\nabout: Describe this issue template's purpose here.\ntitle: ''\nlabels: ''\nassignees: ''\n\n"
},
{
"path": ".github/ISSUE_TEMPLATE/feature_request.md",
"chars": 595,
"preview": "---\nname: Feature request\nabout: Suggest an idea for this project\ntitle: ''\nlabels: ''\nassignees: ''\n\n---\n\n**Is your fea"
},
{
"path": ".github/workflows/macos.yml",
"chars": 689,
"preview": "name: macOS\n\non:\n push:\n branches:\n - dev\n - master\n pull_request:\n\njobs:\n xcode:\n runs-on: macos-10."
},
{
"path": ".github/workflows/ubuntu.yml",
"chars": 1091,
"preview": "name: Ubuntu\n\non:\n push:\n branches:\n - dev\n - master\n pull_request:\n\njobs:\n ci_test_clang:\n runs-on: "
},
{
"path": ".gitignore",
"chars": 1151,
"preview": "# 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 s"
},
{
"path": "CMakeLists.txt",
"chars": 4592,
"preview": "cmake_minimum_required(VERSION 3.7)\nproject(Navigation-core)\n\ninclude(FetchContent)\n\nFetchContent_Declare(json\n GIT_REP"
},
{
"path": "CODE_OF_CONDUCT.md",
"chars": 3011,
"preview": "# <p align=\"center\"> Navigine Open Source Code of Conduct </p>\n\nThis code of conduct provides guidance on participation "
},
{
"path": "CONTRIBUTING.md",
"chars": 1103,
"preview": "# <p align=\"center\"> Contributing to Navigine Routing Library </p>\n\nThe easiest way to help out is to submit bug reports"
},
{
"path": "LICENSE",
"chars": 1077,
"preview": "MIT License\n\nCopyright (c) 2021 Navigine Corporation\n\nPermission is hereby granted, free of charge, to any person obtain"
},
{
"path": "README.md",
"chars": 7744,
"preview": "<a href=\"http://navigine.com\"><img src=\"https://navigine.com/assets/web/images/logo.svg\" align=\"right\" height=\"60\" width"
},
{
"path": "include/navigine/navigation-core/CHANGELOG.md",
"chars": 195,
"preview": "# Backlog of changes in navigation protocol\n\n## Version 1.0\n\n* Added first version of the navigation protocol\n\n## Versio"
},
{
"path": "include/navigine/navigation-core/README.md",
"chars": 1651,
"preview": "Message format\n============================\n\nEach message consists of one line of json format and describes an individua"
},
{
"path": "include/navigine/navigation-core/barriers_geometry_builder.h",
"chars": 409,
"preview": "/** barriers_geometry_builder.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_BARRIERS_GEOMETRY_BUILDER_H\n#def"
},
{
"path": "include/navigine/navigation-core/boost_geometry_adaptation.h",
"chars": 1398,
"preview": "#pragma once\n\n#include <boost/geometry.hpp>\n#include <boost/geometry/geometries/geometries.hpp>\n#include <boost/geometry"
},
{
"path": "include/navigine/navigation-core/declare_identifier.h",
"chars": 753,
"preview": "#pragma once\n\n#include <string>\n\n#define DECLARE_IDENTIFIER(IdentifierType) \\\nstruct IdentifierType { \\\n std::string "
},
{
"path": "include/navigine/navigation-core/geolevel.h",
"chars": 710,
"preview": "/** geolevel.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_GEOLEVEL_H\n#define NAVIGINE_GEOLEVEL_H\n\n#include "
},
{
"path": "include/navigine/navigation-core/graph.h",
"chars": 1660,
"preview": "/** graph.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_GRAPH_H\n#define NAVIGINE_GRAPH_H\n\n#include <vector>\n"
},
{
"path": "include/navigine/navigation-core/graph_particle.h",
"chars": 785,
"preview": "/** graph_particle.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_GRAPH_PARTICLE_H\n#define NAVIGINE_GRAPH"
},
{
"path": "include/navigine/navigation-core/level.h",
"chars": 1696,
"preview": "/** level.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_LEVEL_H\n#define NAVIGINE_LEVEL_H\n\n#include <string>\n"
},
{
"path": "include/navigine/navigation-core/level_collector.h",
"chars": 1061,
"preview": "/** level_collector.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_LEVEL_COLLECTOR_H\n#define NAVIGINE_LEVEL_C"
},
{
"path": "include/navigine/navigation-core/level_geometry.h",
"chars": 680,
"preview": "/** 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"
},
{
"path": "include/navigine/navigation-core/location_point.h",
"chars": 411,
"preview": "/** location_point.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_LOCATION_POINT_H\n#define NAVIGINE_LOCAT"
},
{
"path": "include/navigine/navigation-core/logger.h",
"chars": 1988,
"preview": "/** logger.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_LOGGER_H\n#define NAVIGINE_LOGGER_H\n\n#include <strin"
},
{
"path": "include/navigine/navigation-core/motion_info.h",
"chars": 863,
"preview": "/** motion_info.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_MOTION_INFO_H\n#define NAVIGINE_MOTION_INFO"
},
{
"path": "include/navigine/navigation-core/navigation_client.h",
"chars": 792,
"preview": "#pragma once\n\n#include \"navigation_input.h\"\n#include \"navigation_output.h\"\n#include \"navigation_state.h\"\n#include \"navig"
},
{
"path": "include/navigine/navigation-core/navigation_input.h",
"chars": 1257,
"preview": "/** navigation_input.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_INPUT_H\n#define NAVIGINE_N"
},
{
"path": "include/navigine/navigation-core/navigation_output.h",
"chars": 907,
"preview": "#ifndef NAVIGINE_NAVIGATION_OUTPUT_H\n#define NAVIGINE_NAVIGATION_OUTPUT_H\n\n#include <string>\n#include <vector>\n\n#include"
},
{
"path": "include/navigine/navigation-core/navigation_settings.h",
"chars": 7137,
"preview": "#ifndef NAVIGINE_NAVIGATION_SETTINGS_H\n#define NAVIGINE_NAVIGATION_SETTINGS_H\n\n#include <map>\n#include <vector>\n#include"
},
{
"path": "include/navigine/navigation-core/navigation_state.h",
"chars": 3073,
"preview": "/** navigation_state.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_STATE_H\n#define NAVIGINE_N"
},
{
"path": "include/navigine/navigation-core/point.h",
"chars": 1327,
"preview": "/** point.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_POINT_H\n#define NAVIGINE_POINT_H\n\nnamespace navi"
},
{
"path": "include/navigine/navigation-core/radiomap.h",
"chars": 729,
"preview": "/** radiomap.h\n*\n* Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_RADIOMAP_H\n#define NAVIGINE_RADIOMAP_H\n\n#include "
},
{
"path": "include/navigine/navigation-core/reference_point.h",
"chars": 1021,
"preview": "/** reference_point.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_REFERENCE_POINT_H\n#define NAVIGINE_REF"
},
{
"path": "include/navigine/navigation-core/transmitter.h",
"chars": 1236,
"preview": "#pragma once\n\n#include <string>\n#include <vector>\n#include <cmath>\n#include \"declare_identifier.h\"\n#include <navigine/na"
},
{
"path": "include/navigine/navigation-core/vector3d.h",
"chars": 1693,
"preview": "/** vector3d.h\n*\n** Copyv2 (c) 2018 Navigine. All v2s reserved.\n*\n*/\n\n#ifndef NAVIGINE_VECTOR3D_H\n#define NAVIGINE_VECTO"
},
{
"path": "include/navigine/navigation-core/version.h",
"chars": 122,
"preview": "#ifndef NAVIGINE_VERSION_H\n#define NAVIGINE_VERSION_H\n\n#define NAVIGATION_LIBRARY_VERSION \"2.0\" // 09 July 2019\n\n#endi"
},
{
"path": "include/navigine/navigation-core/xy_particle.h",
"chars": 513,
"preview": "/** xy_particle.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_PARTICLE_H\n#define NAVIGINE_PARTICLE_H\n\n#i"
},
{
"path": "src/README",
"chars": 66,
"preview": "the implementations of navigation algorithms developed by Navigine"
},
{
"path": "src/barriers_geometry_builder.cpp",
"chars": 647,
"preview": "/** barriers_geometry_builder.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n#include <iostream>\n#include <navigine/navig"
},
{
"path": "src/device_properties.h",
"chars": 1429,
"preview": "#ifndef NAVIGINE_DEVICE_PROPERTIES_H\n#define NAVIGINE_DEVICE_PROPERTIES_H\n\n#include <string>\n\nnamespace navigine {\nnames"
},
{
"path": "src/geometry.cpp",
"chars": 5521,
"preview": "#include <cmath>\n#include \"geometry.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\n// Get distance from A to B\ndo"
},
{
"path": "src/geometry.h",
"chars": 2735,
"preview": "/** geometry.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_GEOMETRY_H\n#define NAVIGINE_GEOMETRY_H\n\n#incl"
},
{
"path": "src/knn/knn_utils.cpp",
"chars": 14463,
"preview": "/** knn_utils.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include <iterator>\n#include <fstream>\n#include <tuple>\n\n#i"
},
{
"path": "src/knn/knn_utils.h",
"chars": 2418,
"preview": "/** 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 <navigine/n"
},
{
"path": "src/level.cpp",
"chars": 1642,
"preview": "/** level.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include <navigine/navigation-core/level.h>\n\nnamespace navigine"
},
{
"path": "src/level_collector.cpp",
"chars": 5206,
"preview": "/** level_collector.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include <navigine/navigation-core/level_collector.h>"
},
{
"path": "src/level_estimator/barometer.cpp",
"chars": 3400,
"preview": "#include <navigine/navigation-core/navigation_input.h>\n\n#include <cmath>\n#include <numeric>\n#include <vector>\n\n#include "
},
{
"path": "src/level_estimator/barometer.h",
"chars": 844,
"preview": "/** barometer.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef BAROMETER_H\n#define BAROMETER_H\n\n#include <navigine/n"
},
{
"path": "src/level_estimator/level_estimator.cpp",
"chars": 954,
"preview": "/** level_estimator.cpp\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#include <navigine/navigation-core/navigation_settings.h"
},
{
"path": "src/level_estimator/level_estimator.h",
"chars": 925,
"preview": "/** level_estimator.h\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#ifndef LEVEL_ESTIMATOR_H\n#define LEVEL_ESTIMATOR_H\n\n#incl"
},
{
"path": "src/level_estimator/level_estimator_radiomap.cpp",
"chars": 3389,
"preview": "/** level_estimator_radiomap.cpp\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#include <cmath>\n#include <navigine/navigation-"
},
{
"path": "src/level_estimator/level_estimator_radiomap.h",
"chars": 1107,
"preview": "/** level_estimator_radiomap.h\n*\n** Copyright (c) 2017 Navigine.\n*\n*/\n\n#ifndef LEVEL_ESTIMATOR_RADIOMAP_H\n#define LEVEL_"
},
{
"path": "src/level_estimator/level_estimator_transmitters.cpp",
"chars": 2067,
"preview": "/** level_estimator_transmitters.cpp\n*\n** Copyright (c) 2019 Navigine.\n*\n*/\n\n#include \"level_estimator_transmitters.h\"\n\n"
},
{
"path": "src/level_estimator/level_estimator_transmitters.h",
"chars": 693,
"preview": "/** level_estimator_transmitters.h\n*\n** Copyright (c) 2019 Navigine.\n*\n*/\n\n#ifndef LEVEL_ESTIMATOR_TRANSMITTERS_H\n#defin"
},
{
"path": "src/level_estimator/level_history.cpp",
"chars": 856,
"preview": "/** level_history.cpp\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#include \"level_history.h\"\n\nnamespace navigine {\nnamesp"
},
{
"path": "src/level_estimator/level_history.h",
"chars": 577,
"preview": "/** 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"
},
{
"path": "src/level_geometry.cpp",
"chars": 1487,
"preview": "#include <navigine/navigation-core/level_collector.h>\n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace { }\n\n"
},
{
"path": "src/likelihood/likelihood.cpp",
"chars": 886,
"preview": "#include <navigine/navigation-core/navigation_input.h>\n#include <navigine/navigation-core/level_collector.h>\n#include <n"
},
{
"path": "src/likelihood/likelihood.h",
"chars": 2775,
"preview": "#ifndef LIKELIHOOD_H\n#define LIKELIHOOD_H\n\n#include <navigine/navigation-core/xy_particle.h>\n#include <navigine/navigati"
},
{
"path": "src/likelihood/likelihood_radiomap.cpp",
"chars": 5776,
"preview": "#include \"likelihood_radiomap.h\"\n#include \"../knn/knn_utils.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\n#ifdef"
},
{
"path": "src/likelihood/likelihood_radiomap.h",
"chars": 2702,
"preview": "#ifndef LIKELIHOOD_TRIANGULATION_H\n#define LIKELIHOOD_TRIANGULATION_H\n\n#include <boost/optional.hpp>\n#include <navigine/"
},
{
"path": "src/measurement_types.h",
"chars": 491,
"preview": "#pragma once\n\n#include <navigine/navigation-core/navigation_input.h>\n\nnamespace navigine {\nnamespace navigation_core {\n\n"
},
{
"path": "src/measurements/measurement_preprocessor.cpp",
"chars": 12415,
"preview": "/** measurement_preprocessor.cpp\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#include <algorithm>\n#include <numeric>\n\n#in"
},
{
"path": "src/measurements/measurement_preprocessor.h",
"chars": 2920,
"preview": "#ifndef MEASUREMENT_PREPROCESSOR_H\n#define MEASUREMENT_PREPROCESSOR_H\n\n#include <navigine/navigation-core/navigation_inp"
},
{
"path": "src/navigation_client_impl.cpp",
"chars": 8453,
"preview": "#include <iostream>\n#include <navigine/navigation-core/navigation_settings.h>\n#include \"navigation_client_impl.h\"\n#inclu"
},
{
"path": "src/navigation_client_impl.h",
"chars": 2535,
"preview": "#pragma once\n\n#include <navigine/navigation-core/navigation_client.h>\n#include <navigine/navigation-core/navigation_inpu"
},
{
"path": "src/navigation_error_codes.h",
"chars": 671,
"preview": "/** navigation_error_codes.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_ERROR_CODES_H\n#defin"
},
{
"path": "src/point.cpp",
"chars": 1503,
"preview": "/** point.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include <navigine/navigation-core/point.h>\n#include <cmath>\n\nn"
},
{
"path": "src/position.h",
"chars": 1133,
"preview": "/** position.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_POSITION_H\n#define NAVIGINE_POSITION_H\n\n#incl"
},
{
"path": "src/position_estimator/Readme.md",
"chars": 1,
"preview": "\n"
},
{
"path": "src/position_estimator/docs/KNN.md",
"chars": 10372,
"preview": "# K-Nearest Neighbours Positioning Algorithm\n\n\nNote:: The algorithm requires special radiomap data measurement that can "
},
{
"path": "src/position_estimator/position_estimator.h",
"chars": 1238,
"preview": "#ifndef POSITION_ESTIMATOR_H\n#define POSITION_ESTIMATOR_H\n\n#include <navigine/navigation-core/navigation_output.h>\n#incl"
},
{
"path": "src/position_estimator/position_estimator_knn.cpp",
"chars": 3089,
"preview": "/** position_estimator_knn.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include <algorithm>\n#include <numeric>\n#inclu"
},
{
"path": "src/position_estimator/position_estimator_knn.h",
"chars": 1361,
"preview": "/** position_estimator_knn.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef POSITION_ESTIMATOR_KNN_H\n#define POSITIO"
},
{
"path": "src/position_estimator/position_estimator_outdoor.cpp",
"chars": 4513,
"preview": "/** position_estimator_outdoor.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n#include <navigine/navigation-core/navigati"
},
{
"path": "src/position_estimator/position_estimator_outdoor.h",
"chars": 2221,
"preview": "/** position_estimator_outdoor.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_POSITION_ESTIMATOR_OUTDOOR_"
},
{
"path": "src/position_estimator/position_estimator_zone.cpp",
"chars": 2014,
"preview": "/** position_estimator_zone.cpp\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#include <navigine/navigation-core/navigation"
},
{
"path": "src/position_estimator/position_estimator_zone.h",
"chars": 799,
"preview": "/** position_estimator_zone.h\n *\n * Copyright (c) 2017 Navigine.\n *\n */\n\n#ifndef POSITION_ESTIMATOR_ZONE_H\n#define POSIT"
},
{
"path": "src/position_postprocessor/navigation_time_smoother.cpp",
"chars": 2061,
"preview": "/** navigation_time_smoother.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include \"navigation_time_smoother.h\"\n\nnames"
},
{
"path": "src/position_postprocessor/navigation_time_smoother.h",
"chars": 566,
"preview": "/** navigation_time_smoother.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef NAVIGINE_NAVIGATION_TIME_SMOOTHER_H\n#d"
},
{
"path": "src/position_postprocessor/polynomial_fit.cpp",
"chars": 3991,
"preview": "/** polynomial_fit.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include \"polynomial_fit.h\"\n\nnamespace navigine {\nnamesp"
},
{
"path": "src/position_postprocessor/polynomial_fit.h",
"chars": 1167,
"preview": "/** polynomial_fit.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef POLYNOMIAL_FIT_H\n#define POLYNOMIAL_FIT_H\n\n#incl"
},
{
"path": "src/position_postprocessor/position_postprocessor.cpp",
"chars": 8293,
"preview": "#include <navigine/navigation-core/navigation_settings.h>\n#include \"../geometry.h\"\n#include \"position_postprocessor.h\"\n#"
},
{
"path": "src/position_postprocessor/position_postprocessor.h",
"chars": 2076,
"preview": "#ifndef POSITION_POSTPROCESSOR_H\n#define POSITION_POSTPROCESSOR_H\n\n#include <navigine/navigation-core/motion_info.h>\n#in"
},
{
"path": "src/position_postprocessor/position_smoother.h",
"chars": 525,
"preview": "#ifndef NAVIGINE_POSITION_SMOOTHER_H\n#define NAVIGINE_POSITION_SMOOTHER_H\n\n#include <navigine/navigation-core/navigation"
},
{
"path": "src/position_postprocessor/position_smoother_ab.cpp",
"chars": 2856,
"preview": "#include <navigine/navigation-core/navigation_settings.h>\n#include \"position_smoother_ab.h\"\n\nnamespace navigine {\nnamesp"
},
{
"path": "src/position_postprocessor/position_smoother_ab.h",
"chars": 756,
"preview": "#ifndef NAVIGINE_POSITION_SMOOTHER_AB_H\n#define NAVIGINE_POSITION_SMOOTHER_AB_H\n\n#include \"position_smoother.h\"\n\nnamespa"
},
{
"path": "src/position_postprocessor/position_smoother_lstsq.cpp",
"chars": 1469,
"preview": "#include <navigine/navigation-core/navigation_settings.h>\n#include \"position_smoother_lstsq.h\"\n\nnamespace navigine {\nnam"
},
{
"path": "src/position_postprocessor/position_smoother_lstsq.h",
"chars": 761,
"preview": "#ifndef NAVIGINE_POSITION_SMOOTHER_LSTSQ_H\n#define NAVIGINE_POSITION_SMOOTHER_LSTSQ_H\n\n#include \"position_smoother.h\"\n#i"
},
{
"path": "src/radiomap.cpp",
"chars": 845,
"preview": "/** radiomap.cpp\n *\n * Copyright (c) 2019 Navigine.\n *\n */\n\n#include <navigine/navigation-core/radiomap.h>\n\nnamespace na"
},
{
"path": "src/sensor_fusion/complementary_filter.cpp",
"chars": 5692,
"preview": "/** complementary_filter.cpp\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#include <navigine/navigation-core/navigation_in"
},
{
"path": "src/sensor_fusion/complementary_filter.h",
"chars": 2372,
"preview": "/** complementary_filter\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef COMPLEMENTARY_FILTER_H\n#define COMPLEMENTARY"
},
{
"path": "src/sensor_fusion/pedometer.cpp",
"chars": 8275,
"preview": "/** pedometer.cpp\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#include <navigine/navigation-core/navigation_input.h>\n\n#includ"
},
{
"path": "src/sensor_fusion/pedometer.h",
"chars": 1962,
"preview": "/** pedometer.h\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_PEDOMETER_H\n#define NAVIGINE_PEDOMETER_H\n\n#inclu"
},
{
"path": "src/sensor_fusion/quaternion.cpp",
"chars": 2547,
"preview": "/** quaternion.cpp\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#include <limits>\n#include \"quaternion.h\"\n\nnamespace navigine "
},
{
"path": "src/sensor_fusion/quaternion.h",
"chars": 1393,
"preview": "/** quaternion.h\n*\n* Copyright (c) 2018 Navigine.\n*\n*/\n\n#ifndef NAVIGINE_QUATERNION_H\n#define NAVIGINE_QUATERNION_H\n\n#in"
},
{
"path": "src/sensor_fusion/sensor_fusion.cpp",
"chars": 1748,
"preview": "#include <algorithm>\n#include <numeric>\n#include <navigine/navigation-core/navigation_settings.h>\n#include \"sensor_fusio"
},
{
"path": "src/sensor_fusion/sensor_fusion.h",
"chars": 836,
"preview": "/** sensor_measurement.h\n *\n * Copyright (c) 2018 Navigine.\n *\n */\n\n#ifndef _SENSOR_FUSION_H_\n#define _SENSOR_FUSION_H_\n"
},
{
"path": "src/triangulation.cpp",
"chars": 3189,
"preview": "#include \"triangulation.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double INDENT_METERS = 10.0;\n"
},
{
"path": "src/triangulation.h",
"chars": 4536,
"preview": "#ifndef NAVIGINE_TRIANGULATION_H\n#define NAVIGINE_TRIANGULATION_H\n\n#include <vector>\n#include <string>\n#include <map>\n\n#"
},
{
"path": "src/trilateration.cpp",
"chars": 4231,
"preview": "/** trilateration.cpp\n *\n * Copyright (c) 2021 Navigine.\n *\n */\n#include \"trilateration.h\"\n#include <Eigen/Dense>\n\nnames"
},
{
"path": "src/trilateration.h",
"chars": 338,
"preview": "#pragma once\n#include <boost/optional.hpp>\n#include <navigine/navigation-core/level.h>\n\nnamespace navigine {\nnamespace n"
},
{
"path": "src/vector3d.cpp",
"chars": 2059,
"preview": "/** vector3d.cpp\n*\n** Copyright (c) 2018 Navigine.\n*\n*/\n\n#include <limits>\n#include <navigine/navigation-core/vector3d.h"
},
{
"path": "standalone_algorithms/README.md",
"chars": 1630,
"preview": "<a href=\"http://navigine.com\"><img src=\"https://navigine.com/assets/web/images/logo.svg\" align=\"right\" height=\"60\" width"
},
{
"path": "standalone_algorithms/System description.md",
"chars": 6660,
"preview": "The performance of indoor positioning system depends on correct description of building structure that is used for navig"
},
{
"path": "standalone_algorithms/complementary_filter/CMakeLists.txt",
"chars": 2132,
"preview": "cmake_minimum_required(VERSION 3.7)\nproject(Navigation-core)\n\noption(BUILD_TESTS \"Build tests\" ON)\noption(BUILD_EXAMP"
},
{
"path": "standalone_algorithms/complementary_filter/README.md",
"chars": 8446,
"preview": "### Complementary filter\n\nComplementary filter fuses data from IMU (Intertial Measurement Units) sensors such as acceler"
},
{
"path": "standalone_algorithms/complementary_filter/examples/main.cpp",
"chars": 6480,
"preview": "#include <complementary_filter.h>\n#include <fstream>\n#include <sstream>\n#include <iostream>\n#include <random>\n\nusing nam"
},
{
"path": "standalone_algorithms/complementary_filter/helpers/plot_angles.py",
"chars": 618,
"preview": "import pandas as pd\nimport matplotlib.pylab as plt\nimport numpy as np\n\ndf1 = pd.read_csv('./../test_data/simulation.log'"
},
{
"path": "standalone_algorithms/complementary_filter/include/complementary_filter.h",
"chars": 2007,
"preview": "#pragma once\n\n#include <quaternion.h>\n#include <vector3d.h>\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Se"
},
{
"path": "standalone_algorithms/complementary_filter/include/quaternion.h",
"chars": 1231,
"preview": "#pragma once\n\n#include <vector>\n#include <stdexcept>\n#include <cmath>\n#include <tuple>\n#include <vector3d.h>\n\nnamespace "
},
{
"path": "standalone_algorithms/complementary_filter/include/vector3d.h",
"chars": 1893,
"preview": "#pragma once\n\n#include <cmath>\n#include <vector>\n#include <limits>\n\nnamespace navigine {\nnamespace navigation_core {\n\nst"
},
{
"path": "standalone_algorithms/complementary_filter/src/complementary_filter.cpp",
"chars": 4994,
"preview": "#include \"complementary_filter.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double MIN_SAMPLING_PE"
},
{
"path": "standalone_algorithms/complementary_filter/src/quaternion.cpp",
"chars": 2495,
"preview": "#include \"quaternion.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nQuaternion::Quaternion(double _w, double _x, "
},
{
"path": "standalone_algorithms/complementary_filter/src/vector3d.cpp",
"chars": 1644,
"preview": "#include <vector3d.h>\n\nnamespace navigine {\nnamespace navigation_core {\n\nVector3d::Vector3d(double _x, double _y, double"
},
{
"path": "standalone_algorithms/complementary_filter/tests/complementary_filter_test.cpp",
"chars": 3793,
"preview": "#include <complementary_filter.h>\n#include <fstream>\n#include <sstream>\n#include <iostream>\n#include <random>\n\n#define B"
},
{
"path": "standalone_algorithms/pedometer/CMakeLists.txt",
"chars": 2076,
"preview": "cmake_minimum_required(VERSION 3.7)\nproject(Navigation-core)\n\noption(BUILD_TESTS \"Build tests\" OFF)\noption(BUILD_EXAM"
},
{
"path": "standalone_algorithms/pedometer/README.md",
"chars": 1837,
"preview": "### Pedometer\n\nThe pedometer estimates steps based on accelerometer measurements collected from the user device.\n\nThe al"
},
{
"path": "standalone_algorithms/pedometer/examples/main.cpp",
"chars": 1403,
"preview": "#include <pedometer.h>\n#include <fstream>\n#include <sstream>\n#include <iostream>\n\nusing namespace navigine::navigation_c"
},
{
"path": "standalone_algorithms/pedometer/include/pedometer.h",
"chars": 1891,
"preview": "#pragma once\n\n#include <deque>\n#include <algorithm>\n#include <vector3d.h>\n\nnamespace navigine {\nnamespace navigation_cor"
},
{
"path": "standalone_algorithms/pedometer/include/vector3d.h",
"chars": 1893,
"preview": "#pragma once\n\n#include <cmath>\n#include <vector>\n#include <limits>\n\nnamespace navigine {\nnamespace navigation_core {\n\nst"
},
{
"path": "standalone_algorithms/pedometer/src/pedometer.cpp",
"chars": 6679,
"preview": "#include <cmath>\n#include <pedometer.h>\n#include <numeric>\n\nnamespace navigine {\nnamespace navigation_core {\n\nnamespace "
},
{
"path": "standalone_algorithms/pedometer/src/vector3d.cpp",
"chars": 1644,
"preview": "#include <vector3d.h>\n\nnamespace navigine {\nnamespace navigation_core {\n\nVector3d::Vector3d(double _x, double _y, double"
},
{
"path": "standalone_algorithms/pedometer/tests/pedometer_test.cpp",
"chars": 10221,
"preview": "#include <fstream>\n#include <sstream>\n#include <iostream>\n\n#include <pedometer.h>\n\n#define BOOST_TEST_DYN_LINK\n#define B"
},
{
"path": "standalone_algorithms/position_estimation/CMakeLists.txt",
"chars": 860,
"preview": "cmake_minimum_required(VERSION 3.7)\nproject(Position-estimation)\n\nset(\"target\" \"position-estimation\")\n\nlist(APPEND \"${ta"
},
{
"path": "standalone_algorithms/position_estimation/README.md",
"chars": 4969,
"preview": "### Position Estimation\n\nUsing RSSI data from transmitters it is possible to estimate the position of moving object. Est"
},
{
"path": "standalone_algorithms/position_estimation/examples/main.cpp",
"chars": 3975,
"preview": "#include <fstream>\n#include <sstream>\n#include <iostream>\n#include \"measurement_preprocessor.h\"\n#include \"nearest_transm"
},
{
"path": "standalone_algorithms/position_estimation/helpers/plot_positions.py",
"chars": 477,
"preview": "import pandas as pd\nimport matplotlib.pylab as plt\n\ndf_calculated = pd.read_csv('../logs/output.log', sep=' ', names=['t"
},
{
"path": "standalone_algorithms/position_estimation/include/measurement_preprocessor.h",
"chars": 1829,
"preview": "#pragma once\n\n#include <functional>\n#include <vector>\n#include <map>\n#include <string>\n\n#include \"navigation_structures."
},
{
"path": "standalone_algorithms/position_estimation/include/navigation_structures.h",
"chars": 1179,
"preview": "#pragma once\n\n#include <string>\n#include <vector>\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Point2D\n{\n "
},
{
"path": "standalone_algorithms/position_estimation/include/nearest_transmitter_estimator.h",
"chars": 711,
"preview": "#pragma once\n\n#include <map>\n#include \"transmitter.h\"\n#include \"navigation_structures.h\"\n#include \"position_smoother.h\"\n"
},
{
"path": "standalone_algorithms/position_estimation/include/position_smoother.h",
"chars": 496,
"preview": "#pragma once\n\n#include \"navigation_structures.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nclass PositionSmooth"
},
{
"path": "standalone_algorithms/position_estimation/include/transmitter.h",
"chars": 397,
"preview": "#pragma once\n\n#include <string>\n#include <vector>\n\nnamespace navigine {\nnamespace navigation_core {\n\nstruct Transmitter "
},
{
"path": "standalone_algorithms/position_estimation/logs/transmitters.txt",
"chars": 31035,
"preview": "(404,404,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 138.973 88.546 BEACON\n(284,284,A07C5CA8-59EB-4EA8-9956-30B776E0FEDC) 139."
},
{
"path": "standalone_algorithms/position_estimation/src/measurement_preprocessor.cpp",
"chars": 5807,
"preview": "#include <numeric>\n#include <algorithm>\n\n#include \"measurement_preprocessor.h\"\n\nnamespace navigine {\nnamespace navigatio"
},
{
"path": "standalone_algorithms/position_estimation/src/nearest_transmitter_estimator.cpp",
"chars": 1872,
"preview": "#include <cmath>\n#include <algorithm>\n#include \"nearest_transmitter_estimator.h\"\n\nnamespace navigine {\nnamespace navigat"
},
{
"path": "standalone_algorithms/position_estimation/src/position_smoother.cpp",
"chars": 1297,
"preview": "#include \"position_smoother.h\"\n\nnamespace navigine {\nnamespace navigation_core {\n\nstatic const double MAX_ALPHA = 0.9;\ns"
},
{
"path": "standalone_algorithms/trilateteration/CMakeLists.txt",
"chars": 713,
"preview": "cmake_minimum_required(VERSION 3.7)\nproject(Trilateration)\n\nset(\"test-target\" \"test\")\n\nfile(GLOB ${PROJECT_NAME}_TEST_SO"
},
{
"path": "standalone_algorithms/trilateteration/README.md",
"chars": 3472,
"preview": "### Trilateration\n\nThis is a method of determining coordinates of a device using measurements of pseudo distances to all"
},
{
"path": "standalone_algorithms/trilateteration/src/beacon.cpp",
"chars": 4533,
"preview": "/** beacon.cpp\n *\n * Author: Aleksei Smirnov <aleksei.smirnov@navigine.ru>\n * Copyright (c) 2014 Navigine. All rights re"
},
{
"path": "standalone_algorithms/trilateteration/src/beacon.h",
"chars": 3852,
"preview": "/** beacon.h\n *\n * Author: Aleksei Smirnov <aleksei.smirnov@navigine.ru>\n * Copyright (c) 2014 Navigine. All rights rese"
},
{
"path": "standalone_algorithms/trilateteration/src/test_trilateration.cpp",
"chars": 3016,
"preview": "/** test_trilateration.cpp\n *\n * Author: Aleksei Smirnov <aleksei.smirnov@navigine.com>\n * Copyright (c) 2014 Navigine. "
},
{
"path": "standalone_algorithms/trilateteration/src/test_trilateration.h",
"chars": 304,
"preview": "/** test_trilateration.h\n *\n * Author: Aleksei Smirnov <aleksei.smirnov@navigine.com>\n * Copyright (c) 2014 Navigine. Al"
},
{
"path": "standalone_algorithms/trilateteration/src/trilateration.cpp",
"chars": 11544,
"preview": " /** trilateration.cpp\n *\n * Author: Aleksei Smirnov <aleksei.smirnov@navigine.ru>\n * Copyright (c) 2014 Navigine. All "
},
{
"path": "standalone_algorithms/trilateteration/src/trilateration.h",
"chars": 3442,
"preview": "/** trilateration.h\n *\n * Author: Aleksei Smirnov <aleksei.smirnov@navigine.ru>\n * Copyright (c) 2014 Navigine. All righ"
},
{
"path": "tests/navigation_test.cpp",
"chars": 4016,
"preview": "/** navigation_test.cpp\n *\n * Copyright (c) 2014 Navigine.\n *\n */ \n\n#include <navigine/navigation-core/navigation_client"
},
{
"path": "tools/verification/helpers.cpp",
"chars": 26019,
"preview": "#include <navigine/navigation-core/navigation_input.h>\n#include <nlohmann/json.hpp>\n\n#include <algorithm>\n#include <iost"
},
{
"path": "tools/verification/helpers.h",
"chars": 1335,
"preview": "#ifndef _HELPERS_H_\n#define _HELPERS_H_\n\n#include <vector>\n#include <navigine/navigation-core/level.h>\n#include <navigin"
}
]
About this extraction
This page contains the full source code of the Navigine/Indoor-Positioning-And-Navigation-Algorithms GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 149 files (418.2 KB), approximately 127.3k tokens, and a symbol index with 337 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.