├── .gitattributes ├── .github └── workflows │ ├── build-and-test.yml │ └── pip.yml ├── .gitignore ├── CITATION.cff ├── CMakeLists.txt ├── COPYING.txt ├── README.md ├── cmake ├── Config.cmake.in └── utils.cmake ├── docs ├── benchmark.md └── images │ ├── benchmark_gauss_build_time.png │ ├── benchmark_gauss_knn_search_time.png │ └── benchmark_gauss_radius_search_time.png ├── examples ├── CMakeLists.txt ├── benchmark │ ├── CMakeLists.txt │ ├── benchmark.hpp │ ├── bin_to_ascii.cpp │ ├── bm_nanoflann.cpp │ ├── bm_opencv_flann.cpp │ ├── bm_pico_cover_tree.cpp │ ├── bm_pico_kd_tree.cpp │ ├── nano_adaptor.hpp │ ├── plot_benchmarks.py │ └── uosr_to_bin.cpp ├── eigen │ ├── CMakeLists.txt │ └── eigen.cpp ├── kd_forest │ ├── CMakeLists.txt │ ├── kd_forest.cpp │ ├── mnist.hpp │ └── sift.hpp ├── kd_tree │ ├── CMakeLists.txt │ ├── kd_tree_creation.cpp │ ├── kd_tree_custom_metric.cpp │ ├── kd_tree_custom_point_type.cpp │ ├── kd_tree_custom_search_visitor.cpp │ ├── kd_tree_custom_space_type.cpp │ ├── kd_tree_dynamic_arrays.cpp │ ├── kd_tree_minimal.cpp │ ├── kd_tree_save_and_load.cpp │ └── kd_tree_search.cpp ├── opencv │ ├── CMakeLists.txt │ └── opencv.cpp ├── pico_toolshed │ ├── CMakeLists.txt │ └── pico_toolshed │ │ ├── dynamic_space.hpp │ │ ├── format │ │ ├── endian.hpp │ │ ├── format_ascii.hpp │ │ ├── format_bin.hpp │ │ ├── format_mnist.hpp │ │ ├── format_uosr.hpp │ │ └── format_xvecs.hpp │ │ ├── point.hpp │ │ └── scoped_timer.hpp ├── pico_understory │ ├── CMakeLists.txt │ └── pico_understory │ │ ├── cover_tree.hpp │ │ ├── internal │ │ ├── cover_tree_base.hpp │ │ ├── cover_tree_builder.hpp │ │ ├── cover_tree_data.hpp │ │ ├── cover_tree_node.hpp │ │ ├── cover_tree_search.hpp │ │ ├── kd_tree_priority_search.hpp │ │ ├── matrix_space.hpp │ │ ├── matrix_space_traits.hpp │ │ ├── point_traits.hpp │ │ ├── rkd_tree_builder.hpp │ │ ├── rkd_tree_hh_data.hpp │ │ └── static_buffer.hpp │ │ ├── kd_forest.hpp │ │ └── metric.hpp └── python │ ├── CMakeLists.txt │ └── kd_tree.py ├── setup.py ├── src ├── CMakeLists.txt ├── pico_tree │ ├── CMakeLists.txt │ └── pico_tree │ │ ├── array_traits.hpp │ │ ├── core.hpp │ │ ├── distance.hpp │ │ ├── eigen3_traits.hpp │ │ ├── internal │ │ ├── box.hpp │ │ ├── kd_tree_builder.hpp │ │ ├── kd_tree_data.hpp │ │ ├── kd_tree_node.hpp │ │ ├── kd_tree_search.hpp │ │ ├── memory.hpp │ │ ├── point.hpp │ │ ├── point_wrapper.hpp │ │ ├── search_visitor.hpp │ │ ├── segment.hpp │ │ ├── space_wrapper.hpp │ │ ├── stream_wrapper.hpp │ │ └── type_traits.hpp │ │ ├── kd_tree.hpp │ │ ├── map.hpp │ │ ├── map_traits.hpp │ │ ├── metric.hpp │ │ ├── opencv_traits.hpp │ │ ├── point_traits.hpp │ │ ├── space_traits.hpp │ │ └── vector_traits.hpp └── pyco_tree │ ├── CMakeLists.txt │ └── pico_tree │ ├── CMakeLists.txt │ ├── __init__.py │ └── _pyco_tree │ ├── _pyco_tree.cpp │ ├── core.hpp │ ├── darray.hpp │ ├── def_core.hpp │ ├── def_darray.cpp │ ├── def_darray.hpp │ ├── def_kd_tree.cpp │ ├── def_kd_tree.hpp │ ├── kd_tree.hpp │ └── py_array_map.hpp └── test ├── CMakeLists.txt ├── pico_tree ├── CMakeLists.txt ├── box_test.cpp ├── common.hpp ├── cover_tree_test.cpp ├── distance_test.cpp ├── eigen3_traits_test.cpp ├── kd_tree_builder_test.cpp ├── kd_tree_test.cpp ├── metric_test.cpp ├── opencv_traits_test.cpp ├── point_map_test.cpp ├── segment_test.cpp ├── space_map_test.cpp ├── space_map_traits_test.cpp └── vector_traits_test.cpp └── pyco_tree ├── CMakeLists.txt └── kd_tree_test.py /.gitattributes: -------------------------------------------------------------------------------- 1 | # Set the default behavior, in case people don't have core.autocrlf set. 2 | * text=auto 3 | 4 | # Explicitly declare text files you want to always be normalized and converted 5 | # to native line endings on checkout. 6 | *.gitattributes text 7 | *.gitignore text 8 | 9 | *.c text 10 | *.h text 11 | *.cpp text 12 | *.hpp text 13 | *.py text 14 | *.md text 15 | *.txt text 16 | *.yml text 17 | *.cff text 18 | *.cmake text 19 | *.cmake.in text 20 | -------------------------------------------------------------------------------- /.github/workflows/build-and-test.yml: -------------------------------------------------------------------------------- 1 | name: build-and-test 2 | 3 | on: [push] 4 | 5 | jobs: 6 | build: 7 | strategy: 8 | matrix: 9 | os: [macos-latest, ubuntu-latest, windows-latest] 10 | 11 | runs-on: ${{ matrix.os }} 12 | 13 | steps: 14 | - name: Add ./install to $PATH 15 | shell: bash 16 | # Within the Windows environment the GITHUB_WORKSPACE variable uses a \ as a separator. MinGW cmake cannot handle this. 17 | # ${GITHUB_WORKSPACE//'\'/'/'} replaces \ with / using the bash shell. 18 | run: | 19 | echo "${GITHUB_WORKSPACE//'\'/'/'}/install" >> $GITHUB_PATH 20 | echo "${GITHUB_WORKSPACE//'\'/'/'}/install/bin" >> $GITHUB_PATH 21 | 22 | # Clones to ${{ github.workspace }}. 23 | - name: Clone PicoTree 24 | uses: actions/checkout@v3 25 | 26 | - name: Clone Google Test 27 | uses: actions/checkout@v3 28 | with: 29 | repository: google/googletest 30 | ref: release-1.12.1 31 | path: googletest 32 | 33 | - name: Clone Eigen 34 | shell: bash 35 | run: | 36 | git clone https://gitlab.com/libeigen/eigen.git --branch 3.3.9 --depth 1 || exit 1 37 | cd eigen && git switch -c 3.3.9 38 | 39 | - name: Clone OpenCV 40 | uses: actions/checkout@v3 41 | with: 42 | repository: opencv/opencv 43 | ref: 4.6.0 44 | path: opencv 45 | 46 | - name: CMake Google Test 47 | uses: Jaybro/action-cmake@v1 48 | with: 49 | cmake-source-dir: googletest 50 | cmake-build-dir: googletest/build 51 | # GTest is build statically but the CMake + Visual Studio combination wants to link against it as being dynamic. 52 | # https://github.com/google/googletest/tree/release-1.8.1/googletest#visual-studio-dynamic-vs-static-runtimes 53 | cmake-configure-flags: -Dgtest_force_shared_crt=ON 54 | cmake-install: true 55 | 56 | - name: CMake Eigen 57 | uses: Jaybro/action-cmake@v1 58 | with: 59 | cmake-source-dir: eigen 60 | cmake-build-dir: eigen/build 61 | cmake-install: true 62 | 63 | - name: CMake OpenCV 64 | uses: Jaybro/action-cmake@v1 65 | with: 66 | cmake-source-dir: opencv 67 | cmake-build-dir: opencv/build 68 | # The output directories for binaries and libraries may vary in Windows depending on architecture and VS version. OpenCV 69 | # allows setting the OPENCV_BIN_INSTALL_PATH and OPENCV_LIB_INSTALL_PATH cmake variables to change these (for Windows 70 | # only). The directories are relative to the CMAKE_INSTALL_PREFIX variable. 71 | cmake-configure-flags: -DBUILD_LIST=core -DOPENCV_BIN_INSTALL_PATH="bin" -DOPENCV_LIB_INSTALL_PATH="lib" -DBUILD_PERF_TESTS=OFF -DBUILD_TESTS=OFF -DBUILD_DOCS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_JAVA=OFF -DBUILD_JPEG=OFF -DBUILD_ZLIB=OFF -DBUILD_OPENJPEG=OFF -DVIDEOIO_ENABLE_PLUGINS=OFF -DWITH_OPENJPEG=OFF -DWITH_LAPACK=OFF -DWITH_CUDA=OFF -DWITH_OPENMP=OFF -DWITH_DIRECTX=OFF -DWITH_OPENGL=OFF -DCV_DISABLE_OPTIMIZATION=ON -DCV_ENABLE_INTRINSICS=OFF -DCV_TRACE=OFF -DCPU_BASELINE="" -DOPENCV_DNN_CUDA=OFF -DWITH_OPENCL=OFF -DWITH_OPENCLAMDBLAS=OFF -DWITH_OPENCLAMDFFT=OFF -DWITH_OPENCL_D3D11_NV=OFF -DWITH_OPENCL_SVM=OFF 72 | cmake-install: true 73 | 74 | - name: CMake PicoTree 75 | uses: Jaybro/action-cmake@v1 76 | with: 77 | cmake-ctest: true 78 | -------------------------------------------------------------------------------- /.github/workflows/pip.yml: -------------------------------------------------------------------------------- 1 | name: pip 2 | 3 | on: [push] 4 | 5 | jobs: 6 | build: 7 | strategy: 8 | matrix: 9 | os: [macos-latest, ubuntu-latest, windows-latest] 10 | 11 | runs-on: ${{ matrix.os }} 12 | 13 | steps: 14 | # Clones to ${{ github.workspace }}. 15 | - name: Clone PicoTree 16 | uses: actions/checkout@v3 17 | 18 | - name: Setup Python 19 | uses: actions/setup-python@v4 20 | with: 21 | python-version: "3.10" 22 | 23 | # On macos-latest, OpenMP is not installed and not visible to CMake. 24 | - name: Setup OpenMP for macOS 25 | if: runner.os == 'macOS' 26 | run: | 27 | brew install libomp 28 | brew link --force libomp 29 | 30 | # The global version of pybind11 allows us to find it via CMake. 31 | - name: Update setup related tools 32 | run: | 33 | python -m pip install wheel 34 | python -m pip install setuptools 35 | python -m pip install ninja 36 | python -m pip install scikit-build 37 | python -m pip install pybind11-global 38 | 39 | - name: Install with pip 40 | run: python -m pip install ./ -v 41 | 42 | - name: Test Python bindings 43 | run: python -m unittest discover -s ./test/pyco_tree -p '*_test.py' -v 44 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # General 2 | [Bb]uild/ 3 | _clang-format 4 | .clang-format 5 | 6 | # Visual Studio Code 7 | .vscode/ 8 | .settings/ 9 | .cproject 10 | .project 11 | 12 | # Visual Studio 13 | x64/ 14 | x86/ 15 | [Ww][Ii][Nn]32/ 16 | *.suo 17 | *.user 18 | 19 | # CLion 20 | cmake-build-*/ 21 | 22 | # Eclipse 23 | [Dd]ebug/ 24 | [Rr]elease/ 25 | 26 | # Python 27 | __pycache__ 28 | _skbuild 29 | -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | message: "Please cite this software using these metadata." 3 | authors: 4 | - given-names: Jonathan 5 | family-names: Broere 6 | title: >- 7 | "PicoTree: a C++ header only library for fast nearest neighbor and range 8 | searches using a KdTree." 9 | abstract: >- 10 | "PicoTree: a C++ header only library for fast nearest neighbor and range 11 | searches using a KdTree. It supports interfacing with Eigen, OpenCV, and 12 | custom data types and provides optional Python bindings." 13 | type: software 14 | repository: 'https://github.com/Jaybro/pico_tree' 15 | license: MIT 16 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # CMake version 3.12 provides FindPython. 2 | # CMake version 3.9 provides OpenMP per language. 3 | cmake_minimum_required(VERSION 3.12) 4 | 5 | include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/utils.cmake) 6 | 7 | project(pico_tree 8 | LANGUAGES CXX 9 | VERSION 1.0.0 10 | DESCRIPTION "PicoTree is a C++ header only library for fast nearest neighbor searches and range searches using a KdTree." 11 | HOMEPAGE_URL "https://github.com/Jaybro/pico_tree") 12 | 13 | if(NOT CMAKE_BUILD_TYPE) 14 | set(CMAKE_BUILD_TYPE "Release") 15 | endif() 16 | 17 | # ############################################################################## 18 | # PicoTree, examples, unit tests and documentation. 19 | # ############################################################################## 20 | set(PROJECT_PACKAGE_NAME "PicoTree") 21 | add_subdirectory(src) 22 | 23 | # Ignored when running cmake from setup.py using scikit-build. 24 | if(NOT SKBUILD) 25 | option(BUILD_EXAMPLES "Enable the creation of PicoTree examples." ON) 26 | message(STATUS "BUILD_EXAMPLES: ${BUILD_EXAMPLES}") 27 | 28 | if(BUILD_EXAMPLES) 29 | add_subdirectory(examples) 30 | endif() 31 | 32 | find_package(GTest QUIET) 33 | 34 | if(GTEST_FOUND) 35 | include(CTest) 36 | message(STATUS "BUILD_TESTING: ${BUILD_TESTING}") 37 | 38 | if(BUILD_TESTING) 39 | # Tests are dependent on some common code. 40 | # For now, the understory is considered important enough to be tested. 41 | if(NOT TARGET pico_toolshed) 42 | add_subdirectory(examples/pico_toolshed) 43 | add_subdirectory(examples/pico_understory) 44 | endif() 45 | 46 | enable_testing() 47 | add_subdirectory(test) 48 | endif() 49 | else() 50 | message(STATUS "GTest not found. Unit tests cannot be build.") 51 | endif() 52 | 53 | find_package(Doxygen QUIET) 54 | 55 | if(DOXYGEN_FOUND) 56 | set(DOC_TARGET_NAME ${PROJECT_NAME}_doc) 57 | 58 | # Hide the internal namespace from the documentation. 59 | # set(DOXYGEN_EXCLUDE_SYMBOLS "internal") 60 | doxygen_add_docs( 61 | ${DOC_TARGET_NAME} 62 | src/pico_tree) 63 | 64 | message(STATUS "Doxygen found. To build the documentation: cmake --build . --target ${DOC_TARGET_NAME}") 65 | else() 66 | message(STATUS "Doxygen not found. Documentation cannot be build.") 67 | endif() 68 | endif() 69 | -------------------------------------------------------------------------------- /COPYING.txt: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 - 2025 Jonathan Broere 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /cmake/Config.cmake.in: -------------------------------------------------------------------------------- 1 | @PACKAGE_INIT@ 2 | include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_PACKAGE_TARGETS_NAME@.cmake") 3 | -------------------------------------------------------------------------------- /cmake/utils.cmake: -------------------------------------------------------------------------------- 1 | function(set_default_target_properties TARGET_NAME) 2 | set_target_properties(${TARGET_NAME} 3 | PROPERTIES 4 | RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin 5 | ) 6 | endfunction() 7 | 8 | function(has_cxx_compile_feature VAR_HAS_FEATURE FEATURE) 9 | foreach(i ${CMAKE_CXX_COMPILE_FEATURES}) 10 | if(${i} STREQUAL ${FEATURE}) 11 | set(${VAR_HAS_FEATURE} TRUE PARENT_SCOPE) 12 | return() 13 | endif() 14 | endforeach() 15 | set(${VAR_HAS_FEATURE} FALSE PARENT_SCOPE) 16 | endfunction() 17 | -------------------------------------------------------------------------------- /docs/benchmark.md: -------------------------------------------------------------------------------- 1 | # Benchmark 2 | 3 | One of the PicoTree examples contains [benchmarks](../examples/benchmark/) of different KdTree implementations. This page compares the one of PicoTree with respect to [nanoflann](https://github.com/jlblancoc/nanoflann) and describes how to reproduce the input that was used for benchmarking. 4 | 5 | The results described in this document were generated on 29-08-2021 using MinGW GCC 10.3, PicoTree v0.7.4 and Nanoflann v1.3.2. 6 | 7 | Note: The performance of PicoTree v0.8.3 released on 26-09-2023 is identical to that of v0.7.4. However, the build algorithm of nanoflann v1.5.0 regressed and has become 90% slower. 8 | 9 | # Data sets 10 | 11 | The [Robotic 3D Scan Repository](http://kos.informatik.uni-osnabrueck.de/3Dscans/) provides several 3D point clouds that have been generated using a LiDAR scanner. The following has been used for the comparison benchmark: 12 | 13 | * #21 - Bremen Gaussian Point. Authors: Dorit Borrmann and Andreas Nüchter from Jacobs University Bremen gGmbH, Germany. 14 | 15 | # Results 16 | 17 | The different KdTree implementations are compared to each other with respect to the running times of the build, radius search and knn search algorithms, while fixing certain parameters. The speed of each algorithm is plotted against the leaf size of the tree. Each algorithm sets the following parameters: 18 | 19 | * Build algorithm: Compile-time vs. run-time tree dimension for the following building techniques: 20 | * Nanoflann Midpoint variation. 21 | * PicoTree Sliding Midpoint (along the longest axis). 22 | * Radius search algorithm: The radius in meters divided by 10 (i.e. 1.5m and 3.0m). 23 | * Knn algorithm: The number of neighbors searched. 24 | 25 | The running time of the benchmark was kept reasonable by using two subsets of points and storing those in a simple binary format. The final point cloud sizes were as follows: 26 | 27 | * Part 1: 7733372 points. 28 | * Part 2: 7200863 points. 29 | 30 | Both parts are 360 degree scans taken from different positions. The first is used to build a tree and the second for querying that tree. Note that each run time describes a single invocation of a build algorithm and n invocations of the others. 31 | 32 | ![Build Time](./images/benchmark_gauss_build_time.png)![Radius Search Time](./images/benchmark_gauss_radius_search_time.png) 33 | 34 | ![Knn Search Time](./images/benchmark_gauss_knn_search_time.png) 35 | 36 | # Running a new benchmark 37 | 38 | The following steps can be taken to generate data sets: 39 | 40 | 1. Download and unpack a data set. This results in a directory containing pairs of `.3d` and `.pose` files, each representing a LiDAR scan. 41 | 2. Select any of the scans (corresponding pairs of `.3d` and `.pose` files) to compile into a binary. 42 | 3. Run the `uosr_to_bin` executable as a sibling to the selected scans to generate a `scans.bin` file. 43 | 4. Two point clouds are required for a benchmark. The first should be named `scans0.bin` and the second `scans1.bin`. 44 | 45 | To reproduce the exact point clouds used by the benchmark on the Bremen Gaussian Point dataset, use scans 0-8 for the first cloud and scans 9-17 for the second. 46 | 47 | To get performance statistics: 48 | 49 | 5. Run the `bm_pico_kd_tree` or `bm_nanoflann` executables as a sibling to the `scans.bin` file and set the output format to `json`. 50 | 6. Run `plot_benchmarks.py` to show and store the performance plots (requires Python with [Matplotlib](https://matplotlib.org/)). 51 | 52 | Note the following: 53 | 54 | * A `scans.txt` file can be generated from the `scans.bin` file by running the `bin_to_ascii` executable (as a sibling to the binary file). Each line in the output file is a 3D point. 55 | -------------------------------------------------------------------------------- /docs/images/benchmark_gauss_build_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaybro/pico_tree/c5f719837df9707ee12d94cb0108aa0c34bfe96f/docs/images/benchmark_gauss_build_time.png -------------------------------------------------------------------------------- /docs/images/benchmark_gauss_knn_search_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaybro/pico_tree/c5f719837df9707ee12d94cb0108aa0c34bfe96f/docs/images/benchmark_gauss_knn_search_time.png -------------------------------------------------------------------------------- /docs/images/benchmark_gauss_radius_search_time.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jaybro/pico_tree/c5f719837df9707ee12d94cb0108aa0c34bfe96f/docs/images/benchmark_gauss_radius_search_time.png -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(pico_toolshed) 2 | 3 | # Sunlight may not reach every tree equally. 4 | # Trees or other search structures that end up in the understory can still be 5 | # interesting to others, they can be used for comparisons, or perhaps promoted 6 | # later. 7 | add_subdirectory(pico_understory) 8 | 9 | add_subdirectory(kd_tree) 10 | 11 | add_subdirectory(kd_forest) 12 | 13 | find_package(Eigen3 QUIET) 14 | 15 | if(Eigen3_FOUND) 16 | message(STATUS "Eigen3 found. Building Eigen example.") 17 | add_subdirectory(eigen) 18 | else() 19 | message(STATUS "Eigen3 not found. Eigen example skipped.") 20 | endif() 21 | 22 | find_package(OpenCV QUIET) 23 | 24 | if(OpenCV_FOUND) 25 | message(STATUS "OpenCV found. Building OpenCV example.") 26 | add_subdirectory(opencv) 27 | else() 28 | message(STATUS "OpenCV not found. OpenCV example skipped.") 29 | endif() 30 | 31 | find_package(benchmark QUIET) 32 | 33 | if(benchmark_FOUND) 34 | message(STATUS "benchmark found. Building PicoTree benchmarks.") 35 | add_subdirectory(benchmark) 36 | else() 37 | message(STATUS "benchmark not found. PicoTree benchmarks skipped.") 38 | endif() 39 | 40 | # The Python examples only get copied when the bindings module will be build. 41 | if(TARGET _pyco_tree) 42 | add_subdirectory(python) 43 | endif() 44 | -------------------------------------------------------------------------------- /examples/benchmark/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(add_benchmark TARGET_NAME) 2 | add_executable(${TARGET_NAME} ${TARGET_NAME}.cpp) 3 | set_default_target_properties(${TARGET_NAME}) 4 | target_link_libraries(${TARGET_NAME} 5 | PRIVATE 6 | pico_toolshed 7 | benchmark::benchmark 8 | ) 9 | endfunction() 10 | 11 | # ############################################################################## 12 | # bm_pico_kd_tree, bm_pico_cover_tree, bm_nanoflann, bm_opencv_flann 13 | # ############################################################################## 14 | add_benchmark(bm_pico_kd_tree) 15 | 16 | add_benchmark(bm_pico_cover_tree) 17 | target_link_libraries(bm_pico_cover_tree PRIVATE pico_understory) 18 | 19 | find_package(nanoflann QUIET) 20 | 21 | if(nanoflann_FOUND) 22 | message(STATUS "nanoflann found. Building nanoflann benchmark.") 23 | add_benchmark(bm_nanoflann) 24 | target_link_libraries(bm_nanoflann PRIVATE nanoflann::nanoflann) 25 | else() 26 | message(STATUS "nanoflann not found. nanoflann benchmark skipped.") 27 | endif() 28 | 29 | # The FLANN respository does not provide a flannConfig.cmake. So it's more 30 | # easy to go with the OpenCV one. 31 | find_package(OpenCV COMPONENTS core flann QUIET) 32 | 33 | if(OpenCV_FOUND) 34 | message(STATUS "OpenCV found. Building OpenCV FLANN benchmark.") 35 | add_benchmark(bm_opencv_flann) 36 | target_link_libraries(bm_opencv_flann PRIVATE ${OpenCV_LIBS}) 37 | else() 38 | message(STATUS "OpenCV not found. OpenCV FLANN benchmark skipped.") 39 | endif() 40 | 41 | # ############################################################################## 42 | # uosr_to_bin 43 | # ############################################################################## 44 | add_executable(uosr_to_bin uosr_to_bin.cpp) 45 | set_default_target_properties(uosr_to_bin) 46 | target_link_libraries(uosr_to_bin PRIVATE pico_toolshed) 47 | 48 | # ############################################################################## 49 | # bin_to_ascii 50 | # ############################################################################## 51 | add_executable(bin_to_ascii bin_to_ascii.cpp) 52 | set_default_target_properties(bin_to_ascii) 53 | target_link_libraries(bin_to_ascii PRIVATE pico_toolshed) 54 | 55 | # ############################################################################## 56 | # plot_benchmarks 57 | # ############################################################################## 58 | 59 | # TODO These don't get deleted when running: make clean 60 | add_custom_target(plot_benchmarks ALL 61 | COMMAND ${CMAKE_COMMAND} -E copy ${CMAKE_CURRENT_LIST_DIR}/plot_benchmarks.py ${CMAKE_BINARY_DIR}/bin) 62 | -------------------------------------------------------------------------------- /examples/benchmark/benchmark.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | // It seems that there is a "threshold" to the number of functions being 7 | // benchmarked. Having "too many" of them makes them become slow(er). This 8 | // appears to be due to code alignment. See similar issue and video referencing 9 | // that issue: 10 | // 11 | // * https://github.com/google/benchmark/issues/461 12 | // * https://www.youtube.com/watch?v=10MQW-aJU3g&t=197s 13 | // 14 | // Having split up many of the benchmarks into different executables solved it 15 | // for a while, but the problem came back for bm_pico_kd_tree. For now, 16 | // benchmarks are just enabled 1-by-1. 17 | 18 | namespace pico_tree { 19 | 20 | class Benchmark : public benchmark::Fixture { 21 | protected: 22 | using index_type = int; 23 | using scalar_type = float; 24 | using point_type = point_3f; 25 | 26 | public: 27 | Benchmark() { 28 | // Here you may need to be patient depending on the size of the binaries. 29 | // Loaded for each benchmark. 30 | pico_tree::read_bin("./scans0.bin", points_tree_); 31 | pico_tree::read_bin("./scans1.bin", points_test_); 32 | } 33 | 34 | protected: 35 | std::vector points_tree_; 36 | std::vector points_test_; 37 | }; 38 | 39 | } // namespace pico_tree 40 | -------------------------------------------------------------------------------- /examples/benchmark/bin_to_ascii.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main() { 7 | std::filesystem::path dirname_root = "."; 8 | std::filesystem::path filename_bin = "scans.bin"; 9 | std::filesystem::path filename_ascii = "scans.txt"; 10 | std::filesystem::path path_bin = dirname_root / filename_bin; 11 | std::filesystem::path path_ascii = dirname_root / filename_ascii; 12 | 13 | if (!std::filesystem::exists(path_bin)) { 14 | std::cout << path_bin.string() << " doesn't exist." << std::endl; 15 | } else if (!std::filesystem::exists(path_ascii)) { 16 | std::cout << "Reading points in bin format..." << std::endl; 17 | std::vector points; 18 | pico_tree::read_bin(path_bin.string(), points); 19 | std::cout << "Read " << points.size() << " points." << std::endl; 20 | std::cout << "Writing points to ascii xyz format..." << std::endl; 21 | pico_tree::write_ascii(path_ascii.string(), points); 22 | std::cout << "Done." << std::endl; 23 | } else { 24 | std::cout << path_ascii.string() << " already exists." << std::endl; 25 | } 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /examples/benchmark/bm_nanoflann.cpp: -------------------------------------------------------------------------------- 1 | #include "benchmark.hpp" 2 | #include "nano_adaptor.hpp" 3 | 4 | class BmNanoflann : public pico_tree::Benchmark { 5 | public: 6 | using nano_adaptor_type = nano_adaptor; 7 | }; 8 | 9 | template 10 | using nano_kd_tree_ct = nanoflann::KDTreeSingleIndexAdaptor< 11 | nanoflann:: 12 | L2_Simple_Adaptor, 13 | NanoAdaptor_, 14 | NanoAdaptor_::dim, 15 | typename NanoAdaptor_::index_type>; 16 | 17 | template 18 | using nano_kd_tree_rt = nanoflann::KDTreeSingleIndexAdaptor< 19 | nanoflann:: 20 | L2_Simple_Adaptor, 21 | NanoAdaptor_, 22 | -1, 23 | typename NanoAdaptor_::index_type>; 24 | 25 | // **************************************************************************** 26 | // Building the tree 27 | // **************************************************************************** 28 | 29 | BENCHMARK_DEFINE_F(BmNanoflann, BuildCt)(benchmark::State& state) { 30 | std::size_t max_leaf_size = static_cast(state.range(0)); 31 | nano_adaptor_type adaptor(points_tree_); 32 | for (auto _ : state) { 33 | nano_kd_tree_ct tree( 34 | point_type::dim, 35 | adaptor, 36 | nanoflann::KDTreeSingleIndexAdaptorParams(max_leaf_size)); 37 | tree.buildIndex(); 38 | } 39 | } 40 | 41 | BENCHMARK_DEFINE_F(BmNanoflann, BuildRt)(benchmark::State& state) { 42 | std::size_t max_leaf_size = static_cast(state.range(0)); 43 | nano_adaptor_type adaptor(points_tree_); 44 | for (auto _ : state) { 45 | nano_kd_tree_rt tree( 46 | point_type::dim, 47 | adaptor, 48 | nanoflann::KDTreeSingleIndexAdaptorParams(max_leaf_size)); 49 | tree.buildIndex(); 50 | } 51 | } 52 | 53 | // Argument 1: Maximum leaf size. 54 | BENCHMARK_REGISTER_F(BmNanoflann, BuildCt) 55 | ->Unit(benchmark::kMillisecond) 56 | ->Arg(1) 57 | ->DenseRange(6, 14, 2); 58 | 59 | BENCHMARK_REGISTER_F(BmNanoflann, BuildRt) 60 | ->Unit(benchmark::kMillisecond) 61 | ->Arg(1) 62 | ->DenseRange(6, 14, 2); 63 | 64 | // **************************************************************************** 65 | // Knn 66 | // **************************************************************************** 67 | 68 | BENCHMARK_DEFINE_F(BmNanoflann, KnnCt)(benchmark::State& state) { 69 | std::size_t max_leaf_size = static_cast(state.range(0)); 70 | std::size_t knn_count = static_cast(state.range(1)); 71 | nano_adaptor_type adaptor(points_tree_); 72 | nano_kd_tree_ct tree( 73 | point_type::dim, 74 | adaptor, 75 | nanoflann::KDTreeSingleIndexAdaptorParams(max_leaf_size)); 76 | tree.buildIndex(); 77 | 78 | for (auto _ : state) { 79 | std::vector indices(knn_count); 80 | std::vector distances(knn_count); 81 | std::size_t sum = 0; 82 | for (auto const& p : points_test_) { 83 | benchmark::DoNotOptimize( 84 | sum += tree.knnSearch( 85 | p.data(), knn_count, indices.data(), distances.data())); 86 | } 87 | } 88 | } 89 | 90 | // Argument 1: Maximum leaf size. 91 | // Argument 2: K nearest neighbors. 92 | BENCHMARK_REGISTER_F(BmNanoflann, KnnCt) 93 | ->Unit(benchmark::kMillisecond) 94 | ->Args({1, 1}) 95 | ->Args({6, 1}) 96 | ->Args({8, 1}) 97 | ->Args({10, 1}) 98 | ->Args({12, 1}) 99 | ->Args({14, 1}) 100 | ->Args({1, 4}) 101 | ->Args({6, 4}) 102 | ->Args({8, 4}) 103 | ->Args({10, 4}) 104 | ->Args({12, 4}) 105 | ->Args({14, 4}) 106 | ->Args({1, 8}) 107 | ->Args({6, 8}) 108 | ->Args({8, 8}) 109 | ->Args({10, 8}) 110 | ->Args({12, 8}) 111 | ->Args({14, 8}) 112 | ->Args({1, 12}) 113 | ->Args({6, 12}) 114 | ->Args({8, 12}) 115 | ->Args({10, 12}) 116 | ->Args({12, 12}) 117 | ->Args({14, 12}); 118 | 119 | // **************************************************************************** 120 | // Radius 121 | // **************************************************************************** 122 | 123 | BENCHMARK_DEFINE_F(BmNanoflann, RadiusCt)(benchmark::State& state) { 124 | std::size_t max_leaf_size = static_cast(state.range(0)); 125 | scalar_type radius = 126 | static_cast(state.range(1)) / scalar_type(10.0); 127 | scalar_type squared = radius * radius; 128 | nano_adaptor_type adaptor(points_tree_); 129 | nano_kd_tree_ct tree( 130 | point_type::dim, 131 | adaptor, 132 | nanoflann::KDTreeSingleIndexAdaptorParams(max_leaf_size)); 133 | tree.buildIndex(); 134 | 135 | for (auto _ : state) { 136 | std::vector> results; 137 | std::size_t sum = 0; 138 | for (auto const& p : points_test_) { 139 | benchmark::DoNotOptimize( 140 | sum += tree.radiusSearch( 141 | p.data(), 142 | squared, 143 | results, 144 | nanoflann::SearchParameters{0, false})); 145 | } 146 | } 147 | } 148 | 149 | // Argument 1: Maximum leaf size. 150 | // Argument 2: Search radius (divided by 10.0). 151 | BENCHMARK_REGISTER_F(BmNanoflann, RadiusCt) 152 | ->Unit(benchmark::kMillisecond) 153 | ->Args({1, 15}) 154 | ->Args({6, 15}) 155 | ->Args({8, 15}) 156 | ->Args({10, 15}) 157 | ->Args({12, 15}) 158 | ->Args({14, 15}) 159 | ->Args({1, 30}) 160 | ->Args({6, 30}) 161 | ->Args({8, 30}) 162 | ->Args({10, 30}) 163 | ->Args({12, 30}) 164 | ->Args({14, 30}); 165 | 166 | BENCHMARK_MAIN(); 167 | -------------------------------------------------------------------------------- /examples/benchmark/bm_pico_cover_tree.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "benchmark.hpp" 7 | 8 | class BmPicoCoverTree : public pico_tree::Benchmark { 9 | public: 10 | }; 11 | 12 | // Index explicitly set to int. 13 | template 14 | using pico_space = std::reference_wrapper>; 15 | 16 | template 17 | using pico_cover_tree = pico_tree::cover_tree>; 18 | 19 | // **************************************************************************** 20 | // Building the tree 21 | // **************************************************************************** 22 | 23 | BENCHMARK_DEFINE_F(BmPicoCoverTree, BuildCt)(benchmark::State& state) { 24 | scalar_type base = 25 | static_cast(state.range(0)) / scalar_type(10.0); 26 | 27 | for (auto _ : state) { 28 | pico_cover_tree tree(points_tree_, base); 29 | } 30 | } 31 | 32 | BENCHMARK_REGISTER_F(BmPicoCoverTree, BuildCt) 33 | ->Unit(benchmark::kMillisecond) 34 | ->Arg(13) 35 | ->DenseRange(14, 20, 2); 36 | 37 | // **************************************************************************** 38 | // Knn 39 | // **************************************************************************** 40 | 41 | BENCHMARK_DEFINE_F(BmPicoCoverTree, KnnCt)(benchmark::State& state) { 42 | scalar_type base = 43 | static_cast(state.range(0)) / scalar_type(10.0); 44 | pico_tree::size_t knn_count = static_cast(state.range(1)); 45 | 46 | pico_cover_tree tree(points_tree_, base); 47 | 48 | for (auto _ : state) { 49 | std::vector> results; 50 | std::size_t sum = 0; 51 | std::size_t group = 4000; 52 | std::size_t pi = 0; 53 | while (pi < points_test_.size()) { 54 | std::size_t group_end = std::min(pi + group, points_test_.size()); 55 | std::flush(std::cout); 56 | pico_tree::scoped_timer timer("query_group"); 57 | for (; pi < group_end; ++pi) { 58 | auto const& p = points_test_[pi]; 59 | tree.search_knn(p, knn_count, results); 60 | benchmark::DoNotOptimize(sum += results.size()); 61 | } 62 | } 63 | } 64 | } 65 | 66 | BENCHMARK_REGISTER_F(BmPicoCoverTree, KnnCt) 67 | ->Unit(benchmark::kMillisecond) 68 | ->Args({13, 1}); 69 | 70 | BENCHMARK_MAIN(); 71 | -------------------------------------------------------------------------------- /examples/benchmark/nano_adaptor.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | //! Demo point set adaptor for a vector of points. 7 | template 8 | class nano_adaptor { 9 | public: 10 | using index_type = Index_; 11 | using point_type = Point_; 12 | using scalar_type = typename Point_::scalar_type; 13 | static constexpr int dim = static_cast(Point_::dim); 14 | 15 | nano_adaptor(std::vector const& points) : points_(points) {} 16 | 17 | //! \brief Returns the number of points. 18 | inline Index_ kdtree_get_point_count() const { 19 | return static_cast(points_.size()); 20 | } 21 | 22 | //! \brief Returns the dim'th component of the idx'th point in the class: 23 | inline scalar_type kdtree_get_pt(Index_ const idx, Index_ const dim) const { 24 | return points_[static_cast::size_type>(idx)] 25 | .data()[dim]; 26 | } 27 | 28 | // Optional bounding-box computation: return false to default to a standard 29 | // bbox computation loop. 30 | // Return true if the BBOX was already computed by the class and returned in 31 | // "bb" so it can be avoided to redo it again. Look at bb.size() to find out 32 | // the expected dimensionality (e.g. 2 or 3 for point clouds) 33 | template 34 | bool kdtree_get_bbox(BBOX_& /*bb*/) const { 35 | return false; 36 | } 37 | 38 | private: 39 | std::vector const& points_; 40 | }; 41 | -------------------------------------------------------------------------------- /examples/benchmark/plot_benchmarks.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from argparse import ArgumentParser, FileType 4 | import json 5 | import re 6 | import matplotlib.pyplot as plt 7 | 8 | 9 | def create_arguments(): 10 | parser = ArgumentParser(description='benchmark visualization tool') 11 | parser.add_argument('-json_file', '-j', type=FileType('r'), 12 | required=True, 13 | help='JSON file with benchmark data') 14 | 15 | return parser 16 | 17 | 18 | def filter_benchmarks(benchmarks, filter_pattern): 19 | return [x for x in benchmarks if filter_pattern.match(x['name'])] 20 | 21 | 22 | def filter_benchmark_categories(json): 23 | benchmarks = [x for x in json['benchmarks'] if x['name'].endswith('_mean')] 24 | 25 | if not benchmarks: 26 | benchmarks = json['benchmarks'] 27 | 28 | return [ 29 | filter_benchmarks(benchmarks, re.compile(r'.+/Build.+')), 30 | filter_benchmarks(benchmarks, re.compile(r'.+/(Knn|Nn).+')), 31 | filter_benchmarks(benchmarks, re.compile(r'.+/Radius.+')) 32 | ] 33 | 34 | 35 | def create_plots(benchmarks, pattern): 36 | plots = dict() 37 | 38 | for x in benchmarks: 39 | m = pattern.match(x['name']) 40 | k = m.group('tree') + '_' + \ 41 | m.group('type') + (('_' + m.group('arg')) 42 | if m.group('arg') else '') 43 | plots[k] = {'x': [], 'y': []} 44 | 45 | for x in benchmarks: 46 | m = pattern.match(x['name']) 47 | k = m.group('tree') + '_' + \ 48 | m.group('type') + (('_' + m.group('arg')) 49 | if m.group('arg') else '') 50 | plots[k]['x'].append(float(m.group('x'))) 51 | plots[k]['y'].append(float(x['cpu_time'])) 52 | 53 | return plots 54 | 55 | 56 | def create_figure(plots, title): 57 | fig, ax = plt.subplots(figsize=(4, 4), tight_layout=True) 58 | 59 | for label in plots: 60 | x = plots[label]['x'] 61 | y = plots[label]['y'] 62 | ax.plot(x, y, label=label, marker='x') 63 | 64 | ax.set_xlabel('max leaf size') 65 | ax.set_ylabel('cpu time ms') 66 | ax.set_title(title) 67 | ax.legend() 68 | 69 | return fig, ax 70 | 71 | 72 | def main(): 73 | parser = create_arguments() 74 | args = parser.parse_args() 75 | 76 | benchmarks = filter_benchmark_categories(json.load(args.json_file)) 77 | re_info = r'^Bm(?P.+)/(Build|Knn|Nn|Radius)(?P(Ct|Rt)[^/]*)/(?P\d+)(/(?P\d+))?(_mean)?$' 78 | plots = [create_plots(b, re.compile(re_info)) for b in benchmarks] 79 | titles = ['build time', 'knn search time', 'radius search time'] 80 | # Format is determined by filename extension 81 | extension = '.png' 82 | file_names = [ 83 | f'./build_time{extension}', 84 | f'./knn_search_time{extension}', 85 | f'./radius_search_time{extension}'] 86 | 87 | for i in range(len(plots)): 88 | create_figure(plots[i], titles[i])[0].savefig(file_names[i]) 89 | plt.show() 90 | 91 | 92 | if __name__ == '__main__': 93 | main() 94 | -------------------------------------------------------------------------------- /examples/benchmark/uosr_to_bin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main() { 6 | std::filesystem::path dirname_root = "."; 7 | std::filesystem::path filename_bin = "scans.bin"; 8 | std::filesystem::path path_bin = dirname_root / filename_bin; 9 | 10 | if (!std::filesystem::exists(path_bin)) { 11 | std::cout << "Reading scans in uosr format..." << std::endl; 12 | std::vector points; 13 | pico_tree::read_uosr(dirname_root.string(), points); 14 | std::cout << "Read " << points.size() << " points." << std::endl; 15 | std::cout << "Writing scans to bin xyz format..." << std::endl; 16 | pico_tree::write_bin(path_bin.string(), points); 17 | std::cout << "Done." << std::endl; 18 | } else { 19 | std::cout << path_bin.string() << " already exists." << std::endl; 20 | } 21 | 22 | return 0; 23 | } 24 | -------------------------------------------------------------------------------- /examples/eigen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(eigen eigen.cpp) 2 | set_default_target_properties(eigen) 3 | target_link_libraries(eigen PUBLIC pico_toolshed Eigen3::Eigen) 4 | -------------------------------------------------------------------------------- /examples/eigen/eigen.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // Because we use C++17 there is no need to take care of memory alignment: 7 | // https://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html 8 | // https://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html 9 | 10 | template 11 | using points_map_col_major = Eigen::Map>; 15 | 16 | template 17 | using points_map_row_major = Eigen::Map>; 22 | 23 | std::size_t const run_count = 1024 * 1024; 24 | std::size_t const num_points = 1024 * 1024 * 2; 25 | float const point_area = 1000.0; 26 | pico_tree::max_leaf_size_t const max_leaf_count = 16; 27 | 28 | template 29 | std::vector generate_random_eigen_n( 30 | std::size_t n, typename Point_::Scalar size) { 31 | std::vector random(n); 32 | for (auto& p : random) { 33 | p = Point_::Random() * size / typename Point_::Scalar(2.0); 34 | } 35 | 36 | return random; 37 | } 38 | 39 | // Creates a kd_tree from a vector of Eigen::VectorX and searches for nearest 40 | // neighbors. 41 | void basic_vector() { 42 | using point = Eigen::Vector2f; 43 | using scalar = typename point::Scalar; 44 | using index = int; 45 | 46 | // Including provides support for Eigen types 47 | // with std::vector. 48 | pico_tree::kd_tree tree( 49 | generate_random_eigen_n(num_points, point_area), max_leaf_count); 50 | 51 | point p = point::Random() * point_area / scalar(2.0); 52 | 53 | pico_tree::neighbor nn; 54 | pico_tree::scoped_timer t("pico_tree eigen vector", run_count); 55 | for (std::size_t i = 0; i < run_count; ++i) { 56 | tree.search_nn(p, nn); 57 | } 58 | } 59 | 60 | // Creates a kd_tree from an Eigen::Matrix<> and searches for nearest neighbors. 61 | void basic_matrix() { 62 | using kd_tree = pico_tree::kd_tree; 63 | using neighbor = typename kd_tree::neighbor_type; 64 | using scalar = typename Eigen::Matrix3Xf::Scalar; 65 | constexpr int dim = Eigen::Matrix3Xf::RowsAtCompileTime; 66 | 67 | kd_tree tree( 68 | Eigen::Matrix3Xf::Random(dim, num_points) * point_area / scalar(2.0), 69 | max_leaf_count); 70 | 71 | Eigen::Vector3f p = Eigen::Vector3f::Random() * point_area / scalar(2.0); 72 | neighbor nn; 73 | pico_tree::scoped_timer t("pico_tree eigen matrix", run_count); 74 | for (std::size_t i = 0; i < run_count; ++i) { 75 | tree.search_nn(p, nn); 76 | } 77 | } 78 | 79 | // Creates a kd_tree from a col-major matrix. The matrix maps an 80 | // std::vector. 81 | void col_major_support() { 82 | using point = Eigen::Vector3f; 83 | using map = points_map_col_major; 84 | using kd_tree = pico_tree::kd_tree; 85 | using neighbor = typename kd_tree::neighbor_type; 86 | using scalar = typename point::Scalar; 87 | constexpr Eigen::Index dim = point::RowsAtCompileTime; 88 | 89 | auto points = generate_random_eigen_n(num_points, point_area); 90 | point p = point::Random() * point_area / scalar(2.0); 91 | 92 | std::cout << "Eigen RowMajor: " << map::IsRowMajor << std::endl; 93 | 94 | kd_tree tree( 95 | map(points.data()->data(), dim, static_cast(num_points)), 96 | max_leaf_count); 97 | 98 | std::vector knn; 99 | pico_tree::scoped_timer t("pico_tree col major", run_count); 100 | for (std::size_t i = 0; i < run_count; ++i) { 101 | tree.search_knn(p, 1, knn); 102 | } 103 | } 104 | 105 | // Creates a kd_tree from a row-major matrix. The matrix maps an 106 | // std::vector. 107 | void row_major_support() { 108 | using point = Eigen::RowVector3f; 109 | using map = points_map_row_major; 110 | using kd_tree = pico_tree::kd_tree; 111 | using neighbor = typename kd_tree::neighbor_type; 112 | using scalar = typename point::Scalar; 113 | constexpr Eigen::Index dim = point::ColsAtCompileTime; 114 | 115 | auto points = generate_random_eigen_n(num_points, point_area); 116 | point p = point::Random() * point_area / scalar(2.0); 117 | 118 | std::cout << "Eigen RowMajor: " << point::IsRowMajor << std::endl; 119 | 120 | kd_tree tree( 121 | map(points.data()->data(), static_cast(num_points), dim), 122 | max_leaf_count); 123 | 124 | std::vector knn; 125 | pico_tree::scoped_timer t("pico_tree row major", run_count); 126 | for (std::size_t i = 0; i < run_count; ++i) { 127 | tree.search_knn(p, 1, knn); 128 | } 129 | } 130 | 131 | int main() { 132 | basic_vector(); 133 | basic_matrix(); 134 | col_major_support(); 135 | row_major_support(); 136 | return 0; 137 | } 138 | -------------------------------------------------------------------------------- /examples/kd_forest/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(kd_forest kd_forest.cpp) 2 | set_default_target_properties(kd_forest) 3 | target_link_libraries(kd_forest PUBLIC pico_toolshed pico_understory) 4 | -------------------------------------------------------------------------------- /examples/kd_forest/kd_forest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "mnist.hpp" 10 | #include "sift.hpp" 11 | 12 | template 13 | std::size_t run_kd_forest( 14 | std::vector const& train, 15 | std::vector const& test, 16 | std::vector> const& nns, 17 | std::size_t forest_size, 18 | std::size_t forest_max_leaf_size, 19 | std::size_t forest_max_leaves_visited) { 20 | using space = std::reference_wrapper const>; 21 | 22 | auto rkd_tree = [&train, &forest_max_leaf_size, &forest_size]() { 23 | pico_tree::scoped_timer t0("kd_forest build"); 24 | return pico_tree::kd_forest( 25 | train, forest_max_leaf_size, forest_size); 26 | }(); 27 | 28 | pico_tree::scoped_timer t1("kd_forest query"); 29 | pico_tree::neighbor nn; 30 | 31 | std::size_t equal = 0; 32 | for (std::size_t i = 0; i < nns.size(); ++i) { 33 | rkd_tree.search_nn(test[i], forest_max_leaves_visited, nn); 34 | 35 | if (nns[i].index == nn.index) { 36 | ++equal; 37 | } 38 | } 39 | 40 | return equal; 41 | } 42 | 43 | template 44 | void run_kd_tree( 45 | std::vector const& train, 46 | std::vector const& test, 47 | std::string const& fn_nns_gt, 48 | pico_tree::max_leaf_size_t tree_max_leaf_size, 49 | std::vector>& nns) { 50 | using space = std::reference_wrapper const>; 51 | 52 | nns.resize(test.size()); 53 | 54 | if (!std::filesystem::exists(fn_nns_gt)) { 55 | std::cout << "Creating " << fn_nns_gt 56 | << " using the kd_tree. Be *very* patient." << std::endl; 57 | 58 | auto kd_tree = [&train, &tree_max_leaf_size]() { 59 | pico_tree::scoped_timer t0("kd_tree build"); 60 | return pico_tree::kd_tree(train, tree_max_leaf_size); 61 | }(); 62 | 63 | { 64 | pico_tree::scoped_timer t1("kd_tree query"); 65 | for (std::size_t i = 0; i < nns.size(); ++i) { 66 | kd_tree.search_nn(test[i], nns[i]); 67 | } 68 | } 69 | 70 | pico_tree::write_bin(fn_nns_gt, nns); 71 | } else { 72 | pico_tree::read_bin(fn_nns_gt, nns); 73 | std::cout << "kd_tree not created. Read " << fn_nns_gt << " instead." 74 | << std::endl; 75 | } 76 | } 77 | 78 | // A kd_forest takes roughly forest_size times longer to build compared to 79 | // building a kd_tree. However, the kd_forest is usually a lot faster with 80 | // queries in high dimensions with the added trade-off that the exact nearest 81 | // neighbor may not be found. 82 | template 83 | void run_dataset( 84 | std::size_t tree_max_leaf_size, 85 | std::size_t forest_size, 86 | std::size_t forest_max_leaf_size, 87 | std::size_t forest_max_leaves_visited) { 88 | using Point = typename Dataset_::point_type; 89 | using scalar_type = typename Point::value_type; 90 | 91 | auto train = Dataset_::read_train(); 92 | auto test = Dataset_::read_test(); 93 | std::vector> nns; 94 | std::string fn_nns_gt = Dataset_::dataset_name + "_nns_gt.bin"; 95 | 96 | run_kd_tree(train, test, fn_nns_gt, tree_max_leaf_size, nns); 97 | 98 | std::size_t equal = run_kd_forest( 99 | train, 100 | test, 101 | nns, 102 | forest_size, 103 | forest_max_leaf_size, 104 | forest_max_leaves_visited); 105 | 106 | std::cout << std::setprecision(10); 107 | std::cout << "Precision: " 108 | << (static_cast(equal) / static_cast(nns.size())) 109 | << std::endl; 110 | } 111 | 112 | int main() { 113 | // forest_max_leaf_size = 16 114 | // forest_max_leaves_visited = 16 115 | // forest_size 8: a precision of around 0.915. 116 | // forest_size 16: a precision of around 0.976. 117 | run_dataset(16, 8, 16, 16); 118 | // forest_max_leaf_size = 32 119 | // forest_max_leaves_visited = 64 120 | // forest_size 8: a precision of around 0.884. 121 | // forest_size 16: a precision of around 0.940. 122 | // forest_size 128: out of memory :'( 123 | run_dataset(16, 8, 32, 64); 124 | return 0; 125 | } 126 | -------------------------------------------------------------------------------- /examples/kd_forest/mnist.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | template 8 | std::array cast(std::array const& i) { 9 | std::array c; 10 | std::transform(i.begin(), i.end(), c.begin(), [](T_ a) -> U_ { 11 | return static_cast(a); 12 | }); 13 | return c; 14 | } 15 | 16 | template 17 | std::vector> cast(std::vector> const& i) { 18 | std::vector> c; 19 | std::transform( 20 | i.begin(), 21 | i.end(), 22 | std::back_inserter(c), 23 | [](std::array const& a) -> std::array { 24 | return cast(a); 25 | }); 26 | return c; 27 | } 28 | 29 | class mnist { 30 | private: 31 | using scalar_type = float; 32 | using image_byte = std::array; 33 | using image_float = std::array; 34 | 35 | static std::vector read_images(std::string const& filename) { 36 | if (!std::filesystem::exists(filename)) { 37 | throw std::runtime_error(filename + " doesn't exist"); 38 | } 39 | 40 | std::vector images_u8; 41 | pico_tree::read_mnist_images(filename, images_u8); 42 | return cast(images_u8); 43 | } 44 | 45 | public: 46 | using point_type = image_float; 47 | 48 | static std::string const dataset_name; 49 | 50 | static std::vector read_train() { 51 | std::string fn_images_train = "train-images.idx3-ubyte"; 52 | return read_images(fn_images_train); 53 | } 54 | 55 | static std::vector read_test() { 56 | std::string fn_images_test = "t10k-images.idx3-ubyte"; 57 | return read_images(fn_images_test); 58 | } 59 | }; 60 | 61 | std::string const mnist::dataset_name = "mnist"; 62 | -------------------------------------------------------------------------------- /examples/kd_forest/sift.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class sift { 7 | private: 8 | using vector_float = std::array; 9 | 10 | static std::vector read_vectors(std::string const& filename) { 11 | if (!std::filesystem::exists(filename)) { 12 | throw std::runtime_error(filename + " doesn't exist"); 13 | } 14 | 15 | std::vector vectors; 16 | pico_tree::read_xvecs(filename, vectors); 17 | return vectors; 18 | } 19 | 20 | public: 21 | using point_type = vector_float; 22 | 23 | static std::string const dataset_name; 24 | 25 | static std::vector read_train() { 26 | std::string fn_images_train = "sift_base.fvecs"; 27 | return read_vectors(fn_images_train); 28 | } 29 | 30 | static std::vector read_test() { 31 | std::string fn_images_test = "sift_query.fvecs"; 32 | return read_vectors(fn_images_test); 33 | } 34 | }; 35 | 36 | std::string const sift::dataset_name = "sift"; 37 | -------------------------------------------------------------------------------- /examples/kd_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | function(add_demo_executable TARGET_NAME) 2 | add_executable(${TARGET_NAME} ${TARGET_NAME}.cpp) 3 | set_default_target_properties(${TARGET_NAME}) 4 | target_link_libraries(${TARGET_NAME} PRIVATE pico_toolshed) 5 | endfunction() 6 | 7 | add_demo_executable(kd_tree_minimal) 8 | 9 | add_demo_executable(kd_tree_creation) 10 | 11 | add_demo_executable(kd_tree_search) 12 | 13 | add_demo_executable(kd_tree_dynamic_arrays) 14 | 15 | add_demo_executable(kd_tree_custom_metric) 16 | 17 | add_demo_executable(kd_tree_custom_point_type) 18 | 19 | add_demo_executable(kd_tree_custom_space_type) 20 | 21 | add_demo_executable(kd_tree_custom_search_visitor) 22 | 23 | add_demo_executable(kd_tree_save_and_load) 24 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_custom_metric.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // This example shows how to create a custom metric for the kd_tree. The kd_tree 6 | // does not support metrics that "normalize" the distance between two points for 7 | // performance reasons. This means that, for example, the L2 metric is not 8 | // supported, but that there is support for the L1 or L2^2 metric. 9 | // 10 | // There are two different categories of metrics: Euclidean and topological. A 11 | // Euclidean metric supports any R^n space where all axes are orthogonal with 12 | // respect to each other. A topological space is the same, but it allows 13 | // wrapping of coordinate values along any of the axes. An implementation of a 14 | // custom metric is provided for both below. 15 | 16 | // The LP^P metric is a generalization of the L2^2 metric. 17 | template 18 | struct metric_lp_p { 19 | static_assert(P_ > 0, "P_CANNOT_BE_ZERO"); 20 | 21 | // Indicate that this metric is a Euclidean one. 22 | using space_category = pico_tree::euclidean_space_tag; 23 | 24 | // Calculates the distance between two points. 25 | template 26 | auto operator()( 27 | InputIterator_ begin1, InputIterator_ end1, InputIterator_ begin2) const { 28 | using scalar_type = 29 | typename std::iterator_traits::value_type; 30 | 31 | scalar_type d{}; 32 | for (; begin1 != end1; ++begin1, ++begin2) { 33 | d += operator()(*begin1 - *begin2); 34 | } 35 | return d; 36 | } 37 | 38 | // Returns the absolute value of x to the power of p. 39 | template 40 | Scalar_ operator()(Scalar_ x) const { 41 | return std::pow(std::abs(x), static_cast(P_)); 42 | } 43 | }; 44 | 45 | // This metric measures distances on the two dimensional ring torus T2. The 46 | // torus is the Cartesian product of two circles S1 x S1. The values of each of 47 | // the point coordinates should be within the range of [0...1]. 48 | struct metric_t2_squared { 49 | // Indicate that this metric is defined on a topological space. The 50 | // topological_space_tag is required because the torus wraps around in both 51 | // dimensions. 52 | using space_category = pico_tree::topological_space_tag; 53 | 54 | // Calculates the distance between two points. 55 | template 56 | auto operator()( 57 | InputIterator_ begin1, InputIterator_ end1, InputIterator_ begin2) const { 58 | using scalar_type = 59 | typename std::iterator_traits::value_type; 60 | 61 | scalar_type d{}; 62 | for (; begin1 != end1; ++begin1, ++begin2) { 63 | d += pico_tree::squared_s1_distance(*begin1, *begin2); 64 | } 65 | return d; 66 | } 67 | 68 | // Distances are squared values. 69 | template 70 | Scalar_ operator()(Scalar_ x) const { 71 | return x * x; 72 | } 73 | 74 | template 75 | void apply_dim_space([[maybe_unused]] int dim, UnaryPredicate_ p) const { 76 | p(pico_tree::one_space_s1{}); 77 | } 78 | }; 79 | 80 | void search_lp3_3() { 81 | using point = pico_tree::point_2f; 82 | using scalar = typename point::scalar_type; 83 | 84 | pico_tree::max_leaf_size_t max_leaf_size = 12; 85 | std::size_t point_count = 1024 * 1024; 86 | scalar area_size = 10; 87 | 88 | using kd_tree = pico_tree::kd_tree, metric_lp_p<3>>; 89 | using neighbor = typename kd_tree::neighbor_type; 90 | 91 | kd_tree tree( 92 | pico_tree::generate_random_n(point_count, area_size), 93 | max_leaf_size); 94 | 95 | neighbor nn; 96 | tree.search_nn(point{area_size / scalar(2), area_size / scalar(2)}, nn); 97 | 98 | std::cout << "Index closest point: " << nn.index << std::endl; 99 | } 100 | 101 | void search_t2() { 102 | using point = pico_tree::point_2f; 103 | using scalar = typename point::scalar_type; 104 | 105 | pico_tree::max_leaf_size_t max_leaf_size = 12; 106 | std::size_t point_count = 1024 * 1024; 107 | scalar area_size = 1; 108 | 109 | using kd_tree = pico_tree::kd_tree, metric_t2_squared>; 110 | using neighbor = typename kd_tree::neighbor_type; 111 | 112 | kd_tree tree( 113 | pico_tree::generate_random_n(point_count, area_size), 114 | max_leaf_size); 115 | 116 | std::array knn; 117 | tree.search_knn(point{area_size, area_size}, knn.begin(), knn.end()); 118 | 119 | // These prints show that wrapping near values 0 ~ 1 is supported. 120 | std::cout << "Closest points (index, distance, point): " << std::endl; 121 | for (auto const& nn : knn) { 122 | std::cout << " " << nn.index << ", " << nn.distance << ", [" 123 | << tree.space()[static_cast(nn.index)] << "]" 124 | << std::endl; 125 | } 126 | } 127 | 128 | int main() { 129 | search_lp3_3(); 130 | search_t2(); 131 | return 0; 132 | } 133 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_custom_point_type.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // This example shows how to write a traits class for a custom point type. 6 | 7 | // PointXYZ is the custom point type for which we write a simple traits class. 8 | struct PointXYZ { 9 | float data[3]; 10 | }; 11 | 12 | // A specialization of point_traits must be defined within the pico_tree 13 | // namespace and provide all the details of this example. 14 | namespace pico_tree { 15 | 16 | template <> 17 | struct point_traits { 18 | using point_type = PointXYZ; 19 | using scalar_type = float; 20 | // Spatial dimension. Set to pico_tree::dynamic_extent when the dimension is 21 | // only known at run-time. 22 | static constexpr pico_tree::size_t dim = 3; 23 | 24 | // Returns a pointer to the coordinates of the input point. 25 | inline static float const* data(PointXYZ const& point) { return point.data; } 26 | 27 | // Returns the number of coordinates or spatial dimension of each point. 28 | inline static constexpr pico_tree::size_t size(PointXYZ const&) { 29 | return dim; 30 | } 31 | }; 32 | 33 | } // namespace pico_tree 34 | 35 | int main() { 36 | std::vector points{{0.0f, 1.0f, 2.0f}, {3.0f, 4.0f, 5.0f}}; 37 | 38 | pico_tree::kd_tree>> tree( 39 | points, pico_tree::max_leaf_size_t(12)); 40 | 41 | PointXYZ query{4.0f, 4.0f, 4.0f}; 42 | pico_tree::neighbor nn; 43 | tree.search_nn(query, nn); 44 | 45 | std::cout << "Index closest point: " << nn.index << std::endl; 46 | 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_custom_search_visitor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // Search visitor that counts how many points were considered as a possible 8 | // nearest neighbor. 9 | template 10 | class search_nn_counter { 11 | public: 12 | using neighbor_type = Neighbor_; 13 | using index_type = typename Neighbor_::index_type; 14 | using scalar_type = typename Neighbor_::scalar_type; 15 | 16 | // Create a visitor for approximate nearest neighbor searching. The argument 17 | // is the search result. 18 | inline search_nn_counter(neighbor_type& nn) : count_(0), nn_(nn) { 19 | // Initial search distance. 20 | nn_.distance = std::numeric_limits::max(); 21 | } 22 | 23 | // Visit current point. This method is required. The search algorithm calls 24 | // this function for every point it encounters in the kd_tree. The arguments 25 | // of the method are respectively the index and distance of the visited point. 26 | inline void operator()(index_type const idx, scalar_type const dst) { 27 | // Only update the nearest neighbor when the point we visit is actually 28 | // closer to the query point. 29 | if (max() > dst) { 30 | nn_ = {idx, dst}; 31 | } 32 | count_++; 33 | } 34 | 35 | // Maximum search distance with respect to the query point. This method is 36 | // required. The nodes of the kd_tree are filtered using this method. 37 | inline scalar_type const& max() const { return nn_.distance; } 38 | 39 | // The amount of points visited during a query. 40 | inline index_type const& count() const { return count_; } 41 | 42 | private: 43 | index_type count_; 44 | neighbor_type& nn_; 45 | }; 46 | 47 | int main() { 48 | using point = pico_tree::point_2f; 49 | using scalar = typename point::scalar_type; 50 | 51 | pico_tree::max_leaf_size_t max_leaf_size = 12; 52 | std::size_t point_count = 1024 * 1024; 53 | scalar area_size = 1000; 54 | 55 | using kd_tree = pico_tree::kd_tree>; 56 | using neighbor = typename kd_tree::neighbor_type; 57 | 58 | kd_tree tree( 59 | pico_tree::generate_random_n(point_count, area_size), 60 | max_leaf_size); 61 | 62 | point q{area_size / scalar(2.0), area_size / scalar(2.0)}; 63 | neighbor nn; 64 | search_nn_counter v(nn); 65 | tree.search_nearest(q, v); 66 | 67 | std::cout << "Number of points visited: " << v.count() << std::endl; 68 | 69 | return 0; 70 | } 71 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_custom_space_type.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // This example shows how to write a traits class for a custom space type (point 7 | // set type). 8 | 9 | namespace pico_tree { 10 | 11 | // Provides an interface for an std::deque. 12 | template 13 | struct space_traits>> { 14 | using space_type = std::deque>; 15 | using point_type = std::array; 16 | using scalar_type = Scalar_; 17 | // Spatial dimension. Set to pico_tree::dynamic_extent when the dimension is 18 | // only known at run-time. 19 | static constexpr pico_tree::size_t dim = Dim_; 20 | 21 | // Returns a point from the input space at the specified index. 22 | template 23 | inline static point_type const& point_at( 24 | space_type const& space, Index_ const index) { 25 | return space[static_cast(index)]; 26 | } 27 | 28 | // Returns number of points contained by the space. 29 | inline static pico_tree::size_t size(space_type const& space) { 30 | return space.size(); 31 | } 32 | 33 | // Returns the number of coordinates or spatial dimension of each point. 34 | inline static constexpr pico_tree::size_t sdim(space_type const&) { 35 | return dim; 36 | } 37 | }; 38 | 39 | } // namespace pico_tree 40 | 41 | int main() { 42 | std::deque> points{ 43 | {0.0f, 1.0f}, {2.0f, 3.0f}, {4.0f, 5.0f}}; 44 | 45 | pico_tree::kd_tree>>> 46 | tree(points, pico_tree::max_leaf_size_t(1)); 47 | 48 | std::array query{4.0f, 4.0f}; 49 | pico_tree::neighbor nn; 50 | tree.search_nn(query, nn); 51 | 52 | std::cout << "Index closest point: " << nn.index << std::endl; 53 | 54 | return 0; 55 | } 56 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_dynamic_arrays.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // This example shows how to work with dynamic size arrays. Support is provided 8 | // for working with an array of scalars or an array of points. 9 | 10 | void array_of_scalars() { 11 | std::size_t count = 6; 12 | constexpr std::size_t dim = 2; 13 | 14 | // Here we create an array of scalars that will be interpreted as a set of 15 | // points: {{0, 1}, {2, 3}, ...} 16 | std::unique_ptr data = std::make_unique(count * dim); 17 | for (std::size_t i = 0; i < (count * dim); ++i) { 18 | data[i] = static_cast(i); 19 | } 20 | 21 | // If dim equals pico_tree::dynamic_extent, then space_map will need a 3rd 22 | // argument: The spatial dimension known at run time. 23 | pico_tree::space_map> map( 24 | data.get(), count); 25 | 26 | pico_tree::kd_tree>> 27 | tree(map, pico_tree::max_leaf_size_t(3)); 28 | 29 | // If dim equals pico_tree::dynamic_extent, then point_map will need a 2nd 30 | // argument: The spatial dimension known at run time. 31 | std::size_t index = 2; 32 | pico_tree::point_map query(data.get() + index * dim); 33 | pico_tree::neighbor nn; 34 | tree.search_nn(query, nn); 35 | 36 | // Prints index 2. 37 | std::cout << "Index closest point: " << nn.index << std::endl; 38 | } 39 | 40 | void array_of_points() { 41 | std::size_t count = 6; 42 | constexpr std::size_t dim = 2; 43 | 44 | // Here we create an array of points: {{0, 1}, {2, 3}, ...} 45 | std::unique_ptr[]> data = 46 | std::make_unique[]>(count); 47 | for (std::size_t i = 0; i < count; ++i) { 48 | for (std::size_t j = 0; j < dim; ++j) { 49 | data[i][j] = static_cast(i * dim + j); 50 | } 51 | } 52 | 53 | pico_tree::space_map> map(data.get(), count); 54 | 55 | pico_tree::kd_tree>> tree( 56 | map, pico_tree::max_leaf_size_t(3)); 57 | 58 | std::size_t index = 1; 59 | std::array const& query = data[index]; 60 | pico_tree::neighbor nn; 61 | tree.search_nn(query, nn); 62 | 63 | // Prints index 1. 64 | std::cout << "Index closest point: " << nn.index << std::endl; 65 | } 66 | 67 | int main() { 68 | array_of_scalars(); 69 | array_of_points(); 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_minimal.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // Provides support for fixed size arrays and std::array. 3 | #include 4 | #include 5 | // Provides support for std::vector. 6 | #include 7 | 8 | // This example shows how to create and query a kd_tree. An std::vector can be 9 | // used for storing points and a point can be either an array or an std::array. 10 | 11 | int main() { 12 | pico_tree::max_leaf_size_t max_leaf_size = 12; 13 | std::vector> points{ 14 | {0.0f, 1.0f, 2.0f}, {3.0f, 4.0f, 5.0f}}; 15 | 16 | // The kd_tree takes the input by value. To prevent a copy, we can either move 17 | // the point set into the tree or the point set can be taken by reference by 18 | // wrapping it in an std::reference_wrapper. Below we take the input by 19 | // reference: 20 | pico_tree::kd_tree tree(std::ref(points), max_leaf_size); 21 | 22 | float query[3] = {4.0f, 4.0f, 4.0f}; 23 | pico_tree::neighbor nn; 24 | tree.search_nn(query, nn); 25 | 26 | std::cout << "Index closest point: " << nn.index << std::endl; 27 | 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_save_and_load.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // This example shows how to save and load a kd_tree to and from a file. Saving 8 | // and loading the kd_tree does not include saving and loading the point set. 9 | 10 | // A kd_tree is stored in a binary format that is architecture dependent. As 11 | // such, saving and loading a file will fail when the file is exchanged between 12 | // machines with different endianness, etc. 13 | 14 | int main() { 15 | std::vector> points{ 16 | {0.0f, 1.0f, 2.0f}, {3.0f, 4.0f, 5.0f}}; 17 | 18 | using kd_tree = pico_tree::kd_tree< 19 | std::reference_wrapper>>>; 20 | 21 | std::string filename = "tree.bin"; 22 | 23 | // Save to file. 24 | kd_tree::save(kd_tree(points, pico_tree::max_leaf_size_t(12)), filename); 25 | 26 | // Load from file. 27 | auto tree = kd_tree::load(points, filename); 28 | std::filesystem::remove(filename); 29 | 30 | float query[3] = {4.0f, 4.0f, 4.0f}; 31 | pico_tree::neighbor nn; 32 | tree.search_nn(query, nn); 33 | 34 | std::cout << "Index closest point: " << nn.index << std::endl; 35 | 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /examples/kd_tree/kd_tree_search.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | // Different search options. 7 | void search_r3() { 8 | using point = pico_tree::point_3f; 9 | using scalar = typename point::scalar_type; 10 | 11 | std::size_t run_count = 1024 * 1024; 12 | pico_tree::max_leaf_size_t max_leaf_size = 12; 13 | std::size_t point_count = 1024 * 1024; 14 | scalar area_size = 1000; 15 | 16 | using kd_tree = pico_tree::kd_tree>; 17 | 18 | kd_tree tree( 19 | pico_tree::generate_random_n(point_count, area_size), 20 | max_leaf_size); 21 | 22 | scalar min_v = 25.1f; 23 | scalar max_v = 37.9f; 24 | point min, max, q; 25 | min.fill(min_v); 26 | max.fill(max_v); 27 | q.fill((max_v + min_v) / 2.0f); 28 | 29 | scalar search_radius = 2.0f; 30 | // When building kd_tree with default template arguments, this is the squared 31 | // distance. 32 | scalar search_radius_metric = tree.metric()(search_radius); 33 | // The ann can not be further away than a factor of (1 + max_error_percentage) 34 | // from the real nn. 35 | scalar max_error_percentage = 0.2f; 36 | // Apply the metric to the max ratio difference. 37 | scalar max_error_ratio_metric = tree.metric()(1.0f + max_error_percentage); 38 | 39 | using neighbor = typename kd_tree::neighbor_type; 40 | using index = typename neighbor::index_type; 41 | 42 | neighbor nn; 43 | std::vector knn; 44 | std::vector idxs; 45 | 46 | { 47 | pico_tree::scoped_timer t("kd_tree nn, radius and box", run_count); 48 | for (std::size_t i = 0; i < run_count; ++i) { 49 | tree.search_nn(q, nn); 50 | tree.search_knn(q, 1, knn); 51 | tree.search_radius(q, search_radius_metric, knn, false); 52 | tree.search_box(min, max, idxs); 53 | } 54 | } 55 | 56 | std::size_t k = 8; 57 | 58 | { 59 | pico_tree::scoped_timer t("kd_tree aknn", run_count); 60 | for (std::size_t i = 0; i < run_count; ++i) { 61 | // When the kd_tree is created with the sliding_midpoint_max_side splitter 62 | // rule (the default argument), approximate nn queries can be answered in 63 | // O(1/e^d log n) time. 64 | tree.search_knn(q, k, max_error_ratio_metric, knn); 65 | } 66 | } 67 | 68 | { 69 | pico_tree::scoped_timer t("kd_tree knn", run_count); 70 | for (std::size_t i = 0; i < run_count; ++i) { 71 | tree.search_knn(q, k, knn); 72 | } 73 | } 74 | } 75 | 76 | // This example shows how to search on the unit circle. Point coordinates must 77 | // lie within the range of [0...1]. Point coordinates wrap at 0 or 1. A point 78 | // with a coordinate value of 0 is considered the same as one with a coordinate 79 | // value of 1. 80 | void search_s1() { 81 | using point = pico_tree::point_1f; 82 | using scalar = typename pico_tree::point_1f::scalar_type; 83 | using space = std::vector; 84 | using neighbor = 85 | pico_tree::kd_tree::neighbor_type; 86 | 87 | pico_tree::kd_tree tree( 88 | pico_tree::generate_random_n(512, scalar(0.0), scalar(1.0)), 89 | pico_tree::max_leaf_size_t(10)); 90 | 91 | std::array knn; 92 | tree.search_knn(point{1.0}, knn.begin(), knn.end()); 93 | 94 | // These prints show that wrapping near values 0 ~ 1 is supported. 95 | std::cout << "Closest points (index, distance, point): " << std::endl; 96 | for (auto const& nn : knn) { 97 | std::cout << " " << nn.index << ", " << nn.distance << ", [" 98 | << tree.space()[static_cast(nn.index)] << "]" 99 | << std::endl; 100 | } 101 | } 102 | 103 | int main() { 104 | search_r3(); 105 | search_s1(); 106 | return 0; 107 | } 108 | -------------------------------------------------------------------------------- /examples/opencv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_executable(opencv opencv.cpp) 2 | set_default_target_properties(opencv) 3 | target_link_libraries(opencv PUBLIC pico_toolshed ${OpenCV_LIBS}) 4 | -------------------------------------------------------------------------------- /examples/opencv/opencv.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | std::size_t const num_points = 1024 * 1024 * 2; 8 | float const point_area = 1000.0; 9 | std::size_t const run_count = 1024 * 1024; 10 | 11 | template 12 | std::vector generate_random_vec_n( 13 | std::size_t n, typename Vec_::value_type size) { 14 | std::random_device rd; 15 | std::mt19937 e2(rd()); 16 | std::uniform_real_distribution dist(0, size); 17 | 18 | std::vector random(n); 19 | for (auto& p : random) { 20 | for (auto& c : p.val) { 21 | c = dist(e2); 22 | } 23 | } 24 | 25 | return random; 26 | } 27 | 28 | // This example shows to build a kd_tree from a vector of cv::Point3. 29 | void basic_vector() { 30 | using index = int; 31 | using scalar = float; 32 | using point = cv::Vec; 33 | std::vector random = 34 | generate_random_vec_n(num_points, point_area); 35 | 36 | pico_tree::kd_tree tree(std::cref(random), pico_tree::max_leaf_size_t(10)); 37 | 38 | auto p = random[random.size() / 2]; 39 | 40 | pico_tree::neighbor nn; 41 | pico_tree::scoped_timer t("pico_tree cv vector", run_count); 42 | for (std::size_t i = 0; i < run_count; ++i) { 43 | tree.search_nn(p, nn); 44 | } 45 | } 46 | 47 | // This example shows to build a kd_tree using a cv::Mat. 48 | void basic_matrix() { 49 | using index = int; 50 | using scalar = float; 51 | 52 | // Multiple columns based on the number of coordinates in a point. 53 | { 54 | constexpr int dim = 3; 55 | cv::Mat random(num_points, dim, cv::DataType::type); 56 | cv::randu(random, scalar(0.0), point_area); 57 | 58 | pico_tree::kd_tree> tree( 59 | random, pico_tree::max_leaf_size_t(10)); 60 | pico_tree::point_map p = tree.space()[tree.space().size() / 2]; 61 | 62 | pico_tree::neighbor nn; 63 | pico_tree::scoped_timer t("pico_tree cv mat", run_count); 64 | for (std::size_t i = 0; i < run_count; ++i) { 65 | tree.search_nn(p, nn); 66 | } 67 | } 68 | 69 | // Single column cv::Mat based on a vector of points. 70 | { 71 | constexpr int dim = 3; 72 | using point = cv::Vec; 73 | std::vector random = 74 | generate_random_vec_n(num_points, point_area); 75 | 76 | pico_tree::kd_tree> tree( 77 | cv::Mat(random), pico_tree::max_leaf_size_t(10)); 78 | 79 | point p = random[random.size() / 2]; 80 | 81 | pico_tree::neighbor nn; 82 | pico_tree::scoped_timer t("pico_tree cv mat", run_count); 83 | for (std::size_t i = 0; i < run_count; ++i) { 84 | tree.search_nn(p, nn); 85 | } 86 | } 87 | } 88 | 89 | int main() { 90 | basic_vector(); 91 | basic_matrix(); 92 | return 0; 93 | } 94 | -------------------------------------------------------------------------------- /examples/pico_toolshed/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(pico_toolshed INTERFACE) 2 | target_include_directories(pico_toolshed INTERFACE ${CMAKE_CURRENT_LIST_DIR}) 3 | target_link_libraries(pico_toolshed INTERFACE PicoTree::PicoTree) 4 | target_sources(pico_toolshed 5 | INTERFACE 6 | ${CMAKE_CURRENT_LIST_DIR}/pico_toolshed/dynamic_space.hpp 7 | ${CMAKE_CURRENT_LIST_DIR}/pico_toolshed/point.hpp 8 | ${CMAKE_CURRENT_LIST_DIR}/pico_toolshed/scoped_timer.hpp 9 | ) 10 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/dynamic_space.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace pico_tree { 8 | 9 | namespace internal { 10 | 11 | template 12 | class dynamic_space_base { 13 | public: 14 | using size_type = pico_tree::size_t; 15 | 16 | inline explicit dynamic_space_base(Space_ space) 17 | : space_(std::move(space)), 18 | sdim_(pico_tree::space_traits::sdim(space_)) {} 19 | 20 | inline size_type sdim() const { return sdim_; } 21 | 22 | protected: 23 | Space_ space_; 24 | size_type sdim_; 25 | }; 26 | 27 | } // namespace internal 28 | 29 | //! 30 | template 31 | class dynamic_space : protected internal::dynamic_space_base { 32 | public: 33 | using internal::dynamic_space_base::dynamic_space_base; 34 | using internal::dynamic_space_base::sdim; 35 | using internal::dynamic_space_base::space_; 36 | 37 | inline operator Space_ const&() const { return space_; } 38 | inline operator Space_&() { return space_; } 39 | }; 40 | 41 | template 42 | class dynamic_space> 43 | : protected internal::dynamic_space_base> { 44 | public: 45 | using internal::dynamic_space_base< 46 | std::reference_wrapper>::dynamic_space_base; 47 | using internal::dynamic_space_base>::sdim; 48 | using internal::dynamic_space_base>::space_; 49 | 50 | inline operator Space_ const&() const { return space_; } 51 | inline operator Space_&() { return space_; } 52 | }; 53 | 54 | template 55 | struct space_traits> : public space_traits { 56 | using space_type = dynamic_space; 57 | using size_type = pico_tree::size_t; 58 | static size_type constexpr dim = pico_tree::dynamic_extent; 59 | 60 | inline static size_type sdim(space_type const& space) { return space.sdim(); } 61 | }; 62 | 63 | } // namespace pico_tree 64 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/format/endian.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace pico_tree::internal { 7 | 8 | template 9 | T swap_endian(T const& u) { 10 | T s; 11 | 12 | std::byte const* up = reinterpret_cast(&u); 13 | std::byte* sp = reinterpret_cast(&s); 14 | for (std::size_t i = 0; i < sizeof(T); ++i) { 15 | sp[i] = up[sizeof(T) - i - 1]; 16 | } 17 | 18 | return s; 19 | } 20 | 21 | template 22 | T_ big_endian_to_native(T_ v) { 23 | static_assert(std::is_integral_v, "NOT_AN_INTEGRAL_TYPE"); 24 | static_assert(sizeof(T_) <= sizeof(std::size_t), "SIZE_UNSUPPORTED"); 25 | 26 | std::byte* v_object = reinterpret_cast(&v); 27 | 28 | T_ native{}; 29 | for (std::size_t i = 0; i < sizeof(T_); ++i) { 30 | native |= static_cast( 31 | static_cast(v_object[i]) << ((sizeof(T_) - 1 - i) * 8)); 32 | } 33 | 34 | return native; 35 | } 36 | 37 | //! \brief Stores the value of an integral type assuming big endian byte 38 | //! ordering. 39 | //! \details The idea is to be oblivious to the native endianness. When a big 40 | //! endian value is read it can be converted to its native counterpart. If the 41 | //! native endianness equals big endian then we're basically making a very 42 | //! elaborate copy. However, use of this class should be quite minimal. 43 | template 44 | struct big_endian { 45 | static_assert(std::is_integral_v, "NOT_AN_INTEGRAL_TYPE"); 46 | 47 | T_ operator()() const { return big_endian_to_native(value); } 48 | 49 | operator T_() const { return this->operator()(); } 50 | 51 | T_ value; 52 | }; 53 | 54 | } // namespace pico_tree::internal 55 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/format/format_ascii.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace pico_tree { 7 | 8 | template 9 | void write_ascii(std::string const& filename, std::vector const& v) { 10 | if (v.empty()) { 11 | return; 12 | } 13 | 14 | std::fstream stream = internal::open_stream(filename, std::ios::out); 15 | stream << std::setprecision(10); 16 | for (auto const& p : v) { 17 | stream << p << '\n'; 18 | } 19 | } 20 | 21 | } // namespace pico_tree 22 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/format/format_bin.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace pico_tree { 7 | 8 | template 9 | void write_bin(std::string const& filename, std::vector const& v) { 10 | if (v.empty()) { 11 | return; 12 | } 13 | 14 | std::fstream stream = 15 | internal::open_stream(filename, std::ios::out | std::ios::binary); 16 | internal::stream_wrapper wrapper(stream); 17 | wrapper.write(v.data(), v.size()); 18 | } 19 | 20 | template 21 | void read_bin(std::string const& filename, std::vector& v) { 22 | std::fstream stream = 23 | internal::open_stream(filename, std::ios::in | std::ios::binary); 24 | internal::stream_wrapper wrapper(stream); 25 | 26 | auto bytes = std::filesystem::file_size(filename); 27 | std::size_t const element_size = sizeof(T_); 28 | std::size_t const element_count = 29 | static_cast(bytes) / element_size; 30 | v.resize(element_count); 31 | wrapper.read(element_count, v.data()); 32 | } 33 | 34 | } // namespace pico_tree 35 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/format/format_mnist.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "endian.hpp" 8 | 9 | // http://yann.lecun.com/exdb/mnist/ 10 | 11 | namespace pico_tree { 12 | 13 | namespace internal { 14 | 15 | struct mnist_images { 16 | struct header { 17 | static constexpr std::int32_t magic_number = 2051; 18 | std::int32_t image_count; 19 | std::int32_t image_width; 20 | std::int32_t image_height; 21 | }; 22 | }; 23 | 24 | struct mnist_labels { 25 | struct header { 26 | static constexpr std::int32_t magic_number = 2049; 27 | std::int32_t label_count; 28 | }; 29 | }; 30 | 31 | } // namespace internal 32 | 33 | inline void read_mnist_images( 34 | std::string const& filename, 35 | std::vector>& images) { 36 | using namespace internal; 37 | 38 | std::fstream stream = open_stream(filename, std::ios::in | std::ios::binary); 39 | stream_wrapper wrapper(stream); 40 | 41 | big_endian magic_number; 42 | wrapper.read(magic_number); 43 | if (magic_number() != mnist_images::header::magic_number) { 44 | throw std::runtime_error("expected mnist images signature"); 45 | } 46 | 47 | mnist_images::header header; 48 | wrapper.read(header); 49 | header.image_count = big_endian{header.image_count}; 50 | header.image_width = big_endian{header.image_width}; 51 | header.image_height = big_endian{header.image_height}; 52 | 53 | if (header.image_width * header.image_height != 28 * 28) { 54 | throw std::runtime_error("unexpected mnist image dimensions"); 55 | } 56 | 57 | images.resize(static_cast(header.image_count)); 58 | wrapper.read(images.size(), images.data()); 59 | } 60 | 61 | inline void read_mnist_labels( 62 | std::string const& filename, std::vector& labels) { 63 | using namespace internal; 64 | 65 | std::fstream stream = open_stream(filename, std::ios::in | std::ios::binary); 66 | stream_wrapper wrapper(stream); 67 | 68 | big_endian magic_number; 69 | wrapper.read(magic_number); 70 | if (magic_number() != mnist_labels::header::magic_number) { 71 | throw std::runtime_error("unexpected minst labels magic number."); 72 | } 73 | 74 | mnist_labels::header header; 75 | wrapper.read(header); 76 | header.label_count = big_endian{header.label_count}; 77 | 78 | labels.resize(static_cast(header.label_count)); 79 | wrapper.read(labels.size(), labels.data()); 80 | } 81 | 82 | } // namespace pico_tree 83 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/format/format_uosr.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace pico_tree { 15 | 16 | namespace internal { 17 | 18 | constexpr auto string_scan = "scan"; 19 | constexpr auto string_pose = ".pose"; 20 | constexpr auto string_3d = ".3d"; 21 | constexpr double deg_to_rad = 3.1415926535897932384626433832795 / 180.0; 22 | 23 | // Transformation that preserves distances between points. Column major. 24 | class isometry { 25 | public: 26 | isometry(std::array const& R, std::array const& t) 27 | : R_{R}, t_{t} {} 28 | 29 | static isometry from_euler( 30 | std::array const& e, std::array const& t) { 31 | double sx = std::sin(e[0]); 32 | double cx = std::cos(e[0]); 33 | double sy = std::sin(e[1]); 34 | double cy = std::cos(e[1]); 35 | double sz = std::sin(e[2]); 36 | double cz = std::cos(e[2]); 37 | 38 | std::array R; 39 | R[0] = cy * cz; 40 | R[1] = sx * sy * cz + cx * sz; 41 | R[2] = -cx * sy * cz + sx * sz; 42 | R[3] = -cy * sz; 43 | R[4] = -sx * sy * sz + cx * cz; 44 | R[5] = cx * sy * sz + sx * cz; 45 | R[6] = sy; 46 | R[7] = -sx * cy; 47 | R[8] = cx * cy; 48 | 49 | return isometry(R, t); 50 | } 51 | 52 | inline void transform(point_3d& p) const { 53 | double rx = p[0] * R_[0] + p[1] * R_[3] + p[2] * R_[6]; 54 | double ry = p[0] * R_[1] + p[1] * R_[4] + p[2] * R_[7]; 55 | double rz = p[0] * R_[2] + p[1] * R_[5] + p[2] * R_[8]; 56 | p[0] = rx + t_[0]; 57 | p[1] = ry + t_[1]; 58 | p[2] = rz + t_[2]; 59 | } 60 | 61 | private: 62 | std::array const R_; 63 | std::array const t_; 64 | }; 65 | 66 | class uosr_scan_reader { 67 | public: 68 | uosr_scan_reader(std::filesystem::path const& path_3d) 69 | : pose_(read_pose( 70 | std::filesystem::path(path_3d).replace_extension(string_pose))), 71 | stream_(internal::open_stream(path_3d.string(), std::ios::in)) {} 72 | 73 | inline bool read_next(point_3d& point) { 74 | std::string line; 75 | 76 | if (!std::getline(stream_, line)) { 77 | return false; 78 | } 79 | 80 | std::string value; 81 | char* end; 82 | std::stringstream ss(line); 83 | ss >> value; 84 | point[0] = std::strtod(value.c_str(), &end); 85 | ss >> value; 86 | point[1] = std::strtod(value.c_str(), &end); 87 | ss >> value; 88 | point[2] = std::strtod(value.c_str(), &end); 89 | // Fourth value would be the reflectance. 90 | // ss >> value; 91 | // float val = std::strtof(value.c_str(), &end); 92 | 93 | pose_.transform(point); 94 | 95 | return true; 96 | } 97 | 98 | private: 99 | isometry read_pose(std::filesystem::path const& path) { 100 | std::fstream stream = internal::open_stream(path.string(), std::ios::in); 101 | std::array e, t; 102 | stream >> t[0] >> t[1] >> t[2] >> e[0] >> e[1] >> e[2]; 103 | 104 | e[0] *= deg_to_rad; 105 | e[1] *= deg_to_rad; 106 | e[2] *= deg_to_rad; 107 | 108 | return isometry::from_euler(e, t); 109 | } 110 | 111 | isometry const pose_; 112 | std::fstream stream_; 113 | }; 114 | 115 | } // namespace internal 116 | 117 | template 118 | inline void read_uosr( 119 | std::string const& root, std::vector>& points) { 120 | if (!std::filesystem::exists(root)) { 121 | std::cout << "Path doesn't exist: " << root << std::endl; 122 | std::exit(EXIT_FAILURE); 123 | } 124 | 125 | if (!std::filesystem::is_directory(root)) { 126 | std::cout << "Path isn't a directory: " << root << std::endl; 127 | std::exit(EXIT_FAILURE); 128 | } 129 | 130 | // This doesn't test if the corresponding .pose exists, that is handled by the 131 | // uosr_scan_reader. 132 | std::regex pattern( 133 | std::string("^") + internal::string_scan + "\\d{3}" + 134 | internal::string_3d + "$"); 135 | std::vector paths; 136 | for (auto const& entry : std::filesystem::directory_iterator(root)) { 137 | if (std::filesystem::is_regular_file(entry) && 138 | std::regex_search(entry.path().filename().string(), pattern)) { 139 | paths.push_back(entry.path()); 140 | } 141 | } 142 | 143 | point_3d point; 144 | for (auto const& path : paths) { 145 | // Printing the string doesn't show escape characters. 146 | std::cout << "Reading from scan: " << path.string() << std::endl; 147 | internal::uosr_scan_reader reader(path); 148 | while (reader.read_next(point)) { 149 | points.push_back(point.cast()); 150 | } 151 | } 152 | } 153 | 154 | } // namespace pico_tree 155 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/format/format_xvecs.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | // http://corpus-texmex.irisa.fr/ 9 | 10 | namespace pico_tree { 11 | 12 | namespace internal { 13 | 14 | template 15 | inline std::string format_string() { 16 | static_assert( 17 | std::is_same_v || std::is_same_v || 18 | std::is_same_v || std::is_same_v, 19 | "TYPE_NOT_ONE_OF_UNSIGNED_CHAR_FLOAT_OR_INT"); 20 | 21 | if constexpr ( 22 | std::is_same_v || std::is_same_v) { 23 | return "bvecs"; 24 | } else if constexpr (std::is_same_v) { 25 | return "fvecs"; 26 | } else { // if constexpr (std::is_same_v) 27 | return "ivecs"; 28 | } 29 | } 30 | 31 | inline std::string to_lower(std::string s) { 32 | std::transform(s.begin(), s.end(), s.begin(), [](auto c) { 33 | return std::tolower(c, std::locale()); 34 | }); 35 | return s; 36 | } 37 | 38 | } // namespace internal 39 | 40 | template 41 | void read_xvecs( 42 | std::string const& filename, std::vector>& v) { 43 | auto format_string = internal::format_string(); 44 | auto filename_lower = internal::to_lower(filename); 45 | 46 | if (filename_lower.compare( 47 | filename_lower.size() - format_string.size(), 48 | format_string.size(), 49 | format_string) != 0) { 50 | throw std::runtime_error("filename expected to end with ." + format_string); 51 | } 52 | 53 | std::fstream fstream = 54 | internal::open_stream(filename, std::ios::in | std::ios::binary); 55 | internal::stream_wrapper stream(fstream); 56 | 57 | auto bytes = std::filesystem::file_size(filename); 58 | std::size_t const element_size = sizeof(T_); 59 | std::size_t const row_size = sizeof(int) + element_size * N_; 60 | std::size_t const row_count = static_cast(bytes) / row_size; 61 | v.resize(row_count); 62 | for (auto& r : v) { 63 | int coords; 64 | stream.read(coords); 65 | stream.read(r); 66 | } 67 | } 68 | 69 | } // namespace pico_tree 70 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/point.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace pico_tree { 10 | 11 | template 12 | struct point : public pico_tree::internal::point { 13 | // Dynamic capability disabled. 14 | static_assert( 15 | Dim_ != pico_tree::dynamic_extent && Dim_ > 0, 16 | "DIM_MUST_NOT_BE_DYNAMIC_AND_>_0"); 17 | 18 | using pico_tree::internal::point::elems_; 19 | using pico_tree::internal::point::size; 20 | using pico_tree::internal::point::fill; 21 | using pico_tree::internal::point::normalize; 22 | using typename pico_tree::internal::point::scalar_type; 23 | using typename pico_tree::internal::point::size_type; 24 | 25 | inline point& operator+=(scalar_type const v) { 26 | for (size_type i = 0; i < size(); ++i) { 27 | elems_[i] += v; 28 | } 29 | return *this; 30 | } 31 | 32 | inline point operator+(scalar_type const v) const { 33 | point p = *this; 34 | p += v; 35 | return p; 36 | } 37 | 38 | inline point& operator-=(scalar_type const v) { 39 | for (size_type i = 0; i < size(); ++i) { 40 | elems_[i] -= v; 41 | } 42 | return *this; 43 | } 44 | 45 | inline point operator-(scalar_type const v) const { 46 | point p = *this; 47 | p -= v; 48 | return p; 49 | } 50 | 51 | template 52 | inline point cast() const { 53 | point other; 54 | for (size_type i = 0; i < size(); ++i) { 55 | other.elems_[i] = static_cast(elems_[i]); 56 | } 57 | return other; 58 | } 59 | }; 60 | 61 | template 62 | struct point_traits> { 63 | using point_type = point; 64 | using scalar_type = Scalar_; 65 | static std::size_t constexpr dim = Dim_; 66 | 67 | inline static scalar_type const* data(point_type const& p) { 68 | return p.data(); 69 | } 70 | 71 | inline static std::size_t constexpr size(point_type const& p) { 72 | return p.size(); 73 | } 74 | }; 75 | 76 | template 77 | inline std::ostream& operator<<( 78 | std::ostream& s, point const& p) { 79 | s << p[0]; 80 | for (std::size_t i = 1; i < Dim_; ++i) { 81 | s << " " << p[i]; 82 | } 83 | return s; 84 | } 85 | 86 | using point_1f = point; 87 | using point_2f = point; 88 | using point_3f = point; 89 | using point_1d = point; 90 | using point_2d = point; 91 | using point_3d = point; 92 | 93 | // Generates n random points uniformly distributed between the box defined by 94 | // min and max. 95 | template 96 | inline std::vector generate_random_n( 97 | std::size_t n, 98 | typename Point_::scalar_type min, 99 | typename Point_::scalar_type max) { 100 | std::random_device rd; 101 | std::mt19937 e2(rd()); 102 | std::uniform_real_distribution dist(min, max); 103 | 104 | std::vector random(n); 105 | for (auto& p : random) { 106 | for (std::size_t i = 0; i < Point_::dim; ++i) { 107 | p[i] = dist(e2); 108 | } 109 | } 110 | 111 | return random; 112 | } 113 | 114 | // Generates n random points uniformly distributed over a box of size size. 115 | template 116 | inline std::vector generate_random_n( 117 | std::size_t n, typename Point_::scalar_type size) { 118 | return generate_random_n(n, typename Point_::scalar_type(0.0), size); 119 | } 120 | 121 | } // namespace pico_tree 122 | -------------------------------------------------------------------------------- /examples/pico_toolshed/pico_toolshed/scoped_timer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace pico_tree { 8 | 9 | // A simple timer that will print its life time to the standard out. 10 | class scoped_timer { 11 | public: 12 | scoped_timer(std::string const& name) 13 | : name_{name}, 14 | start_{std::chrono::high_resolution_clock::now()}, 15 | times_{1} {} 16 | 17 | scoped_timer(std::string const& name, std::size_t times) 18 | : name_{name}, 19 | start_{std::chrono::high_resolution_clock::now()}, 20 | times_{times} {} 21 | 22 | ~scoped_timer() { 23 | std::chrono::duration elapsed_seconds = 24 | std::chrono::high_resolution_clock::now() - start_; 25 | std::cout << "[" << name_ 26 | << "] Elapsed time: " << (elapsed_seconds.count() * 1000.0) 27 | << " ms\n"; 28 | 29 | if (times_ > 1) { 30 | std::cout << "[" << name_ << "] Average time: " 31 | << ((elapsed_seconds.count() / static_cast(times_)) * 32 | 1000.0) 33 | << " ms\n"; 34 | } 35 | } 36 | 37 | scoped_timer(scoped_timer const&) = delete; 38 | scoped_timer(scoped_timer&&) = delete; 39 | scoped_timer& operator=(scoped_timer const&) = delete; 40 | scoped_timer& operator=(scoped_timer&&) = delete; 41 | 42 | private: 43 | std::string name_; 44 | std::chrono::high_resolution_clock::time_point start_; 45 | std::size_t times_; 46 | }; 47 | 48 | } // namespace pico_tree 49 | -------------------------------------------------------------------------------- /examples/pico_understory/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(pico_understory INTERFACE) 2 | target_include_directories(pico_understory INTERFACE ${CMAKE_CURRENT_LIST_DIR}) 3 | target_link_libraries(pico_understory INTERFACE PicoTree::PicoTree) 4 | target_sources(pico_understory 5 | INTERFACE 6 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/cover_tree_base.hpp 7 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/cover_tree_builder.hpp 8 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/cover_tree_data.hpp 9 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/cover_tree_node.hpp 10 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/cover_tree_search.hpp 11 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/kd_tree_priority_search.hpp 12 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/matrix_space_traits.hpp 13 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/matrix_space.hpp 14 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/point_traits.hpp 15 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/rkd_tree_builder.hpp 16 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/rkd_tree_hh_data.hpp 17 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/internal/static_buffer.hpp 18 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/cover_tree.hpp 19 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/metric.hpp 20 | ${CMAKE_CURRENT_LIST_DIR}/pico_understory/kd_forest.hpp 21 | ) 22 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/cover_tree_base.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace pico_tree::internal { 4 | 5 | //! \brief This class contains (what we will call) the "leveling base" of the 6 | //! tree. 7 | //! \details It determines how fast the levels of the tree increase or 8 | //! decrease. When we raise "base" to the power of a certain natural number, 9 | //! that exponent represents the active level of the tree. 10 | //! 11 | //! The papers are written using a base of 2, but for performance reasons they 12 | //! use a base of 1.3. 13 | template 14 | struct base { 15 | template 16 | inline Scalar_ cover_distance(Node_ const& n) const { 17 | return std::pow(value, n.level); 18 | } 19 | 20 | //! Child distance is also the seperation distance. 21 | template 22 | inline Scalar_ child_distance(Node_ const& n) const { 23 | return std::pow(value, n.level - Scalar_(1.0)); 24 | } 25 | 26 | template 27 | inline Scalar_ parent_distance(Node_ const& n) const { 28 | return std::pow(value, n.level + Scalar_(1.0)); 29 | } 30 | 31 | inline Scalar_ level(Scalar_ const dst) const { 32 | return std::log(dst) / std::log(value); 33 | } 34 | 35 | Scalar_ value; 36 | }; 37 | 38 | } // namespace pico_tree::internal 39 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/cover_tree_data.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "cover_tree_node.hpp" 4 | #include "static_buffer.hpp" 5 | 6 | namespace pico_tree::internal { 7 | 8 | //! \brief The data structure that represents a cover_tree. 9 | template 10 | class cover_tree_data { 11 | public: 12 | using index_type = Index_; 13 | using scalar_type = Scalar_; 14 | using node_type = cover_tree_node; 15 | using node_allocator_type = internal::static_buffer; 16 | 17 | //! \brief Memory allocator for tree nodes. 18 | node_allocator_type allocator; 19 | //! \brief Root of the cover_tree. 20 | node_type* root_node; 21 | }; 22 | 23 | } // namespace pico_tree::internal 24 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/cover_tree_node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace pico_tree::internal { 6 | 7 | template 8 | struct cover_tree_node { 9 | using index_type = Index_; 10 | using scalar_type = Scalar_; 11 | 12 | inline bool is_branch() const { return !children.empty(); } 13 | inline bool is_leaf() const { return children.empty(); } 14 | 15 | // TODO Could be moved to the tree. 16 | scalar_type level; 17 | //! \brief Distance to the farthest child. 18 | scalar_type max_distance; 19 | index_type index; 20 | std::vector children; 21 | }; 22 | 23 | } // namespace pico_tree::internal 24 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/cover_tree_search.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "cover_tree_node.hpp" 6 | 7 | namespace pico_tree::internal { 8 | 9 | //! \brief This class provides a search nearest function for the cover_tree. 10 | template < 11 | typename SpaceWrapper_, 12 | typename Metric_, 13 | typename PointWrapper_, 14 | typename Visitor_, 15 | typename Index_> 16 | class search_nearest_metric { 17 | public: 18 | using index_type = Index_; 19 | using scalar_type = typename SpaceWrapper_::scalar_type; 20 | using point_type = point; 21 | using node_type = cover_tree_node; 22 | 23 | search_nearest_metric( 24 | SpaceWrapper_ space, 25 | Metric_ metric, 26 | PointWrapper_ query, 27 | Visitor_& visitor) 28 | : space_(space), metric_(metric), query_(query), visitor_(visitor) {} 29 | 30 | //! \brief Search nearest neighbors starting from \p node. 31 | inline void operator()(node_type const* const node) const { 32 | search_nearest(node); 33 | } 34 | 35 | private: 36 | inline void search_nearest(node_type const* const node) const { 37 | scalar_type const d = 38 | metric_(query_.begin(), query_.end(), space_[node->index]); 39 | if (visitor_.max() > d) { 40 | visitor_(node->index, d); 41 | } 42 | 43 | std::vector> sorted; 44 | sorted.reserve(node->children.size()); 45 | for (auto const child : node->children) { 46 | sorted.push_back( 47 | {child, metric_(query_.begin(), query_.end(), space_[child->index])}); 48 | } 49 | 50 | std::sort( 51 | sorted.begin(), 52 | sorted.end(), 53 | [](std::pair const& a, 54 | std::pair const& b) -> bool { 55 | return a.second < b.second; 56 | }); 57 | 58 | for (auto const& m : sorted) { 59 | // Algorithm 1 from paper "Faster Cover Trees" has a mistake. It checks 60 | // with respect to the nearest point, not the query point itself, 61 | // intersecting the wrong spheres. 62 | // Algorithm 1 from paper "Cover Trees for Nearest Neighbor" is correct. 63 | 64 | // The upper-bound distance a descendant can be is twice the cover 65 | // distance of the node. This is true taking the invariants into account. 66 | // NOTE: In "Cover Trees for Nearest Neighbor" this upper-bound 67 | // practically appears to be half this distance, as new nodes are only 68 | // added when they are within cover distance. 69 | // For "Faster Cover Trees" it is twice the cover distance due to the 70 | // first phase of the insert algorithm (not having a root at infinity). 71 | 72 | // TODO The distance calculation can be cached. When search_neighbor is 73 | // called it's calculated again. 74 | if (visitor_.max() > 75 | (metric_(query_.begin(), query_.end(), space_[m.first->index]) - 76 | m.first->max_distance)) { 77 | search_nearest(m.first); 78 | } 79 | } 80 | } 81 | 82 | SpaceWrapper_ space_; 83 | Metric_ metric_; 84 | PointWrapper_ query_; 85 | Visitor_& visitor_; 86 | }; 87 | 88 | } // namespace pico_tree::internal 89 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/kd_tree_priority_search.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "pico_tree/internal/kd_tree_node.hpp" 7 | #include "pico_tree/internal/point.hpp" 8 | #include "pico_tree/metric.hpp" 9 | 10 | namespace pico_tree::internal { 11 | 12 | //! \brief This class provides a search nearest function for Euclidean spaces. 13 | //! \details S. Arya and D. M. Mount, Algorithms for fast vector quantization, 14 | //! In IEEE Data Compression Conference, pp. 381–390, March 1993. 15 | //! https://www.cs.umd.edu/~mount/Papers/DCC.pdf 16 | //! This paper describes the "Priorty k-d Tree Search" technique to speed up 17 | //! nearest neighbor queries. 18 | template < 19 | typename SpaceWrapper_, 20 | typename Metric_, 21 | typename PointWrapper_, 22 | typename Visitor_, 23 | typename Index_> 24 | class priority_search_nearest_euclidean { 25 | public: 26 | using index_type = Index_; 27 | using scalar_type = typename SpaceWrapper_::scalar_type; 28 | using point_type = point; 29 | //! \brief Node type supported by this priority_search_nearest_euclidean. 30 | using node_type = kd_tree_node_topological; 31 | using queue_pair_type = std::pair; 32 | 33 | inline priority_search_nearest_euclidean( 34 | SpaceWrapper_ space, 35 | Metric_ metric, 36 | std::vector const& indices, 37 | PointWrapper_ query, 38 | size_t max_leaves_visited, 39 | Visitor_& visitor) 40 | : space_(space), 41 | metric_(metric), 42 | indices_(indices), 43 | query_(query), 44 | max_leaves_visited_(max_leaves_visited), 45 | visitor_(visitor) {} 46 | 47 | //! \brief Search nearest neighbors starting from \p root_node. 48 | inline void operator()(node_type const* const root_node) { 49 | std::size_t leaves_visited = 0; 50 | queue_.emplace(scalar_type(0.0), root_node); 51 | while (!queue_.empty()) { 52 | auto const [node_box_distance, node] = queue_.top(); 53 | 54 | if (leaves_visited >= max_leaves_visited_ || 55 | visitor_.max() < node_box_distance) { 56 | break; 57 | } 58 | 59 | queue_.pop(); 60 | 61 | search_nearest(node, node_box_distance); 62 | ++leaves_visited; 63 | } 64 | } 65 | 66 | private: 67 | // Add nodes to the priority queue until a leaf node is reached. 68 | inline void search_nearest( 69 | node_type const* const node, scalar_type node_box_distance) { 70 | if (node->is_leaf()) { 71 | auto begin = indices_.begin() + node->data.leaf.begin_idx; 72 | auto const end = indices_.begin() + node->data.leaf.end_idx; 73 | for (; begin < end; ++begin) { 74 | visitor_(*begin, metric_(query_.begin(), query_.end(), space_[*begin])); 75 | } 76 | } else { 77 | // Go left or right and then check if we should still go down the other 78 | // side based on the current minimum distance. 79 | scalar_type const v = 80 | query_[static_cast(node->data.branch.split_dim)]; 81 | scalar_type old_offset; 82 | scalar_type new_offset; 83 | node_type const* node_1st; 84 | node_type const* node_2nd; 85 | 86 | // On equals we would possibly need to go left as well. However, this is 87 | // handled by the if statement below this one: the check that max search 88 | // radius still hits the split value after having traversed the first 89 | // branch. 90 | // If left_max - v > 0, this means that the query is inside the left node, 91 | // if right_min - v < 0 it's inside the right one. For the area in between 92 | // we just pick the closest one by summing them. 93 | if ((node->data.branch.left_max + node->data.branch.right_min - v - v) > 94 | 0) { 95 | node_1st = node->left; 96 | node_2nd = node->right; 97 | if (v > node->data.branch.left_min) { 98 | old_offset = scalar_type(0); 99 | } else { 100 | old_offset = metric_(node->data.branch.left_min - v); 101 | } 102 | new_offset = metric_(node->data.branch.right_min - v); 103 | } else { 104 | node_1st = node->right; 105 | node_2nd = node->left; 106 | if (v < node->data.branch.right_max) { 107 | old_offset = scalar_type(0); 108 | } else { 109 | old_offset = metric_(node->data.branch.right_max - v); 110 | } 111 | new_offset = metric_(node->data.branch.left_max - v); 112 | } 113 | 114 | // The distance and offset for node_1st is the same as that of its parent. 115 | search_nearest(node_1st, node_box_distance); 116 | 117 | // Calculate the distance to node_2nd. 118 | // NOTE: This method only works with Lp norms to which the exponent is not 119 | // applied. 120 | node_box_distance = node_box_distance - old_offset + new_offset; 121 | 122 | // Add to priority queue to be searched later. 123 | if (visitor_.max() > node_box_distance) { 124 | queue_.emplace(node_box_distance, node_2nd); 125 | } 126 | } 127 | } 128 | 129 | SpaceWrapper_ space_; 130 | Metric_ metric_; 131 | std::vector const& indices_; 132 | PointWrapper_ query_; 133 | size_t max_leaves_visited_; 134 | // TODO This gets created every query. Solving this would require a different 135 | // PicoTree interface. The queue probably shouldn't be too big. 136 | std::priority_queue< 137 | queue_pair_type, 138 | std::vector, 139 | std::greater> 140 | queue_; 141 | Visitor_& visitor_; 142 | }; 143 | 144 | } // namespace pico_tree::internal 145 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/matrix_space.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pico_tree/core.hpp" 4 | #include "pico_tree/map.hpp" 5 | 6 | namespace pico_tree::internal { 7 | 8 | template 9 | struct matrix_space_storage { 10 | constexpr matrix_space_storage(size_t size, size_t) 11 | : size(size), elems(size * sdim) {} 12 | 13 | size_t size; 14 | static constexpr size_t sdim = Dim_; 15 | std::vector elems; 16 | }; 17 | 18 | template 19 | struct matrix_space_storage { 20 | constexpr matrix_space_storage(size_t size, size_t sdim) 21 | : size(size), sdim(sdim), elems(size * sdim) {} 22 | 23 | size_t size; 24 | size_t sdim; 25 | std::vector elems; 26 | }; 27 | 28 | template 29 | class matrix_space { 30 | public: 31 | using scalar_type = Scalar_; 32 | using size_type = pico_tree::size_t; 33 | static size_type constexpr dim = Dim_; 34 | 35 | constexpr matrix_space(size_type size) noexcept : storage_(size, dim) {} 36 | 37 | constexpr matrix_space(size_type size, size_type sdim) noexcept 38 | : storage_(size, sdim) {} 39 | 40 | constexpr point_map operator[]( 41 | size_type i) const noexcept { 42 | return {data(i), storage_.sdim}; 43 | } 44 | 45 | constexpr point_map operator[](size_type i) noexcept { 46 | return {data(i), storage_.sdim}; 47 | } 48 | 49 | constexpr scalar_type const* data() const noexcept { 50 | return storage_.elems.data(); 51 | } 52 | 53 | constexpr scalar_type* data() noexcept { return storage_.elems.data(); } 54 | 55 | constexpr scalar_type const* data(size_type i) const noexcept { 56 | return storage_.elems.data() + i * storage_.sdim; 57 | } 58 | 59 | constexpr scalar_type* data(size_type i) noexcept { 60 | return storage_.elems.data() + i * storage_.sdim; 61 | } 62 | 63 | constexpr size_type size() const noexcept { return storage_.size; } 64 | 65 | constexpr size_type sdim() const noexcept { return storage_.sdim; } 66 | 67 | protected: 68 | internal::matrix_space_storage storage_; 69 | }; 70 | 71 | } // namespace pico_tree::internal 72 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/matrix_space_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pico_tree/map_traits.hpp" 4 | #include "pico_understory/internal/matrix_space.hpp" 5 | 6 | namespace pico_tree { 7 | 8 | template 9 | struct space_traits> { 10 | using space_type = internal::matrix_space; 11 | using point_type = point_map; 12 | using scalar_type = typename space_type::scalar_type; 13 | using size_type = typename space_type::size_type; 14 | static size_type constexpr dim = space_type::dim; 15 | 16 | template 17 | inline static point_type point_at(space_type const& space, Index_ idx) { 18 | return space[static_cast(idx)]; 19 | } 20 | 21 | inline static size_type size(space_type const& space) { return space.size(); } 22 | 23 | inline static size_type sdim(space_type const& space) { return space.sdim(); } 24 | }; 25 | 26 | } // namespace pico_tree 27 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/point_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pico_tree/core.hpp" 4 | #include "pico_tree/internal/point.hpp" 5 | #include "pico_tree/point_traits.hpp" 6 | 7 | namespace pico_tree { 8 | 9 | template 10 | struct point_traits> { 11 | using point_type = internal::point; 12 | using scalar_type = Scalar_; 13 | static size_t constexpr dim = Dim_; 14 | 15 | inline static scalar_type const* data(point_type const& point) { 16 | return point.data(); 17 | } 18 | 19 | inline static std::size_t constexpr size(point_type const& point) { 20 | return point.size(); 21 | } 22 | }; 23 | 24 | } // namespace pico_tree 25 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/rkd_tree_builder.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pico_tree/internal/kd_tree_builder.hpp" 4 | #include "pico_understory/internal/rkd_tree_hh_data.hpp" 5 | 6 | namespace pico_tree::internal { 7 | 8 | template 9 | class build_rkd_tree { 10 | using node_type = typename RKdTreeHhData_::node_type; 11 | using kd_tree_data_type = kd_tree_data; 12 | 13 | public: 14 | using rkd_tree_data_type = RKdTreeHhData_; 15 | 16 | template < 17 | typename SpaceWrapper_, 18 | typename Stop_, 19 | typename Bounds_, 20 | typename Rule_> 21 | std::vector operator()( 22 | SpaceWrapper_ space, 23 | splitter_stop_condition_t const& stop_condition, 24 | splitter_start_bounds_t const& start_bounds, 25 | splitter_rule_t const& rule, 26 | size_t forest_size) { 27 | assert(forest_size > 0); 28 | 29 | using space_wrapper_type = typename rkd_tree_data_type::space_wrapper_type; 30 | using build_kd_tree_type = build_kd_tree; 31 | 32 | std::vector trees; 33 | trees.reserve(forest_size); 34 | for (std::size_t i = 0; i < forest_size; ++i) { 35 | auto r = rkd_tree_data_type::random_rotation(space); 36 | auto s = rkd_tree_data_type::rotate_space(r, space); 37 | auto t = build_kd_tree_type()( 38 | space_wrapper_type(s), stop_condition, start_bounds, rule); 39 | trees.push_back({std::move(r), std::move(s), std::move(t)}); 40 | } 41 | return trees; 42 | } 43 | }; 44 | 45 | } // namespace pico_tree::internal 46 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/rkd_tree_hh_data.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "pico_tree/internal/kd_tree_data.hpp" 6 | #include "pico_tree/internal/point.hpp" 7 | #include "pico_tree/internal/space_wrapper.hpp" 8 | #include "pico_understory/internal/matrix_space_traits.hpp" 9 | #include "point_traits.hpp" 10 | 11 | namespace pico_tree::internal { 12 | 13 | template 14 | inline point random_normal(size_t dim) { 15 | std::random_device rd; 16 | std::mt19937 e(rd()); 17 | std::normal_distribution gaussian(Scalar_(0), Scalar_(1)); 18 | 19 | point v = point::from_size(dim); 20 | 21 | for (size_t i = 0; i < dim; ++i) { 22 | v[i] = gaussian(e); 23 | } 24 | 25 | v.normalize(); 26 | 27 | return v; 28 | } 29 | 30 | // Rotating datasets is computationally expensive. It is quadratic in the 31 | // dimension of the space. Because we're only actually interested in obtaining a 32 | // random orthogonal basis, any orthogonal transformation matrix will do, such 33 | // as the Householder matrix. Householder matrices can be used to obtain linear 34 | // complexity for "rotating" a dataset. C. Silpa-Anan and R. Hartley, Optimised 35 | // KD-trees for fast image descriptor matching, In CVPR, 2008. 36 | // http://vigir.missouri.edu/~gdesouza/Research/Conference_CDs/IEEE_CVPR_2008/data/papers/298.pdf 37 | template 38 | class rkd_tree_hh_data { 39 | public: 40 | using scalar_type = typename Node_::scalar_type; 41 | static size_t constexpr dim = Dim_; 42 | using node_type = Node_; 43 | using rotation_type = point; 44 | using space_type = matrix_space; 45 | using space_wrapper_type = space_wrapper; 46 | 47 | template 48 | static inline auto random_rotation(SpaceWrapper_ space) { 49 | return random_normal(space.sdim()); 50 | } 51 | 52 | template 53 | static inline space_type rotate_space( 54 | rotation_type const& rotation, SpaceWrapper_ space) { 55 | space_type s(space.size(), space.sdim()); 56 | 57 | for (std::size_t i = 0; i < space.size(); ++i) { 58 | auto x = space[i]; 59 | auto y = s.data(i); 60 | rotate_point(rotation, x, y); 61 | } 62 | 63 | return s; 64 | } 65 | 66 | template 67 | point rotate_point(ArrayType_ const& x) const { 68 | point y = 69 | point::from_size(space.sdim()); 70 | rotate_point(rotation, x, y); 71 | return y; 72 | } 73 | 74 | rotation_type rotation; 75 | space_type space; 76 | kd_tree_data tree; 77 | 78 | private: 79 | // In and out can be the same point. 80 | // https://en.wikipedia.org/wiki/Householder_transformation 81 | template 82 | static void rotate_point( 83 | rotation_type const& rotation, ArrayTypeIn_ const& x, ArrayTypeOut_& y) { 84 | scalar_type dot = scalar_type(0); 85 | for (size_t i = 0; i < rotation.size(); ++i) { 86 | dot += rotation[i] * x[i]; 87 | } 88 | dot *= scalar_type(2); 89 | for (size_t i = 0; i < rotation.size(); ++i) { 90 | y[i] = x[i] - (dot * rotation[i]); 91 | } 92 | } 93 | }; 94 | 95 | } // namespace pico_tree::internal 96 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/internal/static_buffer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace pico_tree::internal { 6 | 7 | //! \brief Static MemoryBuffer using a vector. It is a simple memory buffer 8 | //! making deletions of recursive elements a bit easier. 9 | //! \details The buffer owns all memory returned by allocate() and all memory is 10 | //! released when the buffer is destroyed. 11 | template 12 | class static_buffer { 13 | public: 14 | //! \brief Type allocated and stored by the buffer. 15 | using value_type = T_; 16 | 17 | //! Creates a static_buffer having space for \p size elements. 18 | inline explicit static_buffer(std::size_t const size) { 19 | buffer_.reserve(size); 20 | } 21 | 22 | //! \brief Creates an item and returns a pointer to it. 23 | template 24 | inline T_* allocate(Args_&&... args) { 25 | buffer_.emplace_back(std::forward(args)...); 26 | return &buffer_.back(); 27 | } 28 | 29 | private: 30 | //! \private 31 | std::vector buffer_; 32 | }; 33 | 34 | } // namespace pico_tree::internal 35 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/kd_forest.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pico_tree/internal/point_wrapper.hpp" 4 | #include "pico_tree/internal/search_visitor.hpp" 5 | #include "pico_tree/internal/space_wrapper.hpp" 6 | #include "pico_tree/metric.hpp" 7 | #include "pico_understory/internal/kd_tree_priority_search.hpp" 8 | #include "pico_understory/internal/rkd_tree_builder.hpp" 9 | 10 | namespace pico_tree { 11 | 12 | template < 13 | typename Space_, 14 | typename Metric_ = metric_l2_squared, 15 | typename Index_ = int> 16 | class kd_forest { 17 | using space_wrapper_type = internal::space_wrapper; 18 | // priority_search_nearest_euclidean only supports kd_tree_node_topological. 19 | using node_type = internal::kd_tree_node_topological< 20 | Index_, 21 | typename space_wrapper_type::scalar_type>; 22 | using rkd_tree_data_type = 23 | internal::rkd_tree_hh_data; 24 | 25 | public: 26 | //! \brief Size type. 27 | using size_type = size_t; 28 | //! \brief Index type. 29 | using index_type = Index_; 30 | //! \brief Scalar type. 31 | using scalar_type = typename space_wrapper_type::scalar_type; 32 | //! \brief kd_tree dimension. It equals pico_tree::dynamic_extent in case 33 | //! dim is only known at run-time. 34 | static size_type constexpr dim = space_wrapper_type::dim; 35 | //! \brief Point set or adaptor type. 36 | using space_type = Space_; 37 | //! \brief The metric used for various searches. 38 | using metric_type = Metric_; 39 | //! \brief Neighbor type of various search resuls. 40 | using neighbor_type = neighbor; 41 | 42 | kd_forest(space_type space, size_type max_leaf_size, size_type forest_size) 43 | : space_(std::move(space)), 44 | metric_(), 45 | data_( 46 | internal::build_rkd_tree()( 47 | space_wrapper_type(space_), 48 | max_leaf_size_t(max_leaf_size), 49 | bounds_from_space, 50 | sliding_midpoint_max_side, 51 | forest_size)) {} 52 | 53 | //! \brief The kd_forest cannot be copied. 54 | //! \details The kd_forest uses pointers to nodes and copying pointers is not 55 | //! the same as creating a deep copy. 56 | kd_forest(kd_forest const&) = delete; 57 | 58 | //! \brief Move constructor of the kd_forest. 59 | kd_forest(kd_forest&&) = default; 60 | 61 | //! \brief kd_forest copy assignment. 62 | kd_forest& operator=(kd_forest const& other) = delete; 63 | 64 | //! \brief kd_forest move assignment. 65 | kd_forest& operator=(kd_forest&& other) = default; 66 | 67 | //! \brief Returns the nearest neighbor (or neighbors) of point \p x depending 68 | //! on their selection by visitor \p visitor . 69 | template 70 | inline void search_nearest( 71 | P_ const& x, size_type max_leaves_visited, V_& visitor) const { 72 | internal::point_wrapper p(x); 73 | search_nearest( 74 | p, max_leaves_visited, visitor, typename Metric_::space_category()); 75 | } 76 | 77 | //! \brief Searches for the nearest neighbor of point \p x. 78 | //! \details Interpretation of the output distance depends on the Metric. The 79 | //! default metric_l2_squared results in a squared distance. 80 | template 81 | inline void search_nn( 82 | P_ const& x, size_type max_leaves_visited, neighbor_type& nn) const { 83 | internal::search_nn v(nn); 84 | search_nearest(x, max_leaves_visited, v); 85 | } 86 | 87 | private: 88 | //! \brief Returns the nearest neighbor (or neighbors) of point \p x depending 89 | //! on their selection by visitor \p visitor for node \p node. 90 | template 91 | inline void search_nearest( 92 | PointWrapper_ point, 93 | size_type max_leaves_visited, 94 | Visitor_& visitor, 95 | euclidean_space_tag) const { 96 | // Range based for loop (rightfully) results in a warning that shouldn't be 97 | // needed if the user creates the forest with at least a single tree. 98 | for (std::size_t i = 0; i < data_.size(); ++i) { 99 | auto p = data_[i].rotate_point(point); 100 | using point_wrapper_type = internal::point_wrapper; 101 | point_wrapper_type point_wrapper(p); 102 | internal::priority_search_nearest_euclidean< 103 | typename rkd_tree_data_type::space_wrapper_type, 104 | Metric_, 105 | point_wrapper_type, 106 | Visitor_, 107 | index_type>( 108 | typename rkd_tree_data_type::space_wrapper_type(data_[i].space), 109 | metric_, 110 | data_[i].tree.indices, 111 | point_wrapper, 112 | max_leaves_visited, 113 | visitor)(data_[i].tree.root_node); 114 | } 115 | } 116 | 117 | //! \brief Point set used for querying point data. 118 | space_type space_; 119 | //! \brief Metric used for comparing distances. 120 | metric_type metric_; 121 | //! \brief Data structure of the kd_tree. 122 | std::vector data_; 123 | }; 124 | 125 | template 126 | kd_forest(Space_, size_t, size_t) -> kd_forest; 127 | 128 | template < 129 | typename Metric_ = metric_l2_squared, 130 | typename Index_ = int, 131 | typename Space_> 132 | kd_forest, Metric_, Index_> make_kd_forest( 133 | Space_&& space, size_t max_leaf_size, size_t forest_size) { 134 | return kd_forest, Metric_, Index_>( 135 | std::forward(space), max_leaf_size, forest_size); 136 | } 137 | 138 | } // namespace pico_tree 139 | -------------------------------------------------------------------------------- /examples/pico_understory/pico_understory/metric.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace pico_tree { 6 | 7 | //! \brief metric_l2 metric for measuring Euclidean distances between points. 8 | //! \details https://en.wikipedia.org/wiki/Euclidean_distance 9 | //! \see metric_l1 10 | class metric_l2 { 11 | public: 12 | template < 13 | typename InputIterator1_, 14 | typename InputSentinel1_, 15 | typename OutputIterator2_> 16 | constexpr auto operator()( 17 | InputIterator1_ begin1, 18 | InputSentinel1_ end1, 19 | OutputIterator2_ begin2) const { 20 | return std::sqrt( 21 | internal::sum( 22 | begin1, end1, begin2, internal::squared_r1_distance_fn())); 23 | } 24 | 25 | //! \brief Returns the absolute value of \p x. 26 | template 27 | constexpr Scalar_ operator()(Scalar_ const x) const { 28 | return std::abs(x); 29 | } 30 | }; 31 | 32 | } // namespace pico_tree 33 | -------------------------------------------------------------------------------- /examples/python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | file(GLOB PY_SOURCES_ABS "${CMAKE_CURRENT_LIST_DIR}/" *.py) 2 | 3 | # TODO These don't get deleted when running: make clean 4 | add_custom_target(python_examples ALL 5 | COMMAND ${CMAKE_COMMAND} -E copy ${PY_SOURCES_ABS} ${CMAKE_BINARY_DIR}/py) 6 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from skbuild import setup 3 | 4 | 5 | setup( 6 | name='pico_tree', 7 | # The same as the CMake project version. 8 | version='1.0.0', 9 | description='PicoTree Python Bindings', 10 | author='Jonathan Broere', 11 | url='https://github.com/Jaybro/pico_tree', 12 | license='MIT', 13 | packages=['pico_tree'], 14 | package_dir={'': 'src/pyco_tree'}, 15 | cmake_install_dir='src/pyco_tree/pico_tree', 16 | python_requires='>=3.10', 17 | install_requires=['numpy'], 18 | ) 19 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(pico_tree) 2 | add_subdirectory(pyco_tree) 3 | -------------------------------------------------------------------------------- /src/pico_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(GNUInstallDirs) 2 | 3 | ################################################################################ 4 | # PicoTree library. 5 | ################################################################################ 6 | 7 | add_library(${PROJECT_NAME} INTERFACE) 8 | add_library(${PROJECT_PACKAGE_NAME}::${PROJECT_PACKAGE_NAME} ALIAS ${PROJECT_NAME}) 9 | target_include_directories(${PROJECT_NAME} INTERFACE 10 | "$" 11 | "$") 12 | # Language standard above 17 should also be fine. 13 | target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) 14 | set_target_properties(${PROJECT_NAME} PROPERTIES EXPORT_NAME ${PROJECT_PACKAGE_NAME}) 15 | target_compile_options(${PROJECT_NAME} INTERFACE 16 | $,$,$>: 17 | -Wall -Wextra -Wconversion> 18 | $<$: 19 | /W4>>) 20 | 21 | # The target_sources always seem to be made absolute and cannot be used for 22 | # exporting the interface library. 23 | # Error: "INTERFACE_SOURCES property contains path" 24 | # Kept for now should a solution be found later. 25 | # target_sources(${PROJECT_NAME} 26 | # INTERFACE 27 | # ${CMAKE_CURRENT_LIST_DIR}/pico_tree/core.hpp 28 | # ... 29 | # ) 30 | 31 | ################################################################################ 32 | # Generation and installation of Targets.cmake, Config.cmake, 33 | # ConfigVersion.cmake and PicoTree itself. 34 | ################################################################################ 35 | 36 | if(NOT SKBUILD) 37 | set(PROJECT_PACKAGE_TARGETS_NAME ${PROJECT_PACKAGE_NAME}Targets) 38 | set(PROJECT_PACKAGE_TARGETS_FILE_NAME ${PROJECT_PACKAGE_TARGETS_NAME}.cmake) 39 | set(PROJECT_PACKAGE_VERSION_FILE_NAME ${PROJECT_PACKAGE_NAME}ConfigVersion.cmake) 40 | set(PROJECT_PACKAGE_CONFIG_FILE_NAME ${PROJECT_PACKAGE_NAME}Config.cmake) 41 | 42 | # Not adding INCLUDE DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 43 | # See target_include_directories 44 | install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_PACKAGE_TARGETS_NAME}) 45 | 46 | # Allows the target to be exported from the build tree. 47 | export(EXPORT ${PROJECT_PACKAGE_TARGETS_NAME} 48 | FILE "${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_PACKAGE_TARGETS_FILE_NAME}" 49 | NAMESPACE ${PROJECT_PACKAGE_NAME}::) 50 | 51 | # The config mode search procedure uses this path for both Unix and Windows. 52 | set(PACKAGE_INSTALL_DESTINATION 53 | ${PROJECT_PACKAGE_NAME}/share/cmake/${PROJECT_PACKAGE_NAME}) 54 | 55 | include(CMakePackageConfigHelpers) 56 | write_basic_package_version_file( 57 | "${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_PACKAGE_VERSION_FILE_NAME}" 58 | VERSION ${PROJECT_VERSION} 59 | # Beta library will use the minor version for compatibility. 60 | COMPATIBILITY SameMinorVersion) 61 | 62 | configure_package_config_file( 63 | "${PROJECT_SOURCE_DIR}/cmake/Config.cmake.in" 64 | "${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_PACKAGE_CONFIG_FILE_NAME}" 65 | INSTALL_DESTINATION ${PACKAGE_INSTALL_DESTINATION}) 66 | 67 | install(EXPORT ${PROJECT_PACKAGE_TARGETS_NAME} 68 | FILE "${PROJECT_PACKAGE_TARGETS_FILE_NAME}" 69 | NAMESPACE ${PROJECT_PACKAGE_NAME}:: 70 | DESTINATION ${PACKAGE_INSTALL_DESTINATION}) 71 | 72 | install(FILES 73 | "${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_PACKAGE_VERSION_FILE_NAME}" 74 | "${CMAKE_CURRENT_BINARY_DIR}/cmake/${PROJECT_PACKAGE_CONFIG_FILE_NAME}" 75 | DESTINATION ${PACKAGE_INSTALL_DESTINATION}) 76 | 77 | install(DIRECTORY "${CMAKE_CURRENT_LIST_DIR}/${PROJECT_NAME}" 78 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 79 | endif() 80 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/array_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "core.hpp" 6 | #include "point_traits.hpp" 7 | 8 | namespace pico_tree { 9 | 10 | //! \brief Point interface for Scalar_[Dim_]. 11 | template 12 | struct point_traits { 13 | using point_type = Scalar_[Dim_]; 14 | using scalar_type = Scalar_; 15 | using size_type = size_t; 16 | static constexpr size_type dim = static_cast(Dim_); 17 | 18 | //! \brief Returns a pointer to the coordinates of the input point. 19 | inline static constexpr scalar_type const* data(point_type const& point) { 20 | return point; 21 | } 22 | 23 | //! \brief Returns the number of coordinates or spatial dimension of each 24 | //! point. 25 | inline static constexpr size_type size(point_type const&) { return dim; } 26 | }; 27 | 28 | //! \brief Point interface for std::array. 29 | template 30 | struct point_traits> { 31 | using point_type = std::array; 32 | using scalar_type = Scalar_; 33 | using size_type = size_t; 34 | static constexpr size_type dim = static_cast(Dim_); 35 | 36 | //! \brief Returns a pointer to the coordinates of the input point. 37 | inline static constexpr scalar_type const* data(point_type const& point) { 38 | return point.data(); 39 | } 40 | 41 | //! \brief Returns the number of coordinates or spatial dimension of each 42 | //! point. 43 | inline static constexpr size_type size(point_type const&) { return dim; } 44 | }; 45 | 46 | } // namespace pico_tree 47 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/core.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | //! \mainpage PicoTree is a C++ header only library for nearest neighbor 4 | //! searches and range searches using a kd_tree. 5 | //! \file core.hpp 6 | //! \brief Contains various common utilities. 7 | 8 | #include 9 | 10 | namespace pico_tree { 11 | 12 | //! \brief Size type used by PicoTree. 13 | using size_t = std::size_t; 14 | 15 | //! \brief This value can be used in any template argument that wants to know 16 | //! the spatial dimension of the search problem when it can only be known at 17 | //! run-time. In this case the dimension of the problem is provided by the point 18 | //! adaptor. 19 | inline size_t constexpr dynamic_extent = static_cast(-1); 20 | 21 | //! \brief A Neighbor is a point reference with a corresponding distance to 22 | //! another point. 23 | template 24 | struct neighbor { 25 | static_assert(std::is_integral_v, "INDEX_NOT_AN_INTEGRAL_TYPE"); 26 | static_assert(std::is_arithmetic_v, "SCALAR_NOT_AN_ARITHMETIC_TYPE"); 27 | 28 | //! \brief Index type. 29 | using index_type = Index_; 30 | //! \brief Distance type. 31 | using scalar_type = Scalar_; 32 | 33 | //! \brief Default constructor. 34 | //! \details Declaring a custom constructor removes the default one. With 35 | //! C++11 we can bring back the default constructor and keep this struct a POD 36 | //! type. 37 | constexpr neighbor() = default; 38 | //! \brief Constructs a Neighbor given an index and distance. 39 | constexpr neighbor(index_type idx, scalar_type dst) noexcept 40 | : index(idx), distance(dst) {} 41 | 42 | //! \brief Point index of the Neighbor. 43 | index_type index; 44 | //! \brief Distance of the Neighbor with respect to another point. 45 | scalar_type distance; 46 | }; 47 | 48 | //! \brief Compares neighbors by distance. 49 | template 50 | constexpr bool operator<( 51 | neighbor const& lhs, 52 | neighbor const& rhs) noexcept { 53 | return lhs.distance < rhs.distance; 54 | } 55 | 56 | } // namespace pico_tree 57 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/distance.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace pico_tree { 4 | 5 | class one_space_r1 {}; 6 | 7 | class one_space_s1 {}; 8 | 9 | namespace internal { 10 | 11 | //! \brief Simply the number one as a constant. 12 | template 13 | inline T_ constexpr one_v = T_(1.0); 14 | 15 | } // namespace internal 16 | 17 | //! \brief Calculates the distance between two coordinates. 18 | template 19 | constexpr Scalar_ r1_distance(Scalar_ x, Scalar_ y) { 20 | return std::abs(x - y); 21 | } 22 | 23 | //! \brief Calculates the distance between two coordinates on the unit circle 24 | //! s1. The values for \p x or \p y must lie within the range of [0...1]. 25 | template 26 | constexpr Scalar_ s1_distance(Scalar_ x, Scalar_ y) { 27 | Scalar_ const d = std::abs(x - y); 28 | return std::min(d, internal::one_v - d); 29 | } 30 | 31 | //! \brief Calculates the square of a number. 32 | template 33 | constexpr Scalar_ squared(Scalar_ x) { 34 | return x * x; 35 | } 36 | 37 | //! \brief Calculates the squared distance between two coordinates. 38 | template 39 | constexpr Scalar_ squared_r1_distance(Scalar_ x, Scalar_ y) { 40 | return squared(x - y); 41 | } 42 | 43 | //! \brief Calculates the squared distance between two coordinates on the unit 44 | //! circle s1. The values for \p x or \p y must lie within the range of [0...1]. 45 | template 46 | constexpr Scalar_ squared_s1_distance(Scalar_ x, Scalar_ y) { 47 | return squared(s1_distance(x, y)); 48 | } 49 | 50 | } // namespace pico_tree 51 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/kd_tree_data.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "pico_tree/core.hpp" 7 | #include "pico_tree/internal/memory.hpp" 8 | #include "pico_tree/internal/stream_wrapper.hpp" 9 | 10 | namespace pico_tree::internal { 11 | 12 | //! \brief The data structure that represents a kd_tree. 13 | template 14 | class kd_tree_data { 15 | template 16 | class iterator_range { 17 | public: 18 | using iterator_type = Iterator_; 19 | using difference_type = 20 | typename std::iterator_traits::difference_type; 21 | 22 | constexpr iterator_range(Iterator_ begin, Iterator_ end) 23 | : begin_(begin), end_(end) {} 24 | 25 | constexpr Iterator_ begin() const { return begin_; } 26 | constexpr Iterator_ end() const { return end_; } 27 | 28 | private: 29 | Iterator_ begin_; 30 | Iterator_ end_; 31 | }; 32 | 33 | public: 34 | using index_type = typename Node_::index_type; 35 | using scalar_type = typename Node_::scalar_type; 36 | static size_t constexpr dim = Dim_; 37 | using box_type = internal::box; 38 | using node_type = Node_; 39 | using node_allocator_type = chunk_allocator; 40 | using leaf_range_type = 41 | iterator_range::const_iterator>; 42 | 43 | static kd_tree_data load(stream_wrapper& stream) { 44 | typename box_type::size_type sdim; 45 | stream.read(sdim); 46 | 47 | kd_tree_data kd_tree_data{ 48 | {}, box_type(sdim), node_allocator_type(), nullptr}; 49 | kd_tree_data.read(stream); 50 | 51 | return kd_tree_data; 52 | } 53 | 54 | static void save(kd_tree_data const& data, stream_wrapper& stream) { 55 | // Write sdim. 56 | stream.write(data.root_box.size()); 57 | data.write(stream); 58 | } 59 | 60 | inline std::vector leaf_ranges() const { 61 | std::vector ranges; 62 | insert_leaf_range(root_node, ranges); 63 | return ranges; 64 | } 65 | 66 | //! \brief Sorted indices that refer to points inside points_. 67 | std::vector indices; 68 | //! \brief Bounding box of the root node. 69 | box_type root_box; 70 | //! \brief Memory allocator for tree nodes. 71 | node_allocator_type allocator; 72 | //! \brief Root of the kd_tree. 73 | node_type* root_node; 74 | 75 | private: 76 | inline void insert_leaf_range( 77 | node_type const* const node, std::vector& ranges) const { 78 | if (node->is_leaf() && !node->data.leaf.empty()) { 79 | using difference_type = typename leaf_range_type::difference_type; 80 | ranges.push_back(leaf_range_type( 81 | indices.begin() + difference_type(node->data.leaf.begin_idx), 82 | indices.begin() + difference_type(node->data.leaf.end_idx))); 83 | } else { 84 | insert_leaf_range(node->left, ranges); 85 | insert_leaf_range(node->right, ranges); 86 | } 87 | } 88 | 89 | //! \brief Recursively reads the Node and its descendants. 90 | inline node_type* read_node(stream_wrapper& stream) { 91 | node_type* node = allocator.allocate(); 92 | bool is_leaf; 93 | stream.read(is_leaf); 94 | 95 | if (is_leaf) { 96 | stream.read(node->data.leaf); 97 | node->left = nullptr; 98 | node->right = nullptr; 99 | } else { 100 | stream.read(node->data.branch); 101 | node->left = read_node(stream); 102 | node->right = read_node(stream); 103 | } 104 | 105 | return node; 106 | } 107 | 108 | //! \brief Recursively writes the Node and its descendants. 109 | inline void write_node( 110 | node_type const* const node, stream_wrapper& stream) const { 111 | if (node->is_leaf()) { 112 | stream.write(true); 113 | stream.write(node->data.leaf); 114 | } else { 115 | stream.write(false); 116 | stream.write(node->data.branch); 117 | write_node(node->left, stream); 118 | write_node(node->right, stream); 119 | } 120 | } 121 | 122 | inline void read(stream_wrapper& stream) { 123 | stream.read(indices); 124 | // The root box gets the correct size from the kd_tree constructor. 125 | stream.read(root_box.size(), root_box.min()); 126 | stream.read(root_box.size(), root_box.max()); 127 | root_node = read_node(stream); 128 | } 129 | 130 | inline void write(stream_wrapper& stream) const { 131 | stream.write(indices); 132 | stream.write(root_box.min(), root_box.size()); 133 | stream.write(root_box.max(), root_box.size()); 134 | write_node(root_node, stream); 135 | } 136 | }; 137 | 138 | } // namespace pico_tree::internal 139 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/kd_tree_node.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace pico_tree::internal { 4 | 5 | //!\brief Binary node base. 6 | template 7 | struct kd_tree_node_base { 8 | //! \brief Returns if the current node is a branch. 9 | inline bool is_branch() const { return left != nullptr && right != nullptr; } 10 | //! \brief Returns if the current node is a leaf. 11 | inline bool is_leaf() const { return left == nullptr && right == nullptr; } 12 | 13 | template 14 | inline void set_leaf(Index_ begin_idx, Index_ end_idx) { 15 | derived().data.leaf.begin_idx = begin_idx; 16 | derived().data.leaf.end_idx = end_idx; 17 | left = nullptr; 18 | right = nullptr; 19 | } 20 | 21 | inline Derived_& derived() { return *static_cast(this); } 22 | 23 | //! \brief Left child. 24 | Derived_* left; 25 | //! \brief Right child. 26 | Derived_* right; 27 | }; 28 | 29 | //! \brief Tree leaf data. 30 | template 31 | struct kd_tree_leaf { 32 | //! \brief Returns true if the leaf is empty. 33 | constexpr bool empty() const { return begin_idx == end_idx; } 34 | 35 | //! \brief Begin of an index range. 36 | Index_ begin_idx; 37 | //! \brief End of an index range. 38 | Index_ end_idx; 39 | }; 40 | 41 | //! \brief Tree branch data that stores one boundary per child. 42 | template 43 | struct kd_tree_branch_single { 44 | //! \brief Split coordinate / index of the kd_tree spatial dimension. 45 | int split_dim; 46 | //! \brief Maximum coordinate value of the left node box for split_dim. 47 | Scalar_ left_max; 48 | //! \brief Minimum coordinate value of the right node box for split_dim. 49 | Scalar_ right_min; 50 | }; 51 | 52 | //! \brief Tree branch data that stores two boundaries per child. 53 | //! \details Storing both boundaries per child allows the use of identifications 54 | //! (wrapping around). 55 | template 56 | struct kd_tree_branch_double { 57 | //! \brief Split coordinate / index of the kd_tree spatial dimension. 58 | int split_dim; 59 | //! \brief Minimum coordinate value of the left node box for split_dim. 60 | Scalar_ left_min; 61 | //! \brief Maximum coordinate value of the left node box for split_dim. 62 | Scalar_ left_max; 63 | //! \brief Minimum coordinate value of the right node box for split_dim. 64 | Scalar_ right_min; 65 | //! \brief Maximum coordinate value of the right node box for split_dim. 66 | Scalar_ right_max; 67 | }; 68 | 69 | //! \brief NodeData is used to either store branch or leaf information. Which 70 | //! union member is used can be tested with is_branch() or is_leaf(). 71 | template 72 | union kd_tree_node_data { 73 | //! \brief Union branch data. 74 | Branch_ branch; 75 | //! \brief Union leaf data. 76 | Leaf_ leaf; 77 | }; 78 | 79 | //! \brief kd_tree node for a Euclidean space. 80 | template 81 | struct kd_tree_node_euclidean 82 | : public kd_tree_node_base> { 83 | using index_type = Index_; 84 | using scalar_type = Scalar_; 85 | 86 | template 87 | inline void set_branch( 88 | Box_ const& left_box, Box_ const& right_box, size_t const split_dim) { 89 | data.branch.split_dim = static_cast(split_dim); 90 | data.branch.left_max = left_box.max(split_dim); 91 | data.branch.right_min = right_box.min(split_dim); 92 | } 93 | 94 | //! \brief Node data as a union of a leaf and branch. 95 | kd_tree_node_data, kd_tree_branch_single> data; 96 | }; 97 | 98 | //! \brief kd_tree node for a topological space. 99 | template 100 | struct kd_tree_node_topological 101 | : public kd_tree_node_base> { 102 | using index_type = Index_; 103 | using scalar_type = Scalar_; 104 | 105 | template 106 | inline void set_branch( 107 | Box_ const& left_box, Box_ const& right_box, size_t const split_dim) { 108 | data.branch.split_dim = static_cast(split_dim); 109 | data.branch.left_min = left_box.min(split_dim); 110 | data.branch.left_max = left_box.max(split_dim); 111 | data.branch.right_min = right_box.min(split_dim); 112 | data.branch.right_max = right_box.max(split_dim); 113 | } 114 | 115 | //! \brief Node data as a union of a leaf and branch. 116 | kd_tree_node_data, kd_tree_branch_double> data; 117 | }; 118 | 119 | } // namespace pico_tree::internal 120 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/memory.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace pico_tree::internal { 6 | 7 | //! \brief An instance of list_pool_resource constructs fixed size chunks of 8 | //! memory and stores these in a list. Memory is only released when the resource 9 | //! is destructed or when calling the release() method. 10 | //! \details A list_pool_resource is mainly useful for monotonically 11 | //! constructing objects of a single type when the total number to be created 12 | //! cannot be known up front. 13 | //!

14 | //! A previous memory manager implementation was based on the std::deque. The 15 | //! chunk size that it uses can vary across different implementations of the C++ 16 | //! standard, resulting in an unreliable performance of PicoTree. 17 | //!

18 | //! https://en.wikipedia.org/wiki/Memory_pool 19 | template 20 | class list_pool_resource { 21 | private: 22 | struct node; 23 | 24 | public: 25 | static_assert(std::is_trivial_v, "TYPE_T_IS_NOT_TRIVIAL"); 26 | static_assert( 27 | std::is_trivially_destructible_v, 28 | "TYPE_T_IS_NOT_TRIVIALLY_DESTRUCTIBLE"); 29 | 30 | //! \brief Value type allocated by the list_pool_resource. 31 | using value_type = T_; 32 | //! \brief Chunk type allocated by the list_pool_resource. 33 | using chunk = typename node::chunk; 34 | 35 | public: 36 | //! \brief list_pool_resource constructor. 37 | list_pool_resource() : head_(nullptr) {} 38 | 39 | //! \brief A list_pool_resource instance cannot be copied. 40 | //! \details Just no! 41 | list_pool_resource(list_pool_resource const&) = delete; 42 | 43 | //! \private 44 | list_pool_resource(list_pool_resource&& other) : head_(other.head_) { 45 | // So we don't accidentally delete things twice. 46 | other.head_ = nullptr; 47 | } 48 | 49 | //! \private 50 | list_pool_resource& operator=(list_pool_resource const& other) = delete; 51 | 52 | //! \private 53 | list_pool_resource& operator=(list_pool_resource&& other) { 54 | head_ = other.head_; 55 | other.head_ = nullptr; 56 | return *this; 57 | } 58 | 59 | //! \brief list_pool_resource destructor. 60 | virtual ~list_pool_resource() { release(); } 61 | 62 | //! \brief Allocates a chunk of memory and returns a pointer to it. 63 | inline chunk* allocate() { 64 | node* n = new node; 65 | n->prev = head_; 66 | head_ = n; 67 | return &head_->data; 68 | } 69 | 70 | //! \brief Release all memory allocated by this list_pool_resource. 71 | void release() { 72 | // Suppose node was contained by an std::unique_ptr, then it may happen 73 | // that we hit a recursion limit depending on how many nodes are destructed. 74 | while (head_ != nullptr) { 75 | node* n = head_->prev; 76 | delete head_; 77 | head_ = n; 78 | } 79 | } 80 | 81 | private: 82 | node* head_; 83 | }; 84 | 85 | //! \brief Node containing a chunk of memory. 86 | template 87 | struct list_pool_resource::node { 88 | //! \brief Chunk type allocated by the list_pool_resource. 89 | using chunk = std::array; 90 | 91 | node* prev; 92 | chunk data; 93 | }; 94 | 95 | //! \brief An instance of chunk_allocator constructs objects. It does so in 96 | //! chunks of size ChunkSize_ to reduce memory fragmentation. 97 | template 98 | class chunk_allocator final { 99 | private: 100 | using resource = list_pool_resource; 101 | using chunk = typename resource::chunk; 102 | 103 | public: 104 | //! \brief Value type allocated by the chunk_allocator. 105 | using value_type = T_; 106 | 107 | //! \brief chunk_allocator constructor. 108 | chunk_allocator() : object_index_(ChunkSize_) {} 109 | 110 | //! \brief Create an object of type T_ and return a pointer to it. 111 | inline value_type* allocate() { 112 | if (object_index_ == ChunkSize_) { 113 | chunk_ = resource_.allocate(); 114 | object_index_ = 0; 115 | } 116 | 117 | value_type* object = &(*chunk_)[object_index_]; 118 | object_index_++; 119 | 120 | return object; 121 | } 122 | 123 | private: 124 | resource resource_; 125 | std::size_t object_index_; 126 | chunk* chunk_; 127 | }; 128 | 129 | } // namespace pico_tree::internal 130 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/point.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "pico_tree/core.hpp" 10 | 11 | namespace pico_tree::internal { 12 | 13 | //! \details The non-specialized class knows its dimension at compile-time and 14 | //! uses an std::array for storing its data. Faster than using the std::vector 15 | //! in practice. 16 | template 17 | struct point_storage_traits { 18 | using type = std::array; 19 | 20 | static constexpr auto from_size([[maybe_unused]] size_t size) { 21 | assert(size == Dim_); 22 | return type(); 23 | } 24 | }; 25 | 26 | //! \details The specialized class doesn't knows its dimension at compile-time 27 | //! and uses an std::vector for storing its data so it can be resized. 28 | template 29 | struct point_storage_traits { 30 | using type = std::vector; 31 | 32 | static constexpr auto from_size(size_t size) { return type(size); } 33 | }; 34 | 35 | //! \brief A point is a container that stores a contiguous array of elements as 36 | //! an aggregate type. The storage is either as an std::array or an std::vector. 37 | //! Using the storage, elems_, is considered undefined behavior. 38 | //! \details Having elems_ public goes against the against the encapsulation 39 | //! principle but gives us aggregate initialization in return. 40 | template 41 | struct point { 42 | static_assert( 43 | Dim_ == dynamic_extent || Dim_ > 0, "DIM_MUST_BE_DYNAMIC_OR_>_0"); 44 | 45 | //! \private Using Pst__ is considered undefined behavior. 46 | using Pst__ = point_storage_traits; 47 | //! \private Using Elems__ is considered undefined behavior. 48 | using Elems__ = typename Pst__::type; 49 | 50 | using scalar_type = Scalar_; 51 | using size_type = size_t; 52 | static size_type constexpr dim = Dim_; 53 | 54 | //! \brief Creates a point from a size. 55 | static constexpr point from_size(size_t size) { 56 | return {Pst__::from_size(size)}; 57 | } 58 | 59 | //! \brief Fills the storage with value \p v. 60 | constexpr void fill(scalar_type v) { 61 | std::fill(elems_.begin(), elems_.end(), v); 62 | } 63 | 64 | //! \brief Normalize point in place to unit length. 65 | void normalize() { 66 | scalar_type l2 = scalar_type(0); 67 | for (auto& e : elems_) l2 += e * e; 68 | 69 | l2 = scalar_type(1) / std::sqrt(l2); 70 | for (auto& e : elems_) e *= l2; 71 | } 72 | 73 | //! \brief Access the container data. 74 | constexpr scalar_type& operator[](size_type i) noexcept { return elems_[i]; } 75 | 76 | //! \brief Access the container data. 77 | constexpr scalar_type const& operator[](size_type i) const noexcept { 78 | return elems_[i]; 79 | } 80 | 81 | constexpr scalar_type const* data() const noexcept { return elems_.data(); } 82 | 83 | constexpr scalar_type* data() noexcept { return elems_.data(); } 84 | 85 | //! \brief Returns the size of the container. 86 | constexpr size_type size() const noexcept { return elems_.size(); } 87 | 88 | //! \private Using elems_ is considered undefined behavior. 89 | Elems__ elems_; 90 | }; 91 | 92 | } // namespace pico_tree::internal 93 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/point_wrapper.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pico_tree/core.hpp" 4 | #include "pico_tree/point_traits.hpp" 5 | 6 | namespace pico_tree::internal { 7 | 8 | //! \brief The point_wrapper class wraps makes working with any point type 9 | //! through its respective point_traits a bit easier and it allows for the 10 | //! addition of extra convenience methods. 11 | //! \details The internals of PicoTree never use the specializations of the 12 | //! point_traits class directly, but interface with any point type through this 13 | //! wrapper interface. 14 | template 15 | class point_wrapper { 16 | using point_traits_type = point_traits; 17 | using point_type = Point_; 18 | 19 | public: 20 | using scalar_type = typename point_traits_type::scalar_type; 21 | using size_type = size_t; 22 | static constexpr size_type dim = point_traits_type::dim; 23 | 24 | inline explicit point_wrapper(point_type const& point) : point_(point) {} 25 | 26 | inline scalar_type const& operator[](std::size_t index) const { 27 | return data()[index]; 28 | } 29 | 30 | inline auto begin() const { return data(); } 31 | 32 | inline auto end() const { return data() + size(); } 33 | 34 | private: 35 | inline scalar_type const* data() const { 36 | return point_traits_type::data(point_); 37 | } 38 | 39 | constexpr size_type size() const { 40 | if constexpr (dim != dynamic_extent) { 41 | return dim; 42 | } else { 43 | return point_traits_type::size(point_); 44 | } 45 | } 46 | 47 | point_type const& point_; 48 | }; 49 | 50 | } // namespace pico_tree::internal 51 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/segment.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "pico_tree/distance.hpp" 7 | 8 | namespace pico_tree::internal { 9 | 10 | template 11 | struct segment_base { 12 | using scalar_type = Scalar_; 13 | 14 | segment_base(scalar_type min, scalar_type max) : min(min), max(max) {} 15 | 16 | scalar_type min; 17 | scalar_type max; 18 | }; 19 | 20 | template 21 | struct segment_r1 : protected segment_base { 22 | using segment_base::segment_base; 23 | using segment_base::max; 24 | using segment_base::min; 25 | using typename segment_base::scalar_type; 26 | 27 | constexpr segment_r1(scalar_type min, scalar_type max) 28 | : segment_base(min, max) { 29 | assert(min <= max); 30 | } 31 | 32 | constexpr bool contains(scalar_type x) const { return min <= x && x <= max; } 33 | 34 | constexpr bool contains(segment_r1 const& x) const { 35 | return min <= x.min && x.max <= max; 36 | } 37 | 38 | constexpr scalar_type distance(scalar_type x) const { 39 | if (x < min) { 40 | return min - x; 41 | } else if (x > max) { 42 | return x - max; 43 | } else { 44 | return scalar_type(0.0); 45 | } 46 | } 47 | 48 | constexpr scalar_type extent() const { return max - min; } 49 | }; 50 | 51 | template 52 | struct segment_s1 : protected segment_base { 53 | using segment_base::segment_base; 54 | using segment_base::max; 55 | using segment_base::min; 56 | using typename segment_base::scalar_type; 57 | 58 | constexpr segment_s1(scalar_type min, scalar_type max) 59 | : segment_base(min, max) {} 60 | 61 | constexpr bool contains(scalar_type x) const { 62 | if (linear()) { 63 | return min <= x && x <= max; 64 | } else { 65 | return x >= min || x <= max; 66 | } 67 | } 68 | 69 | constexpr bool contains(segment_r1 const& x) const { 70 | if (linear()) { 71 | return min <= x.min && x.max <= max; 72 | } else { 73 | return x.min >= min || x.max <= max; 74 | } 75 | } 76 | 77 | constexpr scalar_type distance_min_max(scalar_type x) const { 78 | if (x < min || x > max) { 79 | return std::min(s1_distance(x, min), s1_distance(x, max)); 80 | } else { 81 | return scalar_type(0.0); 82 | } 83 | } 84 | 85 | constexpr scalar_type distance_max_min(scalar_type x) const { 86 | if (x < max || x > min) { 87 | return scalar_type(0.0); 88 | } else { 89 | return std::min(s1_distance(x, min), s1_distance(x, max)); 90 | } 91 | } 92 | 93 | constexpr scalar_type distance(scalar_type x) const { 94 | if (linear()) { 95 | return distance_min_max(x); 96 | } else { 97 | return distance_max_min(x); 98 | } 99 | } 100 | 101 | constexpr bool linear() const { return min <= max; } 102 | }; 103 | 104 | } // namespace pico_tree::internal 105 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/space_wrapper.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "pico_tree/core.hpp" 4 | #include "pico_tree/internal/box.hpp" 5 | #include "pico_tree/space_traits.hpp" 6 | 7 | namespace pico_tree::internal { 8 | 9 | //! \brief The space_wrapper class wraps makes working with any space type 10 | //! through its respective space_traits a bit easier and it allows for the 11 | //! addition of extra convenience methods. 12 | //! \details The internals of PicoTree never use the specializations of the 13 | //! space_traits class directly, but interface with any space type through this 14 | //! wrapper interface 15 | template 16 | class space_wrapper { 17 | using space_traits_type = space_traits; 18 | using space_type = Space_; 19 | using point_type = typename space_traits_type::point_type; 20 | using point_traits_type = point_traits; 21 | 22 | public: 23 | using scalar_type = typename space_traits_type::scalar_type; 24 | using size_type = size_t; 25 | static size_type constexpr dim = space_traits_type::dim; 26 | 27 | inline explicit space_wrapper(space_type const& space) : space_(space) {} 28 | 29 | template 30 | inline scalar_type const* operator[](Index_ const index) const { 31 | return point_traits_type::data(space_traits_type::point_at(space_, index)); 32 | } 33 | 34 | inline box compute_bounding_box() const { 35 | auto bbox = box::make_inverse_max(sdim()); 36 | for (size_type i = 0; i < size(); ++i) { 37 | bbox.fit(operator[](i)); 38 | } 39 | return bbox; 40 | } 41 | 42 | inline size_type size() const { return space_traits_type::size(space_); } 43 | 44 | constexpr size_type sdim() const { 45 | if constexpr (dim != dynamic_extent) { 46 | return dim; 47 | } else { 48 | return space_traits_type::sdim(space_); 49 | } 50 | } 51 | 52 | private: 53 | space_type const& space_; 54 | }; 55 | 56 | } // namespace pico_tree::internal 57 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/stream_wrapper.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace pico_tree::internal { 8 | 9 | //! \brief Returns an std::fstream given a filename. 10 | //! \details Convenience function that throws an std::runtime_error in case it 11 | //! is unable to open the stream. 12 | inline std::fstream open_stream( 13 | std::string const& filename, std::ios_base::openmode mode) { 14 | std::fstream stream(filename, mode); 15 | 16 | if (!stream.is_open()) { 17 | throw std::runtime_error("unable to open file: " + filename); 18 | } 19 | 20 | return stream; 21 | } 22 | 23 | //! \brief The stream_wrapper class is an std::iostream wrapper that helps read 24 | //! and write various simple data types. 25 | class stream_wrapper { 26 | public: 27 | //! \brief Constructs a stream_wrapper using an input std::iostream. 28 | stream_wrapper(std::iostream& stream) : stream_(stream) {} 29 | 30 | //! \brief Reads a single value from the stream_wrapper. 31 | //! \tparam T_ Type of the value. 32 | template 33 | inline void read(T_& value) { 34 | stream_.read( 35 | reinterpret_cast(&value), 36 | static_cast(sizeof(T_))); 37 | } 38 | 39 | //! \brief Reads a vector of values from the stream_wrapper. 40 | //! \details Reads the size of the vector followed by all its elements. 41 | //! \tparam T_ Type of a value. 42 | template 43 | inline void read(std::vector& values) { 44 | typename std::vector::size_type size; 45 | read(size); 46 | values.resize(size); 47 | read(size, values.data()); 48 | } 49 | 50 | //! \brief Reads a string from the stream_wrapper. 51 | //! \details Reads the size of the string followed by all its elements. 52 | inline void read(std::string& values) { 53 | typename std::string::size_type size; 54 | read(size); 55 | values.resize(size); 56 | read(size, values.data()); 57 | } 58 | 59 | //! \brief Reads an array of values from the stream_wrapper. 60 | //! \tparam T_ Type of a value. 61 | template 62 | inline void read(std::size_t size, T_* values) { 63 | stream_.read( 64 | reinterpret_cast(values), 65 | static_cast(sizeof(T_) * size)); 66 | } 67 | 68 | //! \brief Writes a single value to the stream_wrapper. 69 | //! \tparam T_ Type of the value. 70 | template 71 | inline void write(T_ const& value) { 72 | stream_.write( 73 | reinterpret_cast(&value), 74 | static_cast(sizeof(T_))); 75 | } 76 | 77 | //! \brief Writes a vector of values to the stream_wrapper. 78 | //! \details Writes the size of the vector followed by all its elements. 79 | //! \tparam T_ Type of a value. 80 | template 81 | inline void write(std::vector const& values) { 82 | write(values.size()); 83 | write(values.data(), values.size()); 84 | } 85 | 86 | //! \brief Writes a string to the stream_wrapper. 87 | //! \details Writes the size of the string followed by all its elements. 88 | inline void write(std::string const& values) { 89 | write(values.size()); 90 | write(values.data(), values.size()); 91 | } 92 | 93 | //! \brief Writes an array of values to the stream_wrapper. 94 | //! \tparam T_ Type of a value. 95 | template 96 | inline void write(T_ const* values, std::size_t size) { 97 | stream_.write( 98 | reinterpret_cast(values), 99 | static_cast(sizeof(T_) * size)); 100 | } 101 | 102 | private: 103 | //! \brief Wrapped stream. 104 | std::iostream& stream_; 105 | }; 106 | 107 | } // namespace pico_tree::internal 108 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/internal/type_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace pico_tree::internal { 7 | 8 | template 9 | struct remove_reference_wrapper { 10 | using type = T_; 11 | }; 12 | 13 | template 14 | struct remove_reference_wrapper> { 15 | using type = std::remove_cv_t; 16 | }; 17 | 18 | template 19 | using remove_reference_wrapper_t = typename remove_reference_wrapper::type; 20 | 21 | } // namespace pico_tree::internal 22 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/map.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "core.hpp" 6 | #include "point_traits.hpp" 7 | 8 | namespace pico_tree { 9 | 10 | namespace internal { 11 | 12 | template 13 | struct map_storage { 14 | constexpr map_storage(Element_* data, size_t) noexcept : data(data) {} 15 | 16 | Element_* data; 17 | static size_t constexpr size = Extent_; 18 | }; 19 | 20 | template 21 | struct map_storage { 22 | constexpr map_storage(Element_* data, size_t size) noexcept 23 | : data(data), size(size) {} 24 | 25 | Element_* data; 26 | size_t size; 27 | }; 28 | 29 | template 30 | class map { 31 | public: 32 | static_assert(std::is_object_v, "ELEMENT_NOT_AN_OBJECT_TYPE"); 33 | static_assert( 34 | !std::is_abstract_v, "ELEMENT_CANNOT_BE_AN_ABSTRACT_TYPE"); 35 | static_assert( 36 | Extent_ == dynamic_extent || Extent_ > 0, "DIM_MUST_BE_DYNAMIC_OR_>_0"); 37 | 38 | using element_type = Element_; 39 | using value_type = std::remove_cv_t; 40 | using size_type = size_t; 41 | static constexpr size_type extent = Extent_; 42 | 43 | explicit constexpr map(element_type* data) noexcept 44 | : storage_(data, extent) {} 45 | 46 | constexpr map(element_type* data, size_type size) noexcept 47 | : storage_(data, size) {} 48 | 49 | template 50 | constexpr map( 51 | ContiguousAccessIterator_ begin, ContiguousAccessIterator_ end) noexcept 52 | : storage_(&(*begin), static_cast(end - begin)) {} 53 | 54 | constexpr element_type& operator[](size_type i) const { 55 | return storage_.data[i]; 56 | } 57 | 58 | constexpr element_type* data() const noexcept { return storage_.data; } 59 | 60 | constexpr size_type size() const noexcept { return storage_.size; } 61 | 62 | protected: 63 | map_storage storage_; 64 | }; 65 | 66 | template 67 | struct space_map_matrix_storage { 68 | constexpr space_map_matrix_storage(Scalar_* data, size_t size, size_t) 69 | : data(data), size(size) {} 70 | 71 | Scalar_* data; 72 | size_t size; 73 | static size_t constexpr sdim = Dim_; 74 | }; 75 | 76 | template 77 | struct space_map_matrix_storage { 78 | constexpr space_map_matrix_storage(Scalar_* data, size_t size, size_t sdim) 79 | : data(data), size(size), sdim(sdim) {} 80 | 81 | Scalar_* data; 82 | size_t size; 83 | size_t sdim; 84 | }; 85 | 86 | } // namespace internal 87 | 88 | //! \brief The point_map class provides a point interface for an array of 89 | //! scalars. 90 | template 91 | class point_map : protected internal::map { 92 | private: 93 | using base = internal::map; 94 | 95 | public: 96 | static_assert(std::is_arithmetic_v, "SCALAR_NOT_AN_ARITHMETIC_TYPE"); 97 | 98 | using scalar_type = typename base::value_type; 99 | using typename internal::map::element_type; 100 | using typename internal::map::size_type; 101 | static size_type constexpr dim = base::extent; 102 | 103 | using internal::map::map; 104 | using internal::map::operator[]; 105 | using internal::map::data; 106 | using internal::map::size; 107 | }; 108 | 109 | //! \brief The space_map class provides a space interface for an array of 110 | //! points. 111 | template 112 | class space_map : protected internal::map { 113 | using base = internal::map; 114 | 115 | public: 116 | using point_type = typename base::value_type; 117 | using point_element_type = typename base::element_type; 118 | using scalar_type = typename point_traits::scalar_type; 119 | using size_type = size_t; 120 | static size_type constexpr dim = point_traits::dim; 121 | 122 | static_assert( 123 | dim != dynamic_extent, "SPACE_MAP_OF_POINT_DOES_NOT_SUPPORT_DYNAMIC_DIM"); 124 | 125 | using internal::map::operator[]; 126 | using internal::map::data; 127 | using internal::map::size; 128 | 129 | constexpr space_map(point_element_type* data, size_type size) noexcept 130 | : internal::map::map(data, size) {} 131 | 132 | constexpr size_type sdim() const { return dim; } 133 | }; 134 | 135 | //! \brief The space_map class provides a space interface for an array of 136 | //! scalars. 137 | template 138 | class space_map> { 139 | public: 140 | using point_type = point_map; 141 | using scalar_type = typename point_type::scalar_type; 142 | using scalar_element_type = typename point_type::element_type; 143 | using size_type = typename point_type::size_type; 144 | static size_type constexpr dim = point_type::dim; 145 | 146 | constexpr space_map(scalar_element_type* data, size_type size) noexcept 147 | : storage_(data, size, dim) {} 148 | 149 | constexpr space_map( 150 | scalar_element_type* data, size_type size, size_type sdim) noexcept 151 | : storage_(data, size, sdim) {} 152 | 153 | constexpr point_type operator[](size_type i) const noexcept { 154 | return {data(i), storage_.sdim}; 155 | } 156 | 157 | constexpr scalar_element_type* data() const noexcept { return storage_.data; } 158 | 159 | constexpr scalar_element_type* data(size_type i) const noexcept { 160 | return storage_.data + i * storage_.sdim; 161 | } 162 | 163 | constexpr size_type size() const noexcept { return storage_.size; } 164 | 165 | constexpr size_type sdim() const noexcept { return storage_.sdim; } 166 | 167 | protected: 168 | internal::space_map_matrix_storage storage_; 169 | }; 170 | 171 | } // namespace pico_tree 172 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/map_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | //! \file map_traits.hpp 4 | //! \brief Provides an interface for spaces and points when working with raw 5 | //! pointers. 6 | 7 | #include "map.hpp" 8 | #include "space_traits.hpp" 9 | 10 | namespace pico_tree { 11 | 12 | template 13 | struct point_traits> { 14 | using point_type = point_map; 15 | using scalar_type = typename point_type::scalar_type; 16 | using size_type = typename point_type::size_type; 17 | static size_type constexpr dim = Dim_; 18 | 19 | inline static scalar_type const* data(point_type const& point) { 20 | return point.data(); 21 | } 22 | 23 | inline static size_type size(point_type const& point) { return point.size(); } 24 | }; 25 | 26 | //! \brief MapTraits provides an interface for spaces and points when working 27 | //! with a space_map. 28 | template 29 | struct space_traits> { 30 | using space_type = space_map; 31 | using point_type = typename space_type::point_type; 32 | using scalar_type = typename space_type::scalar_type; 33 | using size_type = typename space_type::size_type; 34 | static size_type constexpr dim = space_type::dim; 35 | 36 | template 37 | inline static decltype(auto) point_at(space_type const& space, Index_ idx) { 38 | return space[static_cast(idx)]; 39 | } 40 | 41 | inline static size_type size(space_type const& space) { return space.size(); } 42 | 43 | inline static size_type sdim(space_type const& space) { return space.sdim(); } 44 | }; 45 | 46 | } // namespace pico_tree 47 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/point_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace pico_tree { 4 | 5 | //! \brief point_traits provides an interface for the different point types that 6 | //! are supported by PicoTree. 7 | //! \details Examples of how a point_traits can be created and used are linked 8 | //! below. 9 | //! \tparam Point_ Any of the point types supported by point_traits. 10 | //! \see point_traits 11 | //! \see space_traits> 12 | template 13 | struct point_traits; 14 | 15 | } // namespace pico_tree 16 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/space_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace pico_tree { 7 | 8 | //! \brief space_traits provides an interface for the different space types that 9 | //! are supported by PicoTree. 10 | //! \tparam Space_ Any of the space types supported by space_traits. 11 | template 12 | struct space_traits; 13 | 14 | //! \brief Provides an interface for std::reference_wrapper. 15 | //! \details If Space_ is already a reference type, such as with an Eigen::Map<> 16 | //! or cv::Mat, then using this specialization won't make much sense. 17 | //! \tparam Space_ Any of the space types supported by space_traits. 18 | template 19 | struct space_traits> 20 | : public space_traits> { 21 | //! \brief The space_type of these traits. 22 | //! \details This overrides the space_type of the base class. No other 23 | //! interface changes are required as an std::reference_wrapper can implicitly 24 | //! be converted to its contained reference, which is a reference to an object 25 | //! of the exact same type as that of the space_type of the base class. 26 | using space_type = std::reference_wrapper; 27 | }; 28 | 29 | } // namespace pico_tree 30 | -------------------------------------------------------------------------------- /src/pico_tree/pico_tree/vector_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "core.hpp" 6 | #include "point_traits.hpp" 7 | #include "space_traits.hpp" 8 | 9 | namespace pico_tree { 10 | 11 | //! \brief Provides an interface for std::vector<> and points supported by 12 | //! PointTraits. 13 | //! \tparam Point_ Any of the point types supported by PointTraits. 14 | //! \tparam Allocator_ Allocator type used by the std::vector. 15 | template 16 | struct space_traits> { 17 | //! \brief The space_type of these traits. 18 | using space_type = std::vector; 19 | //! \brief The point type used by space_type. 20 | using point_type = Point_; 21 | //! \brief The scalar type of point coordinates. 22 | using scalar_type = typename point_traits::scalar_type; 23 | //! \brief The size and index type of point coordinates. 24 | using size_type = size_t; 25 | //! \brief Compile time spatial dimension. 26 | static size_type constexpr dim = point_traits::dim; 27 | 28 | static_assert( 29 | dim != dynamic_extent, "VECTOR_OF_POINT_DOES_NOT_SUPPORT_DYNAMIC_DIM"); 30 | 31 | //! \brief Returns the point at \p idx from \p space. 32 | template 33 | inline static Point_ const& point_at(space_type const& space, Index_ idx) { 34 | return space[static_cast(idx)]; 35 | } 36 | 37 | //! \brief Returns number of points contained by \p space. 38 | inline static size_type size(space_type const& space) { return space.size(); } 39 | 40 | //! \brief Returns the number of coordinates or spatial dimension of each 41 | //! point. 42 | inline static size_type constexpr sdim(space_type const&) { return dim; } 43 | }; 44 | 45 | } // namespace pico_tree 46 | -------------------------------------------------------------------------------- /src/pyco_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(NOT SKBUILD) 2 | option(BUILD_BINDINGS "Enable the creation of PicoTree Python bindings." ON) 3 | if(BUILD_BINDINGS) 4 | find_package(Python COMPONENTS Interpreter Development QUIET) 5 | find_package(pybind11 CONFIG QUIET) 6 | find_package(OpenMP QUIET) 7 | if(Python_FOUND AND pybind11_FOUND AND OpenMP_FOUND) 8 | add_subdirectory(pico_tree) 9 | message(STATUS "Python, pybind11 and OpenMP found. Building PycoTree Python Module.") 10 | else() 11 | message(STATUS "Python, pybind11 or OpenMP not found. PycoTree Python Module skipped.") 12 | endif() 13 | endif() 14 | else() 15 | # scikit-build incorrectly detects Python under MSYS2 / MinGW which it then 16 | # "manually" specifies to CMake. To solve this issue we want to ignore some 17 | # of the manually set flags and use find_package(Python) to correct the 18 | # issue. 19 | # The problem starts here: 20 | # https://github.com/scikit-build/scikit-build/blob/master/skbuild/cmaker.py#L358 21 | # The function get_python_library inside cmaker.py uses: 22 | # python_library = sysconfig.get_config_var('LIBRARY') 23 | # It returns a static library, e.g. libpython3.8.a, even though it isn't 24 | # provided via MSYS2. 25 | # https://github.com/msys2/MINGW-packages/issues/3562 26 | 27 | # Ignoring manually specified flags results in a warning. Here follows 28 | # fake usage of the flags. 29 | set(IGNORE_WARNING "${PYTHON_EXECUTABLE}${PYTHON_INCLUDE_DIR}${PYTHON_LIBRARY}") 30 | 31 | # Unset the flags to solve the previously mentioned issue. 32 | unset(PYTHON_EXECUTABLE CACHE) 33 | unset(PYTHON_INCLUDE_DIR CACHE) 34 | unset(PYTHON_LIBRARY CACHE) 35 | 36 | # We want to find the Python version specified by scikit build. This makes 37 | # the build process transparent, but also prevents issues in environments 38 | # where multiple versions of Python are installed. 39 | find_package(Python ${PYTHON_VERSION_STRING} EXACT COMPONENTS Interpreter Development) 40 | find_package(pybind11 CONFIG) 41 | find_package(OpenMP) 42 | 43 | add_subdirectory(pico_tree) 44 | message(STATUS "Building PycoTree Python Module via scikit-build.") 45 | endif() 46 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # The C++ module name equals _pyco_tree in accordance with PEP-8: 2 | # When an extension module written in C or C++ has an accompanying Python 3 | # module that provides a higher level (e.g. more object oriented) interface, 4 | # the C/C++ module has a leading underscore (e.g. _socket). 5 | 6 | # pybind11_add_module seems to assume an MSYS environment when building on a 7 | # Windows machine? 8 | # Configuring using MinGW Makefiles will fail when calling pybind11_add_module 9 | # if $ENV{MSYSTEM} is not defined. This may happen while directly building from 10 | # an IDE such as Visual Studio Code. Solve by adding the following to the 11 | # settings.json of VSC: 12 | # 13 | # "cmake.environment": { 14 | # "MSYSTEM": "MINGW64" 15 | # }, 16 | pybind11_add_module(_pyco_tree MODULE) 17 | target_sources(_pyco_tree 18 | PRIVATE 19 | ${CMAKE_CURRENT_LIST_DIR}/_pyco_tree/_pyco_tree.cpp 20 | ${CMAKE_CURRENT_LIST_DIR}/_pyco_tree/def_darray.cpp 21 | ${CMAKE_CURRENT_LIST_DIR}/_pyco_tree/def_kd_tree.cpp 22 | ) 23 | target_link_libraries(_pyco_tree PRIVATE PicoTree::PicoTree OpenMP::OpenMP_CXX) 24 | 25 | set_target_properties(_pyco_tree 26 | PROPERTIES 27 | LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/py/pico_tree" 28 | ) 29 | 30 | file(GLOB PY_SOURCES_ABS "${CMAKE_CURRENT_LIST_DIR}" *.py) 31 | # TODO These don't get deleted when running: make clean 32 | add_custom_command(TARGET _pyco_tree POST_BUILD 33 | COMMAND ${CMAKE_COMMAND} -E copy ${PY_SOURCES_ABS} ${CMAKE_BINARY_DIR}/py/pico_tree) 34 | 35 | if(SKBUILD) 36 | # scikit-build copies all built targets into setup(cmake_install_dir=) 37 | # while preserving the relative directory as set by install(DESTINATION). 38 | # Target _pyco_tree gets copied directly into the cmake_install_dir with 39 | # its install DESTINATION set to ".". 40 | install(TARGETS _pyco_tree LIBRARY DESTINATION .) 41 | # scikit-build copies all .py files from the setup(package_dir=) argument. 42 | #file(GLOB PY_SOURCES_REL RELATIVE "${CMAKE_CURRENT_LIST_DIR}" *.py) 43 | #install(FILES ${PY_SOURCES_REL} DESTINATION .) 44 | endif() 45 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/__init__.py: -------------------------------------------------------------------------------- 1 | __all__ = ['DArray', 'Metric', 'KdTree', 'load_kd_tree', 'save_kd_tree'] 2 | 3 | # TODO Generate .pyi stub files for _pyco_tree. 4 | 5 | from ._pyco_tree import __doc__, DArray, Metric, KdTree, load_kd_tree, save_kd_tree 6 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/_pyco_tree/_pyco_tree.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "darray.hpp" 6 | #include "def_core.hpp" 7 | #include "def_darray.hpp" 8 | #include "def_kd_tree.hpp" 9 | 10 | PYBIND11_MODULE(_pyco_tree, m) { 11 | m.doc() = 12 | R"ptdoc( 13 | PicoTree: a module for fast nearest neighbor and range searches using a 14 | KdTree. It wraps the C++ PicoTree library. 15 | )ptdoc"; 16 | 17 | // Registered dtypes. 18 | PYBIND11_NUMPY_DTYPE(pyco_tree::neighbor_f, index, distance); 19 | PYBIND11_NUMPY_DTYPE(pyco_tree::neighbor_d, index, distance); 20 | 21 | pyco_tree::def_darray(m); 22 | pyco_tree::def_kd_tree(m); 23 | } 24 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/_pyco_tree/core.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace py = pybind11; 8 | 9 | namespace pyco_tree { 10 | 11 | template 12 | struct string_traits; 13 | 14 | template <> 15 | struct string_traits { 16 | static std::string type_string() { return "float32"; } 17 | }; 18 | 19 | template <> 20 | struct string_traits { 21 | static std::string type_string() { return "float64"; } 22 | }; 23 | 24 | template <> 25 | struct string_traits { 26 | static std::string type_string() { return "L1"; } 27 | }; 28 | 29 | template <> 30 | struct string_traits { 31 | static std::string type_string() { return "L2Squared"; } 32 | }; 33 | 34 | template <> 35 | struct string_traits { 36 | static std::string type_string() { return "LPInf"; } 37 | }; 38 | 39 | inline bool is_row_major(py::array const& array) { 40 | return (array.flags() & py::array::c_style) > 0; 41 | } 42 | 43 | inline bool is_col_major(py::array const& array) { 44 | return (array.flags() & py::array::f_style) > 0; 45 | } 46 | 47 | inline bool is_contiguous(py::array const& array) { 48 | return is_row_major(array) || is_col_major(array); 49 | } 50 | 51 | struct array_layout { 52 | array_layout(py::array const& array) 53 | : info(array.request()), 54 | row_major(is_row_major(array)), 55 | index_inner(row_major ? 1 : 0), 56 | index_outer(row_major ? 0 : 1) {} 57 | 58 | inline py::ssize_t inner_stride() const { return info.shape[index_inner]; } 59 | 60 | inline py::ssize_t outer_stride() const { return info.shape[index_outer]; } 61 | 62 | py::buffer_info info; 63 | bool row_major; 64 | std::size_t index_inner; 65 | std::size_t index_outer; 66 | }; 67 | 68 | template 69 | inline bool is_dim_compatible(pico_tree::size_t d) { 70 | return Dim_ == d; 71 | } 72 | 73 | template <> 74 | inline bool is_dim_compatible(pico_tree::size_t) { 75 | return true; 76 | } 77 | 78 | } // namespace pyco_tree 79 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/_pyco_tree/def_core.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "py_array_map.hpp" 4 | 5 | namespace pyco_tree { 6 | 7 | using space_xf = py_array_map; 8 | using space_xd = py_array_map; 9 | using space_2f = py_array_map; 10 | using space_2d = py_array_map; 11 | using space_3f = py_array_map; 12 | using space_3d = py_array_map; 13 | 14 | template 15 | using space_x = py_array_map; 16 | 17 | using neighbor_f = pico_tree::neighbor; 18 | using neighbor_d = pico_tree::neighbor; 19 | 20 | } // namespace pyco_tree 21 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/_pyco_tree/def_darray.cpp: -------------------------------------------------------------------------------- 1 | #include "def_darray.hpp" 2 | 3 | #include "darray.hpp" 4 | 5 | namespace py = pybind11; 6 | 7 | namespace pyco_tree { 8 | 9 | void def_darray(pybind11::module& m) { 10 | py::class_( 11 | m, 12 | "DArray", 13 | R"ptdoc( 14 | A class whose instance represents a dynamic array of numpy arrays. 15 | Resizing an array and its contents is not possible but the values of 16 | the numpy arrays may be modified. The numpy arrays always have a single 17 | dimension and they don't have to be of equal size. 18 | )ptdoc") 19 | .def( 20 | py::init([](py::object dtype) { 21 | return darray(py::dtype::from_args(dtype)); 22 | }), 23 | py::arg("dtype").none(false), 24 | R"ptdoc( 25 | Create a darray from any object that can be used to construct a numpy 26 | dtype. 27 | )ptdoc") 28 | .def( 29 | "__iter__", 30 | [](darray& a) { return py::make_iterator(a.begin(), a.end()); }, 31 | py::keep_alive<0, 1>(), 32 | "Return an iterator over the contained ndarrays.") 33 | .def( 34 | "__getitem__", 35 | [](darray& a, darray::difference_type i) { 36 | if (i < 0) { 37 | i += a.size(); 38 | } 39 | 40 | if (i < 0 || static_cast(i) >= a.size()) { 41 | throw py::index_error(); 42 | } 43 | 44 | return a[static_cast(i)]; 45 | }, 46 | py::arg("i").none(false), 47 | py::keep_alive<0, 1>(), 48 | "x.__getitem__(y) <==> x[y].") 49 | // Slicing protocol 50 | .def( 51 | "__getitem__", 52 | [](darray const& a, py::slice slice) -> darray { 53 | std::size_t start, stop, step, slice_length; 54 | 55 | if (!slice.compute(a.size(), &start, &stop, &step, &slice_length)) 56 | throw py::error_already_set(); 57 | 58 | return a.copy(start, step, slice_length); 59 | }, 60 | py::arg("s"), 61 | "x.__getitem__(y) <==> x[y].") 62 | .def("__len__", &darray::size, "Return len(self).") 63 | .def_property_readonly( 64 | "dtype", 65 | &darray::dtype, 66 | "Get the dtype of neighbors returned by this array."); 67 | } 68 | 69 | } // namespace pyco_tree 70 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/_pyco_tree/def_darray.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace pyco_tree { 6 | 7 | void def_darray(pybind11::module& m); 8 | 9 | } 10 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/_pyco_tree/def_kd_tree.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace pyco_tree { 6 | 7 | void def_kd_tree(pybind11::module& m); 8 | 9 | } // namespace pyco_tree 10 | -------------------------------------------------------------------------------- /src/pyco_tree/pico_tree/_pyco_tree/py_array_map.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | #include "core.hpp" 9 | 10 | namespace py = pybind11; 11 | 12 | namespace pyco_tree { 13 | 14 | //! \brief The py_array_map class adds the row_major property to the 15 | //! pico_tree::space_map class. 16 | template 17 | class py_array_map 18 | : public pico_tree::space_map> { 19 | public: 20 | using point_type = pico_tree::point_map; 21 | using typename pico_tree::space_map::scalar_type; 22 | using typename pico_tree::space_map::size_type; 23 | using pico_tree::space_map::dim; 24 | 25 | inline py_array_map( 26 | Scalar_* data, size_type npts, size_type sdim, bool row_major) 27 | : pico_tree::space_map(data, npts, sdim), 28 | row_major_(row_major) {} 29 | 30 | inline bool row_major() const { return row_major_; } 31 | 32 | private: 33 | bool row_major_; 34 | }; 35 | 36 | template 37 | py_array_map make_map(py::array const space) { 38 | array_layout layout(space); 39 | 40 | if (layout.info.ndim != 2) { 41 | throw std::invalid_argument("array ndim not 2"); 42 | } 43 | 44 | if (!is_contiguous(space)) { 45 | throw std::invalid_argument("array not contiguous"); 46 | } 47 | 48 | // We always want the memory layout to look like x,y,z,x,y,z,...,x,y,z. 49 | // This means that the shape of the inner dimension should equal the spatial 50 | // dimension of the kd_tree. 51 | if (!is_dim_compatible( 52 | static_cast(layout.inner_stride()))) { 53 | throw std::invalid_argument( 54 | "incompatible kd_tree sdim and array inner stride"); 55 | } 56 | 57 | return py_array_map( 58 | static_cast(layout.info.ptr), 59 | static_cast(layout.outer_stride()), 60 | static_cast(layout.inner_stride()), 61 | layout.row_major); 62 | } 63 | 64 | } // namespace pyco_tree 65 | 66 | namespace pico_tree { 67 | 68 | template 69 | struct space_traits> 70 | : public space_traits>> { 71 | using space_type = pyco_tree::py_array_map; 72 | }; 73 | 74 | } // namespace pico_tree 75 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(pico_tree) 2 | add_subdirectory(pyco_tree) 3 | -------------------------------------------------------------------------------- /test/pico_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(GoogleTest) 2 | 3 | set(TEST_TARGET_NAME ${PROJECT_NAME}_test) 4 | add_executable(${TEST_TARGET_NAME}) 5 | set_default_target_properties(${TEST_TARGET_NAME}) 6 | set_target_properties(${TEST_TARGET_NAME} 7 | PROPERTIES 8 | 9 | # The library should be compliant with the version of the standard that is 10 | # minimally required by this library. 11 | CXX_STANDARD 17 12 | CXX_STANDARD_REQUIRED ON 13 | CXX_EXTENSIONS OFF 14 | ) 15 | include_directories(${CMAKE_CURRENT_LIST_DIR}) 16 | 17 | set(TEST_TARGET_SOURCES 18 | ${CMAKE_CURRENT_LIST_DIR}/box_test.cpp 19 | ${CMAKE_CURRENT_LIST_DIR}/cover_tree_test.cpp 20 | ${CMAKE_CURRENT_LIST_DIR}/distance_test.cpp 21 | ${CMAKE_CURRENT_LIST_DIR}/kd_tree_builder_test.cpp 22 | ${CMAKE_CURRENT_LIST_DIR}/kd_tree_test.cpp 23 | ${CMAKE_CURRENT_LIST_DIR}/metric_test.cpp 24 | ${CMAKE_CURRENT_LIST_DIR}/point_map_test.cpp 25 | ${CMAKE_CURRENT_LIST_DIR}/segment_test.cpp 26 | ${CMAKE_CURRENT_LIST_DIR}/space_map_test.cpp 27 | ${CMAKE_CURRENT_LIST_DIR}/space_map_traits_test.cpp 28 | ${CMAKE_CURRENT_LIST_DIR}/vector_traits_test.cpp 29 | ) 30 | 31 | # gtest_add_tests fails on generator expressions like: 32 | # $<$:${CMAKE_CURRENT_LIST_DIR}/eigen.cpp> 33 | find_package(Eigen3 QUIET) 34 | 35 | if(Eigen3_FOUND) 36 | message(STATUS "Eigen3 found. Building Eigen unit tests.") 37 | set(TEST_TARGET_SOURCES 38 | ${TEST_TARGET_SOURCES} 39 | ${CMAKE_CURRENT_LIST_DIR}/eigen3_traits_test.cpp 40 | ) 41 | else() 42 | message(STATUS "Eigen3 not found. Eigen unit tests skipped.") 43 | endif() 44 | 45 | find_package(OpenCV QUIET) 46 | 47 | if(OpenCV_FOUND) 48 | message(STATUS "OpenCV found. Building OpenCV unit tests.") 49 | set(TEST_TARGET_SOURCES 50 | ${TEST_TARGET_SOURCES} 51 | ${CMAKE_CURRENT_LIST_DIR}/opencv_traits_test.cpp 52 | ) 53 | else() 54 | message(STATUS "OpenCV not found. OpenCV unit tests skipped.") 55 | endif() 56 | 57 | target_sources(${TEST_TARGET_NAME} PRIVATE ${TEST_TARGET_SOURCES}) 58 | target_link_libraries(${TEST_TARGET_NAME} 59 | ${PROJECT_NAME} 60 | pico_toolshed 61 | 62 | # Testing the understory structures here as well. 63 | pico_understory 64 | GTest::GTest 65 | GTest::Main 66 | $<$:Eigen3::Eigen> 67 | $<$:opencv_core> 68 | ) 69 | 70 | gtest_add_tests( 71 | TARGET ${TEST_TARGET_NAME} 72 | TEST_LIST ${TEST_TARGET_NAME}_list 73 | ) 74 | -------------------------------------------------------------------------------- /test/pico_tree/box_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace pico_tree; 6 | using namespace pico_tree::internal; 7 | 8 | namespace { 9 | 10 | constexpr size_t dynamic_box_dim = 4; 11 | 12 | inline size_t constexpr dimension(size_t d) { 13 | return d != dynamic_extent ? d : dynamic_box_dim; 14 | } 15 | 16 | } // namespace 17 | 18 | template 19 | class BoxTest : public testing::Test {}; 20 | 21 | template 22 | class BoxTest> : public testing::Test { 23 | public: 24 | BoxTest() : box_(dimension(Dim_)) {} 25 | 26 | protected: 27 | box box_; 28 | }; 29 | 30 | template 31 | class BoxTest> : public testing::Test { 32 | public: 33 | BoxTest() 34 | : min_(dimension(Dim_)), 35 | max_(dimension(Dim_)), 36 | box_(min_.data(), max_.data(), dimension(Dim_)) {} 37 | 38 | protected: 39 | std::vector min_; 40 | std::vector max_; 41 | box_map box_; 42 | }; 43 | 44 | using BoxTestTypes = testing::Types< 45 | box, 46 | box, 47 | box, 48 | box_map, 49 | box_map, 50 | box_map>; 51 | 52 | TYPED_TEST_SUITE(BoxTest, BoxTestTypes); 53 | 54 | TYPED_TEST(BoxTest, size) { 55 | size_t dim = TypeParam::dim; 56 | if (dim != dynamic_extent) { 57 | EXPECT_EQ(this->box_.size(), dim); 58 | } else { 59 | EXPECT_EQ(this->box_.size(), dynamic_box_dim); 60 | } 61 | } 62 | 63 | TYPED_TEST(BoxTest, Accessors) { 64 | this->box_.fill_inverse_max(); 65 | for (size_t i = 0; i < this->box_.size(); ++i) { 66 | EXPECT_EQ(this->box_.min(i), this->box_.min()[i]); 67 | EXPECT_EQ(this->box_.max(i), this->box_.max()[i]); 68 | } 69 | } 70 | 71 | TYPED_TEST(BoxTest, FillInverseMax) { 72 | using scalar_type = typename TypeParam::scalar_type; 73 | 74 | this->box_.fill_inverse_max(); 75 | for (size_t i = 0; i < this->box_.size(); ++i) { 76 | EXPECT_EQ(this->box_.min(i), std::numeric_limits::max()); 77 | EXPECT_EQ(this->box_.max(i), std::numeric_limits::lowest()); 78 | } 79 | } 80 | 81 | TYPED_TEST(BoxTest, Fit) { 82 | using scalar_type = typename TypeParam::scalar_type; 83 | 84 | this->box_.fill_inverse_max(); 85 | size_t dim = dimension(TypeParam::dim); 86 | 87 | // Fit a point. 88 | std::vector p(dim, scalar_type(0)); 89 | this->box_.fit(p.data()); 90 | for (size_t i = 0; i < this->box_.size(); ++i) { 91 | EXPECT_EQ(this->box_.min(i), scalar_type(0)); 92 | EXPECT_EQ(this->box_.max(i), scalar_type(0)); 93 | } 94 | 95 | // Fit a box. 96 | std::vector min(dim, scalar_type(-1)); 97 | std::vector max(dim, scalar_type(+1)); 98 | this->box_.fit( 99 | box_map(min.data(), max.data(), dim)); 100 | for (size_t i = 0; i < this->box_.size(); ++i) { 101 | EXPECT_EQ(this->box_.min(i), min[i]); 102 | EXPECT_EQ(this->box_.max(i), max[i]); 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /test/pico_tree/cover_tree_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "common.hpp" 8 | 9 | // The anonymous namespace gives the function a unique "name" when there is 10 | // another one with the exact same signature. 11 | namespace { 12 | 13 | template 14 | using space = std::reference_wrapper>; 15 | 16 | template 17 | using cover_tree = pico_tree::cover_tree>; 18 | 19 | template 20 | void query_radius( 21 | std::size_t const point_count, 22 | typename Point_::scalar_type const area_size, 23 | typename Point_::scalar_type const radius) { 24 | using scalar_type = typename Point_::scalar_type; 25 | 26 | std::vector random = 27 | pico_tree::generate_random_n(point_count, area_size); 28 | cover_tree tree(random, scalar_type(2.0)); 29 | 30 | test_radius(tree, radius); 31 | } 32 | 33 | template 34 | void query_knn( 35 | std::size_t const point_count, 36 | typename Point_::scalar_type const area_size, 37 | pico_tree::size_t const k) { 38 | using scalar_type = typename Point_::scalar_type; 39 | 40 | std::vector random = 41 | pico_tree::generate_random_n(point_count, area_size); 42 | cover_tree tree(random, scalar_type(2.0)); 43 | 44 | // This line compile time "tests" the move capability of the tree. 45 | auto tree2 = std::move(tree); 46 | 47 | test_knn(tree2, k); 48 | } 49 | 50 | } // namespace 51 | 52 | TEST(CoverTreeTest, QueryRadiusSubset2d) { 53 | query_radius(1024 * 128, 100.0f, 2.5f); 54 | } 55 | 56 | TEST(CoverTreeTest, QueryKnn1) { 57 | query_knn(1024 * 128, 100.0f, 1); 58 | } 59 | 60 | TEST(CoverTreeTest, QueryKnn10) { 61 | query_knn(1024 * 128, 100.0f, 10); 62 | } 63 | -------------------------------------------------------------------------------- /test/pico_tree/distance_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace pt = pico_tree; 6 | 7 | namespace { 8 | 9 | inline constexpr float abs_error = 0.00001f; 10 | 11 | } 12 | 13 | TEST(DistanceTest, R1Distance) { 14 | EXPECT_NEAR(pt::r1_distance(1.9f, 1.1f), 0.8f, abs_error); 15 | EXPECT_NEAR(pt::r1_distance(1.1f, 1.9f), 0.8f, abs_error); 16 | } 17 | 18 | TEST(DistanceTest, S1Distance) { 19 | EXPECT_NEAR(pt::s1_distance(0.4f, 0.6f), 0.2f, abs_error); 20 | EXPECT_NEAR(pt::s1_distance(0.6f, 0.4f), 0.2f, abs_error); 21 | EXPECT_NEAR(pt::s1_distance(0.1f, 0.9f), 0.2f, abs_error); 22 | } 23 | 24 | TEST(DistanceTest, Squared) { 25 | EXPECT_NEAR(pt::squared(0.8f), 0.64f, abs_error); 26 | } 27 | 28 | TEST(DistanceTest, SquaredR1Distance) { 29 | EXPECT_NEAR(pt::squared_r1_distance(1.9f, 1.1f), 0.64f, abs_error); 30 | EXPECT_NEAR(pt::squared_r1_distance(1.1f, 1.9f), 0.64f, abs_error); 31 | } 32 | 33 | TEST(DistanceTest, SquaredS1Distance) { 34 | EXPECT_NEAR(pt::squared_s1_distance(0.4f, 0.6f), 0.04f, abs_error); 35 | EXPECT_NEAR(pt::squared_s1_distance(0.6f, 0.4f), 0.04f, abs_error); 36 | EXPECT_NEAR(pt::squared_s1_distance(0.1f, 0.9f), 0.04f, abs_error); 37 | } 38 | -------------------------------------------------------------------------------- /test/pico_tree/eigen3_traits_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "common.hpp" 6 | 7 | template 8 | void check_eigen_adaptor_interface() { 9 | ColMatrix_ col_matrix = ColMatrix_::Random(4, 8); 10 | RowMatrix_ row_matrix = RowMatrix_::Random(4, 8); 11 | 12 | check_space_adaptor( 13 | ColMatrix_::RowsAtCompileTime)>( 14 | std::cref(col_matrix), 15 | static_cast(col_matrix.rows()), 16 | static_cast(col_matrix.cols()), 17 | static_cast(col_matrix.cols() - 1), 18 | col_matrix.col(col_matrix.cols() - 1).data()); 19 | check_space_adaptor( 20 | RowMatrix_::ColsAtCompileTime)>( 21 | std::cref(row_matrix), 22 | static_cast(row_matrix.cols()), 23 | static_cast(row_matrix.rows()), 24 | static_cast(row_matrix.rows() - 1), 25 | row_matrix.row(row_matrix.rows() - 1).data()); 26 | } 27 | 28 | TEST(Eigen3TraitsTest, Interface) { 29 | // Spatial dimension known. 30 | check_eigen_adaptor_interface< 31 | Eigen::Matrix, 32 | Eigen::Matrix>(); 33 | // Spatial dimension dynamic. 34 | check_eigen_adaptor_interface< 35 | Eigen::Matrix, 36 | Eigen::Matrix>(); 37 | } 38 | -------------------------------------------------------------------------------- /test/pico_tree/kd_tree_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "common.hpp" 10 | 11 | // The anonymous namespace gives the function a unique "name" when there is 12 | // another one with the exact same signature. 13 | namespace { 14 | 15 | template 16 | using space = std::reference_wrapper>; 17 | 18 | template 19 | using kd_tree = pico_tree::kd_tree>; 20 | 21 | template 22 | void query_range( 23 | std::size_t const point_count, 24 | typename Point_::scalar_type const area_size, 25 | typename Point_::scalar_type const min_v, 26 | typename Point_::scalar_type const max_v) { 27 | std::vector random = 28 | pico_tree::generate_random_n(point_count, area_size); 29 | kd_tree tree(random, pico_tree::max_leaf_size_t(8)); 30 | 31 | test_box(tree, min_v, max_v); 32 | } 33 | 34 | template 35 | void query_radius( 36 | std::size_t const point_count, 37 | typename Point_::scalar_type const area_size, 38 | typename Point_::scalar_type const radius) { 39 | std::vector random = 40 | pico_tree::generate_random_n(point_count, area_size); 41 | kd_tree tree(random, pico_tree::max_leaf_size_t(8)); 42 | 43 | test_radius(tree, radius); 44 | } 45 | 46 | template 47 | void query_knn( 48 | std::size_t const point_count, 49 | typename Point_::scalar_type const area_size, 50 | pico_tree::size_t k) { 51 | std::vector random = 52 | pico_tree::generate_random_n(point_count, area_size); 53 | kd_tree tree1(random, pico_tree::max_leaf_size_t(8)); 54 | 55 | // "Test" move constructor. 56 | auto tree2 = std::move(tree1); 57 | // "Test" move assignment. 58 | tree1 = std::move(tree2); 59 | 60 | test_knn(tree1, k); 61 | } 62 | 63 | } // namespace 64 | 65 | TEST(KdTreeTest, QueryRangeSubset2d) { 66 | query_range(1024 * 1024, 100.0f, 15.1f, 34.9f); 67 | } 68 | 69 | TEST(KdTreeTest, QueryRangeAll2d) { 70 | query_range(1024, 10.0f, 0.0f, 10.0f); 71 | } 72 | 73 | TEST(KdTreeTest, QueryRadiusSubset2d) { 74 | query_radius(1024 * 1024, 100.0f, 2.5f); 75 | } 76 | 77 | TEST(KdTreeTest, QueryKnn1) { 78 | query_knn(1024 * 1024, 100.0f, 1); 79 | } 80 | 81 | TEST(KdTreeTest, QueryKnn10) { 82 | query_knn(1024 * 1024, 100.0f, 10); 83 | } 84 | 85 | TEST(KdTreeTest, QuerySo2Knn4) { 86 | using point_type = pico_tree::point_1f; 87 | using scalar_type = typename point_type::scalar_type; 88 | using space_type = space; 89 | 90 | std::vector random = pico_tree::generate_random_n( 91 | 256 * 256, scalar_type(0.0), scalar_type(1.0)); 92 | pico_tree::kd_tree tree( 93 | random, pico_tree::max_leaf_size_t(10)); 94 | 95 | test_knn(tree, 8, point_type{1.0}); 96 | test_box(tree, scalar_type(0.90), scalar_type(1.00)); 97 | test_box(tree, scalar_type(0.95), scalar_type(0.05)); 98 | } 99 | 100 | TEST(KdTreeTest, WriteRead) { 101 | using point_type = pico_tree::point_2f; 102 | using index_type = int; 103 | using scalar_type = typename point_type::scalar_type; 104 | std::size_t point_count = 100; 105 | scalar_type area_size = 2; 106 | std::vector random = 107 | pico_tree::generate_random_n(point_count, area_size); 108 | 109 | std::string filename = "tree.bin"; 110 | 111 | // Compile time known dimensions. 112 | { 113 | // The points are not stored. 114 | kd_tree tree(random, pico_tree::max_leaf_size_t(1)); 115 | kd_tree::save(tree, filename); 116 | } 117 | { 118 | // Points are required to load the tree. 119 | kd_tree tree = kd_tree::load(random, filename); 120 | test_knn(tree, index_type(20)); 121 | } 122 | 123 | EXPECT_TRUE(std::filesystem::remove(filename)); 124 | 125 | // Run time known dimensions. 126 | using space_type = pico_tree::dynamic_space>; 127 | 128 | space_type drandom(random); 129 | 130 | { 131 | static_assert( 132 | pico_tree::kd_tree::dim == pico_tree::dynamic_extent, 133 | "KD_TREE_DIM_NOT_DYNAMIC"); 134 | // The points are not stored. 135 | pico_tree::kd_tree tree(drandom, pico_tree::max_leaf_size_t(1)); 136 | pico_tree::kd_tree::save(tree, filename); 137 | } 138 | { 139 | // Points are required to load the tree. 140 | pico_tree::kd_tree tree = 141 | pico_tree::kd_tree::load(drandom, filename); 142 | test_knn(tree, 20); 143 | } 144 | 145 | EXPECT_TRUE(std::filesystem::remove(filename)); 146 | } 147 | 148 | TEST(KdTreeTest, LeafRanges) { 149 | using point_type = pico_tree::point_2f; 150 | using scalar_type = typename point_type::scalar_type; 151 | using index_type = int; 152 | std::size_t point_count = 100; 153 | scalar_type area_size = 2; 154 | std::vector random = 155 | pico_tree::generate_random_n(point_count, area_size); 156 | 157 | kd_tree tree(random, pico_tree::max_leaf_depth_t(2)); 158 | 159 | auto leaf_ranges = tree.leaf_ranges(); 160 | EXPECT_EQ(leaf_ranges.size(), 4); 161 | if (!leaf_ranges.empty()) { 162 | std::ptrdiff_t sum_range_point_count = 0; 163 | index_type index_sum = 0; 164 | for (auto const& r : leaf_ranges) { 165 | sum_range_point_count += std::distance(r.begin(), r.end()); 166 | for (int i : r) { 167 | index_sum += i; 168 | } 169 | } 170 | EXPECT_EQ(static_cast(sum_range_point_count), point_count); 171 | EXPECT_EQ( 172 | static_cast(index_sum), 173 | (point_count - 1) * point_count / 2); 174 | EXPECT_EQ( 175 | std::distance(leaf_ranges.front().begin(), leaf_ranges.back().end()), 176 | point_count); 177 | } 178 | } 179 | -------------------------------------------------------------------------------- /test/pico_tree/metric_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace pti = pico_tree::internal; 9 | 10 | template 11 | inline auto distance(Metric_ const& metric, P0_ const& p0, P1_ const& p1) { 12 | auto w0 = pti::point_wrapper(p0); 13 | auto w1 = pti::point_wrapper(p1); 14 | return metric(w0.begin(), w0.end(), w1.begin()); 15 | } 16 | 17 | TEST(MetricTest, L1) { 18 | pico_tree::point_2f p0{2.0f, 4.0f}; 19 | pico_tree::point_2f p1{10.0f, 1.0f}; 20 | 21 | pico_tree::metric_l1 metric; 22 | 23 | EXPECT_FLOAT_EQ(distance(metric, p0, p1), 11.0f); 24 | EXPECT_FLOAT_EQ(metric(-3.1f), 3.1f); 25 | } 26 | 27 | TEST(MetricTest, L2) { 28 | pico_tree::point_2f p0{7.0f, 5.0f}; 29 | pico_tree::point_2f p1{10.0f, 1.0f}; 30 | 31 | pico_tree::metric_l2 metric; 32 | 33 | EXPECT_FLOAT_EQ(distance(metric, p0, p1), 5.0f); 34 | EXPECT_FLOAT_EQ(metric(-3.1f), 3.1f); 35 | } 36 | 37 | TEST(MetricTest, L2Squared) { 38 | pico_tree::point_2f p0{2.0f, 4.0f}; 39 | pico_tree::point_2f p1{10.0f, 1.0f}; 40 | 41 | pico_tree::metric_l2_squared metric; 42 | 43 | EXPECT_FLOAT_EQ(distance(metric, p0, p1), 73.0f); 44 | EXPECT_FLOAT_EQ(metric(-3.1f), 9.61f); 45 | } 46 | 47 | TEST(MetricTest, LPInf) { 48 | pico_tree::point_2f p0{2.0f, 4.0f}; 49 | pico_tree::point_2f p1{10.0f, 1.0f}; 50 | 51 | pico_tree::metric_lpinf metric; 52 | 53 | EXPECT_FLOAT_EQ(distance(metric, p0, p1), 8.0f); 54 | EXPECT_FLOAT_EQ(metric(-3.1f), 3.1f); 55 | } 56 | 57 | TEST(MetricTest, LNInf) { 58 | pico_tree::point_2f p0{2.0f, 4.0f}; 59 | pico_tree::point_2f p1{10.0f, 1.0f}; 60 | 61 | pico_tree::metric_lninf metric; 62 | 63 | EXPECT_FLOAT_EQ(distance(metric, p0, p1), 3.0f); 64 | EXPECT_FLOAT_EQ(metric(-3.1f), 3.1f); 65 | } 66 | 67 | TEST(MetricTest, SO2) { 68 | pico_tree::point_1f p0{0.5f}; 69 | pico_tree::point_1f p1{0.6f}; 70 | 71 | pico_tree::metric_so2 metric; 72 | 73 | EXPECT_FLOAT_EQ(distance(metric, p0, p1), 0.1f); 74 | EXPECT_FLOAT_EQ(metric(-0.1f), 0.1f); 75 | } 76 | 77 | TEST(MetricTest, SE2Squared) { 78 | // These two points are the concatenation of the points of 79 | // MetricTest.L2Squared and MetricTest.SO2, where the third coordinate is the 80 | // SO2 angle. 81 | pico_tree::point_3f p0{2.0f, 4.0f, 0.5f}; 82 | pico_tree::point_3f p1{10.0f, 1.0f, 0.6f}; 83 | 84 | pico_tree::metric_se2_squared metric; 85 | 86 | // This is the same as the sum of distances from MetricTest.L2Squared and 87 | // MetricTest.SO2 (squared). 88 | EXPECT_FLOAT_EQ(distance(metric, p0, p1), 73.0f + 0.01f); 89 | EXPECT_FLOAT_EQ(metric(-0.1f), 0.01f); 90 | } 91 | -------------------------------------------------------------------------------- /test/pico_tree/opencv_traits_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include "common.hpp" 6 | 7 | TEST(OpenCvTraitsTest, Interface) { 8 | using scalar_type = float; 9 | constexpr pico_tree::size_t dim = 3; 10 | 11 | cv::Mat matrix(8, 3, cv::DataType::type); 12 | cv::randu(matrix, -scalar_type(1.0), scalar_type(1.0)); 13 | cv::Mat row = matrix.row(matrix.rows - 1); 14 | pico_tree::opencv_mat_map space(matrix); 15 | 16 | check_space_adaptor( 17 | space, 18 | static_cast(matrix.cols), 19 | static_cast(matrix.rows), 20 | static_cast(matrix.rows - 1), 21 | row.ptr()); 22 | 23 | static_assert(space.sdim() == dim); 24 | } 25 | -------------------------------------------------------------------------------- /test/pico_tree/point_map_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace pico_tree; 8 | 9 | namespace { 10 | 11 | size_t constexpr dynamic_map_dim = 4; 12 | 13 | constexpr size_t dimension(size_t d) { 14 | return d != dynamic_extent ? d : dynamic_map_dim; 15 | } 16 | 17 | } // namespace 18 | 19 | template 20 | class PointMapTest : public testing::Test {}; 21 | 22 | template 23 | class PointMapTest> : public testing::Test { 24 | public: 25 | static constexpr size_t dim = dimension(Dim_); 26 | 27 | PointMapTest() : map_(scalars_.data(), dim) { 28 | std::iota(scalars_.begin(), scalars_.end(), Scalar_(0.0)); 29 | } 30 | 31 | protected: 32 | std::array scalars_; 33 | point_map map_; 34 | }; 35 | 36 | using PointMapTypes = testing::Types< 37 | point_map, 38 | point_map, 39 | point_map>; 40 | 41 | TYPED_TEST_SUITE(PointMapTest, PointMapTypes); 42 | 43 | TYPED_TEST(PointMapTest, Accessors) { 44 | using scalar_type = typename TypeParam::scalar_type; 45 | 46 | for (size_t i = 0; i < this->map_.size(); ++i) { 47 | EXPECT_EQ(this->map_[i], scalar_type(i)); 48 | } 49 | } 50 | 51 | TYPED_TEST(PointMapTest, size) { 52 | EXPECT_EQ(this->map_.size(), this->scalars_.size()); 53 | } 54 | -------------------------------------------------------------------------------- /test/pico_tree/segment_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace pti = pico_tree::internal; 6 | 7 | namespace { 8 | 9 | inline constexpr float abs_error = 0.00001f; 10 | 11 | } 12 | 13 | TEST(SegmentR1Test, Distance) { 14 | EXPECT_NEAR( 15 | pti::segment_r1(0.6f, 0.9f).distance(0.7f), 0.0f, abs_error); 16 | EXPECT_NEAR( 17 | pti::segment_r1(0.4f, 0.6f).distance(0.3f), 0.1f, abs_error); 18 | } 19 | 20 | TEST(SegmentS1Test, DistanceMinMax) { 21 | EXPECT_NEAR( 22 | pti::segment_s1(0.4f, 0.6f).distance_min_max(0.3f), 23 | 0.1f, 24 | abs_error); 25 | EXPECT_NEAR( 26 | pti::segment_s1(0.4f, 0.6f).distance_min_max(0.5f), 27 | 0.0f, 28 | abs_error); 29 | EXPECT_NEAR( 30 | pti::segment_s1(0.9f, 1.0f).distance_min_max(0.1f), 31 | 0.1f, 32 | abs_error); 33 | } 34 | 35 | TEST(SegmentS1Test, DistanceMaxMin) { 36 | EXPECT_NEAR( 37 | pti::segment_s1(0.9f, 0.1f).distance_max_min(0.0f), 38 | 0.0f, 39 | abs_error); 40 | EXPECT_NEAR( 41 | pti::segment_s1(0.9f, 0.1f).distance_max_min(0.2f), 42 | 0.1f, 43 | abs_error); 44 | } 45 | 46 | TEST(SegmentS1Test, Distance) { 47 | EXPECT_NEAR( 48 | pti::segment_s1(0.9f, 0.1f).distance(0.2f), 0.1f, abs_error); 49 | EXPECT_NEAR( 50 | pti::segment_s1(0.4f, 0.6f).distance(0.3f), 0.1f, abs_error); 51 | } 52 | -------------------------------------------------------------------------------- /test/pico_tree/space_map_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace pico_tree; 8 | 9 | namespace { 10 | 11 | size_t constexpr dynamic_map_dim = 4; 12 | 13 | constexpr size_t dimension(size_t d) { 14 | return d != dynamic_extent ? d : dynamic_map_dim; 15 | } 16 | 17 | } // namespace 18 | 19 | template 20 | class SpaceMapTest : public testing::Test {}; 21 | 22 | template 23 | class SpaceMapTest> : public testing::Test { 24 | public: 25 | SpaceMapTest() : map_(points_.data(), points_.size()) { 26 | std::size_t count = 0; 27 | for (auto& point : points_) { 28 | for (auto& coord : point) { 29 | coord = typename space_map::scalar_type(count); 30 | count++; 31 | } 32 | } 33 | } 34 | 35 | Point_ const& point_at(std::size_t index) const { return points_[index]; } 36 | 37 | std::size_t size() const { return points_.size(); } 38 | 39 | constexpr std::size_t sdim() const { return point_traits::dim; } 40 | 41 | protected: 42 | std::array points_; 43 | space_map map_; 44 | }; 45 | 46 | template 47 | class SpaceMapTest>> : public testing::Test { 48 | public: 49 | static constexpr size_t dim = dimension(Dim_); 50 | 51 | SpaceMapTest() : map_(coords_.data(), coords_.size() / dim, dim) { 52 | std::size_t count = 0; 53 | for (auto& coord : coords_) { 54 | coord = Scalar_(count); 55 | count++; 56 | } 57 | } 58 | 59 | point_map point_at(std::size_t index) const { 60 | return {coords_.data() + index * dim, dim}; 61 | } 62 | 63 | std::size_t size() const { return coords_.size() / dim; } 64 | 65 | constexpr std::size_t sdim() const { return dim; } 66 | 67 | protected: 68 | std::array coords_; 69 | space_map> map_; 70 | }; 71 | 72 | using SpaceMapTypes = testing::Types< 73 | space_map>, 74 | space_map>, 75 | space_map>, 76 | space_map>>; 77 | 78 | TYPED_TEST_SUITE(SpaceMapTest, SpaceMapTypes); 79 | 80 | TYPED_TEST(SpaceMapTest, Accessors) { 81 | for (size_t i = 0; i < this->map_.size(); ++i) { 82 | EXPECT_EQ(this->map_[i].data(), this->point_at(i).data()); 83 | } 84 | } 85 | 86 | TYPED_TEST(SpaceMapTest, size) { EXPECT_EQ(this->map_.size(), this->size()); } 87 | 88 | TYPED_TEST(SpaceMapTest, sdim) { EXPECT_EQ(this->map_.sdim(), this->sdim()); } 89 | -------------------------------------------------------------------------------- /test/pico_tree/space_map_traits_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "common.hpp" 7 | 8 | TEST(SpaceMapTraitsTest, PointMap) { 9 | constexpr pico_tree::size_t dim = 2; 10 | std::vector scalars = {1.0, 2.0, 3.0, 4.0}; 11 | pico_tree::space_map> map_ct( 12 | scalars.data(), scalars.size() / dim); 13 | pico_tree::space_map> 14 | map_rt(scalars.data(), scalars.size() / dim, dim); 15 | 16 | check_space_adaptor( 17 | map_ct, 18 | map_ct.sdim(), 19 | map_ct.size(), 20 | static_cast(0), 21 | map_ct[0].data()); 22 | check_space_adaptor( 23 | map_rt, 24 | map_rt.sdim(), 25 | map_rt.size(), 26 | static_cast(0), 27 | map_rt[0].data()); 28 | } 29 | 30 | TEST(SpaceMapTraitsTest, Point) { 31 | std::vector points = {{1.0f, 2.0f}, {3.0f, 4.0f}}; 32 | pico_tree::space_map map(points.data(), points.size()); 33 | 34 | check_space_adaptor( 35 | map, 36 | map.sdim(), 37 | map.size(), 38 | static_cast(0), 39 | map[0].data()); 40 | } 41 | -------------------------------------------------------------------------------- /test/pico_tree/vector_traits_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "common.hpp" 7 | 8 | TEST(VectorTraitsTest, Interface) { 9 | std::vector points = {{1.0f, 2.0f}, {3.0f, 4.0f}}; 10 | 11 | check_space_adaptor( 12 | points, 13 | pico_tree::point_2f::dim, 14 | points.size(), 15 | static_cast(0), 16 | points[0].data()); 17 | // VectorTraitsTest is used to test the default std::reference_wrapper<> 18 | // specialization. 19 | check_space_adaptor( 20 | std::ref(points), 21 | pico_tree::point_2f::dim, 22 | points.size(), 23 | static_cast(0), 24 | points[0].data()); 25 | // VectorTraitsTest is used to test the default std::reference_wrapper 26 | // specialization. 27 | check_space_adaptor( 28 | std::cref(points), 29 | pico_tree::point_2f::dim, 30 | points.size(), 31 | static_cast(0), 32 | points[0].data()); 33 | } 34 | -------------------------------------------------------------------------------- /test/pyco_tree/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(BUILD_BINDINGS) 2 | file(GLOB PY_SOURCES_ABS "${CMAKE_CURRENT_LIST_DIR}/" *.py) 3 | 4 | # TODO These don't get deleted when running: make clean 5 | add_custom_target(python_tests ALL 6 | COMMAND ${CMAKE_COMMAND} -E copy ${PY_SOURCES_ABS} ${CMAKE_BINARY_DIR}/py) 7 | endif() 8 | --------------------------------------------------------------------------------