├── .github └── workflows │ └── ci.yml ├── .gitignore ├── CHANGELOG.md ├── Cargo.toml ├── LICENSE-APACHE ├── LICENSE-MIT ├── README.md ├── benches ├── candidate_container_comparison.rs └── simple.rs ├── cspell.json └── src ├── heap.rs ├── infinite.rs ├── internal_neighbour.rs ├── internal_parameters.rs ├── lib.rs ├── node.rs └── simple_point.rs /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | on: [push, pull_request] 2 | 3 | name: CI 4 | 5 | jobs: 6 | check: 7 | name: Check 8 | runs-on: ubuntu-latest 9 | steps: 10 | - name: Checkout sources 11 | uses: actions/checkout@v2 12 | 13 | - name: Install stable toolchain 14 | uses: actions-rs/toolchain@v1 15 | with: 16 | profile: minimal 17 | toolchain: stable 18 | override: true 19 | 20 | - name: Run cargo check 21 | uses: actions-rs/cargo@v1 22 | with: 23 | command: check 24 | 25 | test: 26 | name: Test Suite 27 | runs-on: ubuntu-latest 28 | steps: 29 | - name: Checkout sources 30 | uses: actions/checkout@v2 31 | 32 | - name: Install stable toolchain 33 | uses: actions-rs/toolchain@v1 34 | with: 35 | profile: minimal 36 | toolchain: stable 37 | override: true 38 | 39 | - name: Run cargo test 40 | uses: actions-rs/cargo@v1 41 | with: 42 | command: test 43 | 44 | lints: 45 | name: Lints 46 | runs-on: ubuntu-latest 47 | steps: 48 | - name: Checkout sources 49 | uses: actions/checkout@v2 50 | 51 | - name: Install stable toolchain 52 | uses: actions-rs/toolchain@v1 53 | with: 54 | profile: minimal 55 | toolchain: stable 56 | override: true 57 | components: rustfmt, clippy 58 | 59 | - name: Run cargo fmt 60 | uses: actions-rs/cargo@v1 61 | with: 62 | command: fmt 63 | args: --all -- --check 64 | 65 | - name: Run cargo clippy 66 | uses: actions-rs/cargo@v1 67 | with: 68 | command: clippy 69 | args: -- -D warnings 70 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | Cargo.lock 3 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | ### Unreleased 2 | 3 | ### 0.5.0 - 2024-12-20 4 | 5 | * Generalized `simple_point`'s `P2` struct to the `SimplePoint` struct generic over its `D` dimensions. 6 | 7 | ### 0.4.0 - 2024-12-20 8 | 9 | * Made `nabo` `no_std` compatible. 10 | * Renamed `dummy_point` as `simple_point`. 11 | * Added functions to iterate over the points of the tree. 12 | 13 | ### 0.3.0 - 2023-06-28 14 | 15 | * Derive `Clone` trait for `KDTree`. 16 | * Switched to Rust 2021 edition. 17 | * Updated `ordered-float` to version 3.7 and `criterion` to version 0.5. 18 | 19 | ### 0.2.1 - 2021-09-13 20 | 21 | * Fixed linear candidate container to return less than k entries rather than entries with infinity if not enough neighbours can be found. 22 | 23 | ### 0.2.0 - 2021-09-06 24 | 25 | * Improved documentation. 26 | * Made naming of functions in `dummy_point` and unit tests consistent. 27 | 28 | ### 0.1.0 - 2021-09-03 29 | 30 | * Initial re-implementation of the C++ library and adaptation to Rust. 31 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "nabo" 3 | version = "0.5.0" 4 | edition = "2021" 5 | authors = ["Stéphane Magnenat ", "Hannes Sommer "] 6 | license = "MIT OR Apache-2.0" 7 | description = "A fast K Nearest Neighbor (KNN) library for low-dimensional spaces" 8 | repository = "https://github.com/enlightware/nabo-rs" 9 | homepage = "https://github.com/enlightware/nabo-rs" 10 | readme = "README.md" 11 | keywords = ["nearest_neighbor", "K-D_tree", "data_structures", "KNN", "no_std"] 12 | categories = ["mathematics", "science", "computer-vision", "multimedia", "game-development"] 13 | rust-version = "1.63" 14 | 15 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 16 | 17 | [features] 18 | default = ["rand"] 19 | rand = ["dep:rand"] 20 | 21 | [dependencies] 22 | partition = "0.1.2" 23 | num-traits = { version = "0.2", default-features = false, features = ["libm"] } 24 | ordered-float = { version = "4.6", default-features = false, features = ["libm"] } 25 | rand = { version = "0.8", optional = true } 26 | 27 | [dev-dependencies] 28 | rand = "0.8" 29 | float-cmp = "0.10" 30 | criterion = "0.5" 31 | 32 | [profile.release] 33 | debug = 1 34 | 35 | [[bench]] 36 | name = "candidate_container_comparison" 37 | harness = false 38 | required-features = ["rand"] 39 | 40 | [[bench]] 41 | name = "simple" 42 | harness = false 43 | required-features = ["rand"] 44 | -------------------------------------------------------------------------------- /LICENSE-APACHE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | -------------------------------------------------------------------------------- /LICENSE-MIT: -------------------------------------------------------------------------------- 1 | Copyright (c) 2020 nabo contributors 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # nabo 2 | 3 | [![Crates.io][crates-badge]][crates-url] 4 | [![Docs.rs][docs-badge]][docs-url] 5 | [![Build Status][ci-badge]][ci-url] 6 | 7 | [crates-badge]: https://img.shields.io/crates/v/nabo 8 | [crates-url]: https://crates.io/crates/nabo 9 | [docs-badge]: https://img.shields.io/docsrs/nabo 10 | [docs-url]: https://docs.rs/nabo 11 | [ci-badge]: https://github.com/enlightware/nabo-rs/actions/workflows/ci.yml/badge.svg 12 | [ci-url]: https://github.com/enlightware/nabo-rs/actions 13 | 14 | ## Overview 15 | 16 | nabo is a fast K Nearest Neighbour (KNN) library for low-dimensional spaces. 17 | It is a re-implementation in pure Rust of the [C++ library of the same name](https://github.com/ethz-asl/libnabo) by its [original author](http://stephane.magnenat.net). 18 | This work has been sponsored by [Enlightware GmbH](https://enlightware.ch). 19 | 20 | nabo is `no_std` compatible. 21 | 22 | ## Usage 23 | 24 | To use nabo in your project, you need to either: 25 | - Use `nabo::simple_point::SimplePoint` as your point type. 26 | - Implement the `nabo::Point` trait for your own point type. 27 | 28 | If you want to avoid a dependency to `rand`, disable the `rand` feature. 29 | In that case, the random generation of point clouds for `SimplePoint` will not be available. 30 | 31 | ## Benchmark 32 | 33 | You can benchmark nabo using the following command: 34 | 35 | cargo bench 36 | 37 | ## Citing nabo 38 | 39 | If you use nabo in the academic context, please cite this paper that evaluates its performances in the context of robotics mapping research: 40 | 41 | @article{elsebergcomparison, 42 | title={Comparison of nearest-neighbor-search strategies and implementations for efficient shape registration}, 43 | author={Elseberg, J. and Magnenat, S. and Siegwart, R. and N{\"u}chter, A.}, 44 | journal={Journal of Software Engineering for Robotics (JOSER)}, 45 | pages={2--12}, 46 | volume={3}, 47 | number={1}, 48 | year={2012}, 49 | issn={2035-3928} 50 | } 51 | 52 | ## License 53 | 54 | Licensed under either of 55 | 56 | * Apache License, Version 2.0 57 | ([LICENSE-APACHE](LICENSE-APACHE) or http://www.apache.org/licenses/LICENSE-2.0) 58 | * MIT license 59 | ([LICENSE-MIT](LICENSE-MIT) or http://opensource.org/licenses/MIT) 60 | 61 | at your option. 62 | 63 | ## Contribution 64 | 65 | Unless you explicitly state otherwise, any contribution intentionally submitted 66 | for inclusion in this project by you, as defined in the Apache-2.0 license, 67 | shall be dual licensed as above, without any additional terms or conditions. 68 | -------------------------------------------------------------------------------- /benches/candidate_container_comparison.rs: -------------------------------------------------------------------------------- 1 | use std::time::Duration; 2 | 3 | use criterion::{criterion_group, criterion_main, BenchmarkId, Criterion}; 4 | use nabo::simple_point::*; 5 | use nabo::CandidateContainer; 6 | use nabo::KDTree; 7 | use nabo::Parameters; 8 | 9 | fn bench_candidate_container_types(c: &mut Criterion) { 10 | const QUERY_COUNT: u32 = 10000; 11 | const CLOUD_SIZE: u32 = 1000000; 12 | const PARAMETERS: Parameters = Parameters { 13 | epsilon: 0.0, 14 | max_radius: f32::INFINITY, 15 | allow_self_match: true, 16 | sort_results: true, 17 | }; 18 | let cloud = random_point_cloud::<2>(CLOUD_SIZE); 19 | let tree = KDTree::new(&cloud); 20 | let queries = (0..QUERY_COUNT).map(|_| random_point()).collect::>(); 21 | let mut group = c.benchmark_group("CandidateContainerType"); 22 | group.warm_up_time(Duration::from_secs(1)); 23 | group.measurement_time(Duration::from_secs(2)); 24 | for k in [1, 2, 3, 4, 6, 8, 11, 16, 24, 37, 50, 100] { 25 | group.bench_with_input( 26 | BenchmarkId::new("BinaryHeap", k), 27 | &(k, &tree, &queries), 28 | |b, (k, tree, queries)| { 29 | b.iter(|| { 30 | for query in *queries { 31 | #[rustfmt::skip] 32 | tree.knn_advanced( 33 | *k, query, 34 | CandidateContainer::BinaryHeap, 35 | &PARAMETERS, 36 | None, 37 | ); 38 | } 39 | }) 40 | }, 41 | ); 42 | group.bench_with_input( 43 | BenchmarkId::new("Linear", k), 44 | &(k, &tree, &queries), 45 | |b, (k, tree, queries)| { 46 | b.iter(|| { 47 | for query in *queries { 48 | #[rustfmt::skip] 49 | tree.knn_advanced( 50 | *k, query, 51 | CandidateContainer::Linear, 52 | &PARAMETERS, 53 | None, 54 | ); 55 | } 56 | }) 57 | }, 58 | ); 59 | } 60 | group.finish(); 61 | } 62 | 63 | criterion_group!(benches, bench_candidate_container_types); 64 | criterion_main!(benches); 65 | -------------------------------------------------------------------------------- /benches/simple.rs: -------------------------------------------------------------------------------- 1 | #![no_std] 2 | 3 | extern crate alloc; 4 | 5 | use alloc::vec::Vec; 6 | use nabo::simple_point::*; 7 | use nabo::KDTree; 8 | 9 | fn main() { 10 | const QUERY_COUNT: u32 = 20000; 11 | const CLOUD_SIZE: u32 = 1000000; 12 | let cloud = random_point_cloud::<2>(CLOUD_SIZE); 13 | let tree = KDTree::new(&cloud); 14 | let queries = (0..QUERY_COUNT).map(|_| random_point()).collect::>(); 15 | for k in [1, 2, 3, 4, 6, 8, 11, 16, 24] { 16 | for query in &queries { 17 | tree.knn(k, query); 18 | } 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /cspell.json: -------------------------------------------------------------------------------- 1 | { 2 | "language": "en-GB", 3 | "words": [ 4 | "clippy", 5 | "rustfmt", 6 | "nabo", 7 | "libnabo", 8 | "Enlightware", 9 | "ulps" 10 | ] 11 | } 12 | -------------------------------------------------------------------------------- /src/heap.rs: -------------------------------------------------------------------------------- 1 | use alloc::collections::BinaryHeap; 2 | use alloc::vec; 3 | use alloc::vec::Vec; 4 | 5 | use ordered_float::NotNan; 6 | 7 | use crate::internal_neighbour::InternalNeighbour; 8 | use crate::Scalar; 9 | 10 | /// Local trait for keeping candidates in a heap-like behaviour 11 | pub(crate) trait CandidateHeap { 12 | fn new_with_k(k: u32) -> Self; 13 | fn add(&mut self, dist2: NotNan, index: u32); 14 | fn furthest_dist2(&self) -> NotNan; 15 | fn into_vec(self) -> Vec>; 16 | fn into_sorted_vec(self) -> Vec>; 17 | } 18 | 19 | impl CandidateHeap for BinaryHeap> { 20 | fn new_with_k(k: u32) -> Self { 21 | BinaryHeap::with_capacity(k as usize) 22 | } 23 | fn add(&mut self, dist2: NotNan, index: u32) { 24 | let k = self.capacity(); 25 | if self.len() < k { 26 | self.push(InternalNeighbour { index, dist2 }); 27 | } else { 28 | let mut max_heap_value = self.peek_mut().unwrap(); 29 | if dist2 < max_heap_value.dist2 { 30 | *max_heap_value = InternalNeighbour { index, dist2 }; 31 | } 32 | } 33 | } 34 | fn furthest_dist2(&self) -> NotNan { 35 | if self.len() < self.capacity() { 36 | NotNan::new(T::infinity()).unwrap() 37 | } else { 38 | self.peek() 39 | .map_or(NotNan::new(T::infinity()).unwrap(), |n| n.dist2) 40 | } 41 | } 42 | fn into_vec(self) -> Vec> { 43 | BinaryHeap::into_vec(self) 44 | } 45 | fn into_sorted_vec(self) -> Vec> { 46 | BinaryHeap::into_sorted_vec(self) 47 | } 48 | } 49 | 50 | fn keep_finite_elements(v: Vec>) -> Vec> { 51 | let pos = v.iter().position(|n| n.dist2.into_inner() == T::infinity()); 52 | match pos { 53 | None => v, 54 | Some(pos) => { 55 | let mut v = v; 56 | v.truncate(pos); 57 | v 58 | } 59 | } 60 | } 61 | 62 | impl CandidateHeap for Vec> { 63 | fn new_with_k(k: u32) -> Self { 64 | vec![ 65 | InternalNeighbour { 66 | index: 0, 67 | dist2: NotNan::new(T::infinity()).unwrap() 68 | }; 69 | k as usize 70 | ] 71 | } 72 | fn add(&mut self, dist2: NotNan, index: u32) { 73 | if dist2 > self.furthest_dist2() { 74 | return; 75 | } 76 | let mut i = self.len() - 1; 77 | while i > 0 { 78 | if self[i - 1].dist2 > dist2 { 79 | self[i] = self[i - 1]; 80 | } else { 81 | break; 82 | } 83 | i -= 1; 84 | } 85 | self[i].dist2 = dist2; 86 | self[i].index = index; 87 | } 88 | fn furthest_dist2(&self) -> NotNan { 89 | self[self.len() - 1].dist2 90 | } 91 | fn into_vec(self) -> Vec> { 92 | keep_finite_elements(self) 93 | } 94 | fn into_sorted_vec(self) -> Vec> { 95 | keep_finite_elements(self) 96 | } 97 | } 98 | 99 | impl CandidateHeap for InternalNeighbour { 100 | fn new_with_k(k: u32) -> Self { 101 | debug_assert_eq!(k, 1); 102 | InternalNeighbour { 103 | index: 0, 104 | dist2: NotNan::new(T::infinity()).unwrap(), 105 | } 106 | } 107 | fn add(&mut self, dist2: NotNan, index: u32) { 108 | if dist2 < self.dist2 { 109 | self.dist2 = dist2; 110 | self.index = index; 111 | } 112 | } 113 | fn furthest_dist2(&self) -> NotNan { 114 | self.dist2 115 | } 116 | fn into_vec(self) -> Vec> { 117 | vec![self] 118 | } 119 | fn into_sorted_vec(self) -> Vec> { 120 | vec![self] 121 | } 122 | } 123 | 124 | #[cfg(test)] 125 | mod tests { 126 | use crate::infinite::HasInfinite; 127 | use crate::*; 128 | #[test] 129 | fn keep_finite_elements() { 130 | let v = vec![ 131 | InternalNeighbour { 132 | index: 0, 133 | dist2: NotNan::::zero(), 134 | }, 135 | InternalNeighbour { 136 | index: 0, 137 | dist2: NotNan::::infinite(), 138 | }, 139 | InternalNeighbour { 140 | index: 0, 141 | dist2: NotNan::::infinite(), 142 | }, 143 | ]; 144 | let finite_v = crate::heap::keep_finite_elements(v); 145 | assert_eq!( 146 | finite_v, 147 | vec![InternalNeighbour { 148 | index: 0, 149 | dist2: NotNan::::zero(), 150 | }] 151 | ); 152 | } 153 | } 154 | -------------------------------------------------------------------------------- /src/infinite.rs: -------------------------------------------------------------------------------- 1 | /// Local trait for providing infinity, working-around the Rust trait impl limitations 2 | pub(crate) trait HasInfinite { 3 | fn infinite() -> Self; 4 | } 5 | -------------------------------------------------------------------------------- /src/internal_neighbour.rs: -------------------------------------------------------------------------------- 1 | use core::cmp::Ordering; 2 | use ordered_float::{FloatCore, NotNan}; 3 | 4 | use crate::infinite::HasInfinite; 5 | 6 | /// An internal representation of neighbour, to avoid copying the point around 7 | #[derive(Clone, Copy, Debug)] 8 | pub(crate) struct InternalNeighbour { 9 | /// the index of this point 10 | pub(crate) index: u32, 11 | /// the distance to the point 12 | pub(crate) dist2: NotNan, 13 | } 14 | impl Default for InternalNeighbour { 15 | fn default() -> Self { 16 | InternalNeighbour { 17 | index: 0, 18 | dist2: NotNan::::infinite(), 19 | } 20 | } 21 | } 22 | 23 | impl Eq for InternalNeighbour {} 24 | 25 | impl Ord for InternalNeighbour { 26 | fn cmp(&self, other: &Self) -> Ordering { 27 | self.dist2 28 | .cmp(&other.dist2) 29 | .then_with(|| self.index.cmp(&other.index)) 30 | } 31 | } 32 | impl PartialOrd for InternalNeighbour { 33 | fn partial_cmp(&self, other: &Self) -> Option { 34 | Some(self.cmp(other)) 35 | } 36 | } 37 | impl PartialEq for InternalNeighbour { 38 | fn eq(&self, other: &Self) -> bool { 39 | self.index == other.index 40 | } 41 | } 42 | 43 | impl HasInfinite for NotNan { 44 | fn infinite() -> Self { 45 | NotNan::new(T::infinity()).unwrap() 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /src/internal_parameters.rs: -------------------------------------------------------------------------------- 1 | use ordered_float::{FloatCore, NotNan}; 2 | 3 | /// Parameters to be passed unchanged to internal recursive function 4 | pub(crate) struct InternalParameters { 5 | pub(crate) max_error2: NotNan, 6 | pub(crate) max_radius2: NotNan, 7 | pub(crate) allow_self_match: bool, 8 | } 9 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | #![no_std] 2 | #![warn(missing_docs)] 3 | 4 | //! A fast K Nearest Neighbour library for low-dimensional spaces. 5 | //! 6 | //! This crate is a re-implementation in pure Rust of the [C++ library of the same name](https://github.com/ethz-asl/libnabo). 7 | //! This work has been sponsored by [Enlightware GmbH](https://enlightware.ch). 8 | //! 9 | //! # Example 10 | //! ``` 11 | //! use nabo::simple_point::*; 12 | //! use nabo::KDTree; 13 | //! let cloud = random_point_cloud::<2>(10000); 14 | //! let tree = KDTree::new(&cloud); 15 | //! let query = random_point(); 16 | //! let neighbour = tree.knn(3, &query); 17 | //! ``` 18 | //! 19 | //! If you want to have more control on the search, you can use the advanced API: 20 | //! ``` 21 | //! use nabo::simple_point::*; 22 | //! use nabo::KDTree; 23 | //! use nabo::CandidateContainer; 24 | //! use nabo::Parameters; 25 | //! let cloud = random_point_cloud::<2>(10000); 26 | //! let tree = KDTree::new(&cloud); 27 | //! let query = random_point(); 28 | //! let mut touch_count = 0; 29 | //! let neighbour = tree.knn_advanced( 30 | //! 3, 31 | //! &query, 32 | //! CandidateContainer::BinaryHeap, 33 | //! &Parameters { 34 | //! epsilon: 0.0, 35 | //! max_radius: 10.0, 36 | //! allow_self_match: true, 37 | //! sort_results: false, 38 | //! }, 39 | //! Some(&mut touch_count) // statistics 40 | //! ); 41 | //! ``` 42 | 43 | // We forbid the clippy lint here because it suggests to use #[rustfmt::skip], 44 | // which is experimental. See: https://github.com/rust-lang/rust/issues/88591 45 | #![allow(clippy::deprecated_cfg_attr)] 46 | 47 | extern crate alloc; 48 | 49 | mod heap; 50 | mod infinite; 51 | mod internal_neighbour; 52 | mod internal_parameters; 53 | mod node; 54 | pub mod simple_point; 55 | 56 | use alloc::{collections::BinaryHeap, vec, vec::Vec}; 57 | use core::ops::AddAssign; 58 | use internal_parameters::InternalParameters; 59 | use node::Node; 60 | use num_traits::{clamp_max, clamp_min, Bounded, Zero}; 61 | use ordered_float::FloatCore; 62 | pub use ordered_float::{FloatIsNan, NotNan}; 63 | 64 | use heap::CandidateHeap; 65 | use internal_neighbour::InternalNeighbour; 66 | 67 | /// The scalar type for points in the space to be searched 68 | pub trait Scalar: FloatCore + AddAssign + core::fmt::Debug {} 69 | impl Scalar for T {} 70 | 71 | /// A point in the space to be searched 72 | pub trait Point: Default { 73 | /// Sets the value for the `i`-th component, `i` must be within `0..DIM`. 74 | fn set(&mut self, i: u32, value: NotNan); 75 | /// Gets the value for the `i`-th component, `i` must be within `0..DIM`. 76 | fn get(&self, i: u32) -> NotNan; 77 | /// The number of dimension of the space this point lies in. 78 | const DIM: u32; 79 | /// Derived from `DIM`, do not reimplement, use the default! 80 | const DIM_BIT_COUNT: u32 = 32 - Self::DIM.leading_zeros(); 81 | /// Derived from `DIM`, do not reimplement, use the default! 82 | const DIM_MASK: u32 = (1 << Self::DIM_BIT_COUNT) - 1; 83 | /// Derived from `DIM`, do not reimplement, use the default! 84 | const MAX_NODE_COUNT: u32 = ((1u64 << (32 - Self::DIM_BIT_COUNT)) - 1) as u32; 85 | 86 | /// Construct from a slice of valid axis values. 87 | /// 88 | /// If the slice is too short, the point will be right-filled as `Point::default()`. 89 | /// if it is too long, the extra elements will be ignored. 90 | fn from_slice(values: &[NotNan]) -> Self { 91 | let mut p = Self::default(); 92 | for (idx, v) in values.iter().take(Self::DIM as usize).enumerate() { 93 | p.set(idx as u32, *v); 94 | } 95 | p 96 | } 97 | 98 | /// Construct from a slice of raw axis values. 99 | /// 100 | /// If the slice is too short, the point will be right-filled as `Point::default()`. 101 | /// if it is too long, the extra elements will be ignored. 102 | fn from_raw(values: &[T]) -> Result { 103 | let mut p = Self::default(); 104 | for (idx, v) in values.iter().take(Self::DIM as usize).enumerate() { 105 | p.set(idx as u32, NotNan::new(*v)?); 106 | } 107 | Ok(p) 108 | } 109 | } 110 | 111 | /// Helper function to compute the square distance between two points given as slice 112 | #[inline] 113 | fn point_slice_dist2>(lhs: &[NotNan], rhs: &[NotNan]) -> NotNan { 114 | let mut dist2 = NotNan::::zero(); 115 | for index in 0..P::DIM { 116 | let index = index as usize; 117 | let diff = lhs[index] - rhs[index]; 118 | dist2 += diff * diff; 119 | } 120 | dist2 121 | } 122 | 123 | /// The index of a point in the original point cloud 124 | pub type Index = u32; 125 | 126 | /// A neighbour resulting from the search 127 | #[derive(Debug)] 128 | pub struct Neighbour> { 129 | /// the point itself 130 | pub point: P, 131 | /// the squared-distance to the point 132 | pub dist2: NotNan, 133 | /// the index of the point in the original point cloud 134 | pub index: Index, 135 | } 136 | 137 | /// The type of container to keep candidates 138 | pub enum CandidateContainer { 139 | /// use a linear vector to keep candidates, good for small k 140 | Linear, 141 | /// use a binary heap to keep candidates, good for large k 142 | BinaryHeap, 143 | } 144 | 145 | /// Advanced search parameters 146 | pub struct Parameters { 147 | /// maximal ratio of error for approximate search, 0 for exact search; has no effect if the number of neighbours found is smaller than the number requested 148 | pub epsilon: T, 149 | /// maximum radius in which to search, can be used to prune search, is not affected by `epsilon` 150 | pub max_radius: T, 151 | /// allows the return of the same point as the query, if this point is in the point cloud 152 | pub allow_self_match: bool, 153 | /// sort points by distances, when `k` > 1 154 | pub sort_results: bool, 155 | } 156 | 157 | /// A dense vector of search nodes, provides better memory performances than many small objects 158 | type Nodes = Vec>; 159 | 160 | /// A KD-Tree to perform NN-search queries 161 | /// 162 | /// This implementation is inspired of the variant `KDTreeUnbalancedPtInLeavesImplicitBoundsStackOpt` in libnabo C++. 163 | /// Contrary to the latter, it does not keep a reference to the point cloud but copies the point. 164 | /// It retains their index though. 165 | #[derive(Clone, Debug)] 166 | pub struct KDTree> { 167 | /// size of a bucket 168 | bucket_size: u32, 169 | /// search nodes 170 | nodes: Nodes, 171 | /// point data, size cloud.len() * P::DIM 172 | points: Vec>, 173 | /// indices in cloud , size cloud.len() 174 | indices: Vec, 175 | } 176 | 177 | impl> KDTree { 178 | /// Creates a new KD-Tree from a point cloud. 179 | pub fn new(cloud: &[P]) -> Self { 180 | KDTree::new_with_bucket_size(cloud, 8) 181 | } 182 | /// Creates a new KD-Tree from a point cloud. 183 | /// 184 | /// The `bucket_size` can be chosen freely, but must be at least 2. 185 | pub fn new_with_bucket_size(cloud: &[P], bucket_size: u32) -> Self { 186 | // validate input 187 | if bucket_size < 2 { 188 | panic!( 189 | "Bucket size must be at least 2, but {} was passed", 190 | bucket_size 191 | ); 192 | } 193 | if cloud.len() > u32::MAX as usize { 194 | panic!( 195 | "Point cloud is larger than maximum possible size {}", 196 | u32::MAX 197 | ); 198 | } 199 | let estimated_node_count = (cloud.len() / (bucket_size as usize / 2)) as u32; 200 | if estimated_node_count > P::MAX_NODE_COUNT { 201 | panic!("Point cloud has a risk to have more nodes {} than the kd-tree allows {}. The kd-tree has {} bits for dimensions and {} bits for node indices", estimated_node_count, P::MAX_NODE_COUNT, P::DIM_BIT_COUNT, 32 - P::DIM_BIT_COUNT); 202 | } 203 | 204 | // build point vector and compute bounds 205 | let mut build_points: Vec<_> = (0..cloud.len()).collect(); 206 | 207 | // create and populate tree 208 | let mut tree = KDTree { 209 | bucket_size, 210 | nodes: Vec::with_capacity(estimated_node_count as usize), 211 | points: Vec::with_capacity(cloud.len() * P::DIM as usize), 212 | indices: Vec::with_capacity(cloud.len()), 213 | }; 214 | tree.build_nodes(cloud, &mut build_points); 215 | tree 216 | } 217 | 218 | /// Finds the `k` nearest neighbour of `query`, using reasonable default parameters. 219 | /// 220 | /// If there are less than `k` points in the point cloud, the returned vector will be smaller than `k`. 221 | /// The default parameters are: 222 | /// Exact search, no max. radius, allowing self matching, sorting results, and not collecting statistics. 223 | /// If `k` <= 16, a linear vector is used to keep track of candidates, otherwise a binary heap is used. 224 | pub fn knn(&self, k: u32, query: &P) -> Vec> { 225 | let candidate_container = if k <= 16 { 226 | CandidateContainer::Linear 227 | } else { 228 | CandidateContainer::BinaryHeap 229 | }; 230 | #[cfg_attr(rustfmt, rustfmt_skip)] 231 | self.knn_advanced( 232 | k, query, 233 | candidate_container, 234 | &Parameters { 235 | epsilon: T::from(0.0).unwrap(), 236 | max_radius: T::infinity(), 237 | allow_self_match: true, 238 | sort_results: true, 239 | }, 240 | None, 241 | ) 242 | } 243 | 244 | /// Finds the `k` nearest neighbour of `query`, with user-provided parameters. 245 | /// 246 | /// If there are less than `k` points in the point cloud or in the ball around `query` 247 | /// defined by `parameters.max_radius`, the returned vector will be smaller than `k`. 248 | /// The parameters are: 249 | /// * `candidate_container` which container to use to collect candidates, 250 | /// * `parameters` the advanced search parameters, 251 | /// * `touch_statistics`, if `Some(&mut u32)`, return the number of point touched in the provided `u32` reference. 252 | pub fn knn_advanced( 253 | &self, 254 | k: u32, 255 | query: &P, 256 | candidate_container: CandidateContainer, 257 | parameters: &Parameters, 258 | touch_statistics: Option<&mut u32>, 259 | ) -> Vec> { 260 | #[cfg_attr(rustfmt, rustfmt_skip)] 261 | (match candidate_container { 262 | CandidateContainer::Linear => Self::knn_generic_heap::>>, 263 | CandidateContainer::BinaryHeap => Self::knn_generic_heap::>> 264 | })( 265 | self, 266 | k, query, 267 | parameters, touch_statistics 268 | ) 269 | } 270 | 271 | fn knn_generic_heap>( 272 | &self, 273 | k: u32, 274 | query: &P, 275 | parameters: &Parameters, 276 | touch_statistics: Option<&mut u32>, 277 | ) -> Vec> { 278 | let query_as_vec: Vec<_> = (0..P::DIM).map(|i| query.get(i)).collect(); 279 | let Parameters { 280 | epsilon, 281 | max_radius, 282 | allow_self_match, 283 | sort_results, 284 | } = *parameters; 285 | let max_error = epsilon + T::from(1).unwrap(); 286 | let max_error2 = NotNan::new(max_error * max_error).unwrap(); 287 | let max_radius2 = NotNan::new(max_radius * max_radius).unwrap(); 288 | #[cfg_attr(rustfmt, rustfmt_skip)] 289 | self.knn_internal::( 290 | k, &query_as_vec, 291 | &InternalParameters { max_error2, max_radius2, allow_self_match }, 292 | sort_results, touch_statistics, 293 | ) 294 | .into_iter() 295 | .map(|n| self.externalise_neighbour(n)) 296 | .collect() 297 | } 298 | 299 | fn knn_internal>( 300 | &self, 301 | k: u32, 302 | query: &[NotNan], 303 | internal_parameters: &InternalParameters, 304 | sort_results: bool, 305 | touch_statistics: Option<&mut u32>, 306 | ) -> Vec> { 307 | // TODO Const generics: once available, remove `vec!` below. 308 | let mut off = vec![NotNan::::zero(); P::DIM as usize]; 309 | let mut heap = H::new_with_k(k); 310 | #[cfg_attr(rustfmt, rustfmt_skip)] 311 | let leaf_touched_count = self.recurse_knn( 312 | query, 313 | 0, NotNan::::zero(), 314 | &mut heap, &mut off, 315 | internal_parameters, 316 | ); 317 | if let Some(touch_statistics) = touch_statistics { 318 | *touch_statistics = leaf_touched_count; 319 | } 320 | if sort_results { 321 | heap.into_sorted_vec() 322 | } else { 323 | heap.into_vec() 324 | } 325 | } 326 | 327 | #[allow(clippy::too_many_arguments)] 328 | fn recurse_knn>( 329 | &self, 330 | query: &[NotNan], 331 | node: usize, 332 | rd: NotNan, 333 | heap: &mut H, 334 | off: &mut [NotNan], 335 | internal_parameters: &InternalParameters, 336 | ) -> u32 { 337 | self.nodes[node].dispatch_on_type( 338 | heap, 339 | |heap, split_dim, split_val, right_child| { 340 | // split node, see whether we have to recurse 341 | let mut rd = rd; 342 | let split_dim = split_dim as usize; 343 | let old_off = off[split_dim]; 344 | let new_off = query[split_dim] - split_val; 345 | let left_child = node + 1; 346 | let right_child = right_child as usize; 347 | let InternalParameters { 348 | max_radius2, 349 | max_error2, 350 | .. 351 | } = *internal_parameters; 352 | if new_off > NotNan::::zero() { 353 | #[cfg_attr(rustfmt, rustfmt_skip)] 354 | let mut leaf_visited_count = self.recurse_knn( 355 | query, 356 | right_child, rd, 357 | heap, off, 358 | internal_parameters, 359 | ); 360 | rd += new_off * new_off - old_off * old_off; 361 | if rd <= max_radius2 && rd * max_error2 < heap.furthest_dist2() { 362 | off[split_dim] = new_off; 363 | #[cfg_attr(rustfmt, rustfmt_skip)] 364 | let new_visits= self.recurse_knn( 365 | query, 366 | left_child, rd, 367 | heap, off, 368 | internal_parameters, 369 | ); 370 | leaf_visited_count += new_visits; 371 | off[split_dim] = old_off; 372 | } 373 | leaf_visited_count 374 | } else { 375 | #[cfg_attr(rustfmt, rustfmt_skip)] 376 | let mut leaf_visited_count = self.recurse_knn( 377 | query, 378 | left_child, rd, 379 | heap, off, 380 | internal_parameters, 381 | ); 382 | rd += new_off * new_off - old_off * old_off; 383 | if rd <= max_radius2 && rd * max_error2 < heap.furthest_dist2() { 384 | off[split_dim] = new_off; 385 | #[cfg_attr(rustfmt, rustfmt_skip)] 386 | let new_visits = self.recurse_knn( 387 | query, 388 | right_child, rd, 389 | heap, off, 390 | internal_parameters, 391 | ); 392 | leaf_visited_count += new_visits; 393 | off[split_dim] = old_off; 394 | } 395 | leaf_visited_count 396 | } 397 | }, 398 | |heap, bucket_start_index, bucket_size| { 399 | // leaf node, go through the buckets and check elements 400 | let bucket_end_index = bucket_start_index + bucket_size; 401 | for bucket_index in bucket_start_index..bucket_end_index { 402 | let point_index = (bucket_index * P::DIM) as usize; 403 | let point = &self.points[point_index..point_index + (P::DIM as usize)]; 404 | let dist2 = point_slice_dist2::(query, point); 405 | let epsilon = NotNan::new(T::epsilon()).unwrap(); 406 | let InternalParameters { 407 | max_radius2, 408 | allow_self_match, 409 | .. 410 | } = *internal_parameters; 411 | if dist2 < max_radius2 && (allow_self_match || (dist2 > epsilon)) { 412 | heap.add(dist2, bucket_index); 413 | } 414 | } 415 | bucket_size 416 | }, 417 | ) 418 | } 419 | 420 | fn build_nodes(&mut self, cloud: &[P], build_points: &mut [usize]) -> usize { 421 | let count = build_points.len() as u32; 422 | let pos = self.nodes.len(); 423 | 424 | // if remaining points fit in a single bucket, add a node and this bucket 425 | if count <= self.bucket_size { 426 | let bucket_start_index = self.indices.len() as u32; 427 | self.points.reserve(build_points.len() * P::DIM as usize); 428 | self.indices.reserve(build_points.len()); 429 | for point_index in build_points { 430 | let point_index = *point_index; 431 | self.indices.push(point_index as u32); 432 | for i in 0..P::DIM { 433 | self.points.push(cloud[point_index].get(i)); 434 | } 435 | } 436 | self.nodes 437 | .push(Node::new_leaf_node(bucket_start_index, count)); 438 | return pos; 439 | } 440 | 441 | // compute bounds 442 | let (min_bounds, max_bounds) = Self::get_build_points_bounds(cloud, build_points); 443 | 444 | // find the largest dimension of the box 445 | let split_dim = Self::max_delta_index(&min_bounds, &max_bounds); 446 | let split_dim_u = split_dim as usize; 447 | 448 | // split along this dimension 449 | let split_val = (max_bounds[split_dim_u] + min_bounds[split_dim_u]) * T::from(0.5).unwrap(); 450 | let range = max_bounds[split_dim_u] - min_bounds[split_dim_u]; 451 | let (left_points, right_points) = if range == T::from(0).unwrap() { 452 | // degenerate data, split in half and iterate 453 | build_points.split_at_mut(build_points.len() / 2) 454 | } else { 455 | // partition data around split_val on split_dim 456 | partition::partition(build_points, |index| { 457 | cloud[*index].get(split_dim) < split_val 458 | }) 459 | }; 460 | debug_assert_ne!(left_points.len(), 0); 461 | debug_assert_ne!(right_points.len(), 0); 462 | 463 | // add this split 464 | self.nodes.push(Node::new_split_node(split_dim, split_val)); 465 | 466 | // recurse 467 | let left_child = self.build_nodes(cloud, left_points); 468 | debug_assert_eq!(left_child, pos + 1); 469 | let right_child = self.build_nodes(cloud, right_points); 470 | 471 | // write right child index and return 472 | self.nodes[pos].set_child_index(right_child as u32); 473 | pos 474 | } 475 | 476 | fn get_build_points_bounds( 477 | cloud: &[P], 478 | build_points: &[usize], 479 | ) -> (Vec>, Vec>) { 480 | let mut min_bounds = vec![NotNan::::max_value(); P::DIM as usize]; 481 | let mut max_bounds = vec![NotNan::::min_value(); P::DIM as usize]; 482 | for p_index in build_points { 483 | let p = &cloud[*p_index]; 484 | for index in 0..P::DIM { 485 | let index_u = index as usize; 486 | min_bounds[index_u] = clamp_max(p.get(index), min_bounds[index_u]); 487 | max_bounds[index_u] = clamp_min(p.get(index), max_bounds[index_u]); 488 | } 489 | } 490 | (min_bounds, max_bounds) 491 | } 492 | 493 | fn max_delta_index(lower_bound: &[NotNan], upper_bound: &[NotNan]) -> u32 { 494 | lower_bound 495 | .iter() 496 | .zip(upper_bound.iter()) 497 | .enumerate() 498 | .max_by_key(|(_, (l, u))| *u - *l) 499 | .unwrap() 500 | .0 as u32 501 | } 502 | 503 | fn externalise_neighbour(&self, neighbour: InternalNeighbour) -> Neighbour { 504 | let mut point = P::default(); 505 | let base_index = neighbour.index * P::DIM; 506 | for i in 0..P::DIM { 507 | point.set(i, self.points[(base_index + i) as usize]); 508 | } 509 | Neighbour { 510 | point, 511 | dist2: neighbour.dist2, 512 | index: self.indices[neighbour.index as usize], 513 | } 514 | } 515 | 516 | /// Iterate over the indices and points in this KDTree. 517 | /// The order is arbitrary; 518 | /// the indices are the point's location in the slice 519 | /// from which the tree was built. 520 | pub fn iter_idx_points(&self) -> impl Iterator + '_ { 521 | self.indices.iter().cloned().zip(self.iter_points()) 522 | } 523 | 524 | /// Iterate over the points in this KDTree in arbitrary order. 525 | pub fn iter_points(&self) -> impl Iterator + '_ { 526 | self.points 527 | .as_slice() 528 | .chunks(P::DIM as usize) 529 | .map(P::from_slice) 530 | } 531 | } 532 | 533 | #[cfg(test)] 534 | mod tests { 535 | extern crate std; 536 | use crate::*; 537 | use float_cmp::approx_eq; 538 | use simple_point::{random_point, random_point_cloud, P2}; 539 | use std::{dbg, println}; 540 | 541 | // helpers to create cloud 542 | fn cloud3() -> Vec { 543 | vec![P2::new2d(0., 0.), P2::new2d(-1., 3.), P2::new2d(2., -4.)] 544 | } 545 | 546 | // helper to compute the square distance between two points 547 | fn point_dist2>(lhs: &P, rhs: &P) -> NotNan { 548 | let mut dist2 = NotNan::::zero(); 549 | for index in 0..P::DIM { 550 | let diff = lhs.get(index) - rhs.get(index); 551 | dist2 += diff * diff; 552 | } 553 | dist2 554 | } 555 | 556 | // brute force search implementations 557 | fn brute_force_1nn(cloud: &[P2], query: &P2) -> Neighbour { 558 | let mut best_dist2 = f32::infinity(); 559 | let mut best_index = 0; 560 | for (index, point) in cloud.iter().enumerate() { 561 | let dist2 = point_dist2(point, query).into_inner(); 562 | if dist2 < best_dist2 { 563 | best_dist2 = dist2; 564 | best_index = index; 565 | } 566 | } 567 | Neighbour { 568 | point: cloud[best_index], 569 | dist2: NotNan::new(best_dist2).unwrap(), 570 | index: best_index as u32, 571 | } 572 | } 573 | 574 | fn brute_force_knn>( 575 | cloud: &[P2], 576 | query: &P2, 577 | k: u32, 578 | ) -> Vec> { 579 | let mut h = H::new_with_k(k); 580 | for (index, point) in cloud.iter().enumerate() { 581 | let dist2 = point_dist2(point, query); 582 | h.add(dist2, index as u32); 583 | } 584 | h.into_sorted_vec() 585 | .into_iter() 586 | .map(|n| { 587 | let index = n.index as usize; 588 | Neighbour { 589 | point: cloud[index], 590 | dist2: n.dist2, 591 | index: n.index, 592 | } 593 | }) 594 | .collect() 595 | } 596 | 597 | // tests themselves 598 | 599 | #[test] 600 | fn get_build_points_bounds() { 601 | let cloud = cloud3(); 602 | let indices = vec![0, 1, 2]; 603 | let bounds = KDTree::get_build_points_bounds(&cloud, &indices); 604 | assert_eq!(bounds.0, vec![-1., -4.]); 605 | assert_eq!(bounds.1, vec![2., 3.]); 606 | } 607 | 608 | #[test] 609 | fn max_delta_index() { 610 | let b = |x: f32, y: f32| { 611 | [ 612 | NotNan::::new(x).unwrap(), 613 | NotNan::::new(y).unwrap(), 614 | ] 615 | }; 616 | assert_eq!( 617 | KDTree::::max_delta_index(&b(0., 0.), &b(0., 1.)), 618 | 1 619 | ); 620 | assert_eq!( 621 | KDTree::::max_delta_index(&b(0., 0.), &b(-1., 1.)), 622 | 1 623 | ); 624 | assert_eq!( 625 | KDTree::::max_delta_index(&b(0., 0.), &b(-1., -2.)), 626 | 0 627 | ); 628 | } 629 | 630 | #[test] 631 | fn new_tree() { 632 | let cloud = cloud3(); 633 | let tree = KDTree::new_with_bucket_size(&cloud, 2); 634 | dbg!(tree); 635 | } 636 | 637 | #[test] 638 | fn query_1nn_allow_self() { 639 | let mut touch_sum = 0; 640 | const PASS_COUNT: u32 = 20; 641 | const QUERY_COUNT: u32 = 100; 642 | const CLOUD_SIZE: u32 = 1000; 643 | const PARAMETERS: Parameters = Parameters { 644 | epsilon: 0.0, 645 | max_radius: f32::INFINITY, 646 | allow_self_match: true, 647 | sort_results: true, 648 | }; 649 | for _ in 0..PASS_COUNT { 650 | let cloud = random_point_cloud(CLOUD_SIZE); 651 | let tree = KDTree::new(&cloud); 652 | for _ in 0..QUERY_COUNT { 653 | let query = random_point(); 654 | let mut touch_statistics = 0; 655 | 656 | // linear search 657 | let nns_lin = tree.knn_advanced( 658 | 1, 659 | &query, 660 | CandidateContainer::Linear, 661 | &PARAMETERS, 662 | Some(&mut touch_statistics), 663 | ); 664 | assert_eq!(nns_lin.len(), 1); 665 | let nn_lin = &nns_lin[0]; 666 | assert_eq!(nn_lin.point, cloud[nn_lin.index as usize]); 667 | touch_sum += touch_statistics; 668 | // binary 669 | let nns_bin = 670 | tree.knn_advanced(1, &query, CandidateContainer::BinaryHeap, &PARAMETERS, None); 671 | assert_eq!(nns_bin.len(), 1); 672 | let nn_bin = &nns_bin[0]; 673 | assert_eq!(nn_bin.point, cloud[nn_bin.index as usize]); 674 | // brute force 675 | let nn_bf = brute_force_1nn(&cloud, &query); 676 | assert_eq!(nn_bf.point, cloud[nn_bf.index as usize]); 677 | // assertion 678 | assert_eq!( 679 | nn_bin.index, nn_bf.index, 680 | "KDTree binary heap: mismatch indexes\nquery: {}\npoint {}, {}\nvs bf {}, {}", 681 | query, nn_bin.dist2, nn_bin.point, nn_bf.dist2, nn_bf.point 682 | ); 683 | assert_eq!(nn_lin.index, nn_bf.index, "\nKDTree linear heap: mismatch indexes\nquery: {}\npoint {}, {}\nvs bf {}, {}\n", query, nn_lin.dist2, nn_lin.point, nn_bf.dist2, nn_bf.point); 684 | assert!(approx_eq!(f32, *nn_lin.dist2, *nn_bf.dist2, ulps = 2)); 685 | assert!(approx_eq!(f32, *nn_bin.dist2, *nn_bf.dist2, ulps = 2)); 686 | } 687 | } 688 | let touch_pct = (touch_sum * 100) as f32 / (PASS_COUNT * QUERY_COUNT * CLOUD_SIZE) as f32; 689 | println!("Average tree point touched: {} %", touch_pct); 690 | } 691 | 692 | #[test] 693 | fn query_knn_allow_self() { 694 | const QUERY_COUNT: u32 = 100; 695 | const CLOUD_SIZE: u32 = 1000; 696 | const PARAMETERS: Parameters = Parameters { 697 | epsilon: 0.0, 698 | max_radius: f32::INFINITY, 699 | allow_self_match: true, 700 | sort_results: true, 701 | }; 702 | let cloud = random_point_cloud(CLOUD_SIZE); 703 | let tree = KDTree::new(&cloud); 704 | for k in [1, 2, 3, 5, 7, 13] { 705 | for _ in 0..QUERY_COUNT { 706 | let query = random_point(); 707 | // brute force 708 | let nns_bf_lin = brute_force_knn::>>(&cloud, &query, k); 709 | assert_eq!(nns_bf_lin.len(), k as usize); 710 | let nns_bf_bin = 711 | brute_force_knn::>>(&cloud, &query, k); 712 | assert_eq!(nns_bf_bin.len(), k as usize); 713 | // kd-tree 714 | #[cfg_attr(rustfmt, rustfmt_skip)] 715 | let nns_bin = tree.knn_advanced( 716 | k, &query, 717 | CandidateContainer::BinaryHeap, 718 | &PARAMETERS, 719 | None, 720 | ); 721 | assert_eq!(nns_bin.len(), k as usize); 722 | #[cfg_attr(rustfmt, rustfmt_skip)] 723 | let nns_lin = tree.knn_advanced( 724 | k, &query, 725 | CandidateContainer::Linear, 726 | &PARAMETERS, 727 | None, 728 | ); 729 | assert_eq!(nns_lin.len(), k as usize); 730 | // assertion 731 | for i in 0..k as usize { 732 | // get neighbour 733 | let nn_bf_lin = &nns_bf_lin[i]; 734 | let nn_bf_bin = &nns_bf_bin[i]; 735 | let nn_lin = &nns_lin[i]; 736 | let nn_bin = &nns_bin[i]; 737 | // ensure their point data are consistent with the cloud 738 | assert_eq!(nn_bf_lin.point, cloud[nn_bf_lin.index as usize]); 739 | assert_eq!(nn_bf_bin.point, cloud[nn_bf_bin.index as usize]); 740 | assert_eq!(nn_lin.point, cloud[nn_lin.index as usize]); 741 | assert_eq!(nn_bin.point, cloud[nn_bin.index as usize]); 742 | // ensure their indices are consistent 743 | assert_eq!(nn_bf_bin.index, nn_bf_lin.index, "BF binary heap: mismatch indexes at {} on {}\nquery: {}\n bf bin {}, {}\nvs bf lin {}, {}\n", i, k, query, nn_bf_bin.dist2, nn_bf_bin.point, nn_bf_lin.dist2, nn_bf_lin.point); 744 | assert_eq!(nn_lin.index, nn_bf_lin.index, "\nKDTree linear heap: mismatch indexes at {} on {}\nquery: {}\npoint {}, {}\nvs bf {}, {}\n", i, k, query, nn_lin.dist2, nn_lin.point, nn_bf_lin.dist2, nn_bf_lin.point); 745 | assert_eq!(nn_bin.index, nn_bf_lin.index, "\nKDTree binary heap: mismatch indexes {} on {}\nquery: {}\npoint {}, {}\nvs bf {}, {}\n", i, k, query, nn_bin.dist2, nn_bin.point, nn_bf_lin.dist2, nn_bf_lin.point); 746 | // ensure their dist2 are consistent 747 | assert!(approx_eq!( 748 | f32, 749 | *nn_bf_bin.dist2, 750 | *nn_bf_lin.dist2, 751 | ulps = 2 752 | )); 753 | assert!(approx_eq!(f32, *nn_lin.dist2, *nn_bf_lin.dist2, ulps = 2)); 754 | assert!(approx_eq!(f32, *nn_bin.dist2, *nn_bf_lin.dist2, ulps = 2)); 755 | } 756 | } 757 | } 758 | } 759 | 760 | #[test] 761 | fn small_clouds_can_lead_to_neighbours() { 762 | let cloud = vec![P2::new2d(0.0, 0.0), P2::new2d(1.0, 0.0)]; 763 | let tree = KDTree::new(&cloud); 764 | let query = P2::new2d(0.5, 0.0); 765 | for _ in [CandidateContainer::Linear, CandidateContainer::BinaryHeap] { 766 | let nns = tree.knn(3, &query); 767 | assert_eq!(nns.len(), 2); 768 | } 769 | } 770 | 771 | #[test] 772 | fn max_radius_can_lead_to_neighbours() { 773 | let cloud = vec![P2::new2d(0.0, 0.0), P2::new2d(1.0, 0.0)]; 774 | let tree = KDTree::new(&cloud); 775 | let query = P2::new2d(0.1, 0.0); 776 | let parameters = Parameters { 777 | epsilon: 0.0, 778 | max_radius: 0.5, 779 | allow_self_match: false, 780 | sort_results: false, 781 | }; 782 | for container in [CandidateContainer::Linear, CandidateContainer::BinaryHeap] { 783 | let nns = tree.knn_advanced(2, &query, container, ¶meters, None); 784 | assert_eq!(nns.len(), 1); 785 | } 786 | } 787 | } 788 | -------------------------------------------------------------------------------- /src/node.rs: -------------------------------------------------------------------------------- 1 | use core::marker::PhantomData; 2 | 3 | use ordered_float::NotNan; 4 | 5 | use crate::{Point, Scalar}; 6 | 7 | /// Either a split value or the index of the first point in the bucket 8 | #[derive(Clone, Copy)] 9 | union SplitValOrBucketIndex { 10 | /// for split node, split value 11 | split_val: NotNan, 12 | /// for leaf node, start index of bucket 13 | bucket_start_index: u32, 14 | } 15 | 16 | /// Creates the compound index containing the dimension and the index of the child or the bucket size 17 | fn create_dim_child_bucket_size>( 18 | dim: u32, 19 | child_index_or_bucket_size: u32, 20 | ) -> u32 { 21 | dim | (child_index_or_bucket_size << P::DIM_BIT_COUNT) 22 | } 23 | 24 | /// A node of the KD-Tree: either split or leaf 25 | /// 26 | /// If split node, it holds a split dimension and the split value along this dimension, 27 | /// and the index of its right child (its left child is its own index + 1). 28 | /// If leaf node, it holds the start index of the bucket and the number of elements in the bucket. 29 | #[derive(Clone)] 30 | pub(crate) struct Node> { 31 | /// cut dimension for split nodes (dim_bit_count lsb), index of right node or number of points in bucket (rest). 32 | /// Note that left index is current + 1. 33 | dim_child_bucket_size: u32, 34 | /// Either a split value or the index of the first point in the bucket 35 | split_val_or_bucket_start_index: SplitValOrBucketIndex, 36 | phantom: PhantomData

, 37 | } 38 | impl> Node { 39 | pub(crate) fn set_child_index(&mut self, child_index: u32) { 40 | self.dim_child_bucket_size |= child_index << P::DIM_BIT_COUNT; 41 | } 42 | pub(crate) fn new_split_node(split_dim: u32, split_val: NotNan) -> Self { 43 | Node { 44 | dim_child_bucket_size: split_dim, 45 | split_val_or_bucket_start_index: SplitValOrBucketIndex { split_val }, 46 | phantom: PhantomData, 47 | } 48 | } 49 | pub(crate) fn new_leaf_node(bucket_start_index: u32, bucket_size: u32) -> Self { 50 | Node { 51 | dim_child_bucket_size: create_dim_child_bucket_size::(P::DIM, bucket_size), 52 | split_val_or_bucket_start_index: SplitValOrBucketIndex { bucket_start_index }, 53 | phantom: PhantomData, 54 | } 55 | } 56 | /// Depending on the type of node (split or leaf), calls split_cb or leaf_cb with ctx as first argument 57 | #[inline] 58 | pub(crate) fn dispatch_on_type(&self, ctx: Ctx, split_cb: Fs, leaf_cb: Fl) -> R 59 | where 60 | Fl: FnOnce(Ctx, u32, u32) -> R, // ctx, bucket_start_index, bucket_size 61 | Fs: FnOnce(Ctx, u32, NotNan, u32) -> R, // ctx, split_dim, split_val, right_child 62 | { 63 | // SAFETY: interpretation of cut_val_or_bucket_start_index is defined by the 64 | // P::DIM_MASK bits of self.dim_child_bucket_size 65 | // If they have value P::DIM this is a leaf node, otherwise it is a split 66 | // node and they specify the split axis. 67 | if self.dim_child_bucket_size & P::DIM_MASK == P::DIM { 68 | // leaf node 69 | let bucket_start_index = 70 | unsafe { self.split_val_or_bucket_start_index.bucket_start_index }; 71 | let bucket_size = self.dim_child_bucket_size >> P::DIM_BIT_COUNT; 72 | leaf_cb(ctx, bucket_start_index, bucket_size) 73 | } else { 74 | // split node 75 | let split_val = unsafe { self.split_val_or_bucket_start_index.split_val }; 76 | let split_dim = self.dim_child_bucket_size & P::DIM_MASK; 77 | let right_child = self.dim_child_bucket_size >> P::DIM_BIT_COUNT; 78 | split_cb(ctx, split_dim, split_val, right_child) 79 | } 80 | } 81 | } 82 | impl> core::fmt::Debug for Node { 83 | fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { 84 | self.dispatch_on_type( 85 | f, 86 | |f, split_dim, split_val, right_child| { 87 | f.debug_struct("Node(split)") 88 | .field("split_dim", &split_dim) 89 | .field("split_val", &split_val) 90 | .field("right_child", &right_child) 91 | .finish() 92 | }, 93 | |f, bucket_start_index, bucket_size| { 94 | f.debug_struct("Node(leaf)") 95 | .field("bucket_size", &bucket_size) 96 | .field("bucket_start_index", &bucket_start_index) 97 | .finish() 98 | }, 99 | ) 100 | } 101 | } 102 | 103 | #[cfg(test)] 104 | mod tests { 105 | extern crate std; 106 | use crate::simple_point::P2; 107 | use crate::*; 108 | use std::dbg; 109 | 110 | #[test] 111 | fn sizes() { 112 | dbg!(std::mem::size_of::>()); 113 | } 114 | 115 | #[test] 116 | fn dim_bit_count() { 117 | let d: u32 = 4; 118 | let dim_bit_count = 32 - d.leading_zeros(); 119 | assert_eq!(dim_bit_count, 3); 120 | } 121 | } 122 | -------------------------------------------------------------------------------- /src/simple_point.rs: -------------------------------------------------------------------------------- 1 | //! A simple D-dimensional point type 2 | 3 | use core::{ 4 | fmt::Display, 5 | ops::{Add, Sub}, 6 | }; 7 | use ordered_float::NotNan; 8 | #[cfg(any(feature = "rand", test))] 9 | use rand::Rng; 10 | 11 | use num_traits::Bounded; 12 | 13 | use crate::Point; 14 | 15 | /// A simple `f32` `D`-dimensional point type 16 | #[derive(Clone, Copy, PartialEq, Debug)] 17 | pub struct SimplePoint(pub [NotNan; D]); 18 | impl SimplePoint<2> { 19 | /// Creates a new point from (x,y). 20 | pub fn new2d(x: f32, y: f32) -> SimplePoint<2> { 21 | SimplePoint([NotNan::new(x).unwrap(), NotNan::new(y).unwrap()]) 22 | } 23 | /// Creates a new point from (x,y,z). 24 | pub fn new3d(x: f32, y: f32, z: f32) -> SimplePoint<3> { 25 | SimplePoint([ 26 | NotNan::new(x).unwrap(), 27 | NotNan::new(y).unwrap(), 28 | NotNan::new(z).unwrap(), 29 | ]) 30 | } 31 | } 32 | impl Default for SimplePoint { 33 | fn default() -> SimplePoint { 34 | SimplePoint([NotNan::new(0.0).unwrap(); D]) 35 | } 36 | } 37 | impl Bounded for SimplePoint { 38 | fn min_value() -> SimplePoint { 39 | SimplePoint([NotNan::::min_value(); D]) 40 | } 41 | fn max_value() -> SimplePoint { 42 | SimplePoint([NotNan::::max_value(); D]) 43 | } 44 | } 45 | impl Point for SimplePoint { 46 | const DIM: u32 = D as u32; 47 | fn set(&mut self, index: u32, value: NotNan) { 48 | self.0[index as usize] = value; 49 | } 50 | fn get(&self, index: u32) -> NotNan { 51 | self.0[index as usize] 52 | } 53 | } 54 | impl Add for SimplePoint { 55 | type Output = SimplePoint; 56 | 57 | fn add(self, rhs: SimplePoint) -> Self::Output { 58 | SimplePoint(core::array::from_fn(|i| self.0[i] + rhs.0[i])) 59 | } 60 | } 61 | impl Sub for SimplePoint { 62 | type Output = SimplePoint; 63 | 64 | fn sub(self, rhs: SimplePoint) -> Self::Output { 65 | SimplePoint(core::array::from_fn(|i| self.0[i] - rhs.0[i])) 66 | } 67 | } 68 | impl Display for SimplePoint { 69 | fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { 70 | write!(f, "[{}, {}]", self.0[0], self.0[1]) 71 | } 72 | } 73 | 74 | /// A simple 2-D point type 75 | pub type P2 = SimplePoint<2>; 76 | 77 | /// A simple 3-D point type 78 | pub type P3 = SimplePoint<3>; 79 | 80 | /// Creates a random point whose coordinate are in the interval [-100:100]. 81 | #[cfg(any(feature = "rand", test))] 82 | pub fn random_point() -> SimplePoint { 83 | let mut rng = rand::thread_rng(); 84 | SimplePoint(core::array::from_fn(|_| { 85 | NotNan::new(rng.gen_range(-100.0..100.0)).unwrap() 86 | })) 87 | } 88 | 89 | /// Creates a random cloud of count points using [random_point()] for each. 90 | #[cfg(any(feature = "rand", test))] 91 | pub fn random_point_cloud(count: u32) -> alloc::vec::Vec> { 92 | (0..count).map(|_| random_point()).collect() 93 | } 94 | --------------------------------------------------------------------------------