├── .github └── workflows │ └── ci.yml ├── .gitignore ├── Cargo.toml ├── LICENSE ├── README.md ├── ensure_no_std ├── .gitignore ├── Cargo.toml └── src │ └── main.rs ├── src └── lib.rs └── tests ├── lines.rs └── no_solutions.rs /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: ci 2 | on: 3 | push: 4 | branches: 5 | - main 6 | pull_request: 7 | 8 | jobs: 9 | lints: 10 | name: lints 11 | runs-on: ubuntu-latest 12 | steps: 13 | - name: Checkout sources 14 | uses: actions/checkout@v2 15 | 16 | - name: Install beta toolchain 17 | uses: actions-rs/toolchain@v1 18 | with: 19 | profile: minimal 20 | toolchain: beta 21 | override: true 22 | components: rustfmt, clippy 23 | 24 | - name: Set up cache 25 | uses: Swatinem/rust-cache@v1 26 | with: 27 | cache-on-failure: true 28 | 29 | - name: Run cargo fmt 30 | uses: actions-rs/cargo@v1 31 | with: 32 | command: fmt 33 | args: --all -- --check 34 | 35 | - name: Run cargo clippy 36 | uses: actions-rs/cargo@v1 37 | with: 38 | command: clippy 39 | args: --tests -- -D warnings 40 | 41 | no_std: 42 | name: no_std 43 | runs-on: ubuntu-latest 44 | steps: 45 | - name: Checkout sources 46 | uses: actions/checkout@v2 47 | 48 | - name: Install beta toolchain 49 | uses: actions-rs/toolchain@v1 50 | with: 51 | profile: minimal 52 | toolchain: beta 53 | target: armv7a-none-eabi 54 | override: true 55 | 56 | - name: Set up cache 57 | uses: Swatinem/rust-cache@v1 58 | with: 59 | cache-on-failure: true 60 | 61 | - name: Build binary for armv7a-none-eabi 62 | uses: actions-rs/cargo@v1 63 | with: 64 | command: rustc 65 | args: --target=armv7a-none-eabi --manifest-path=ensure_no_std/Cargo.toml 66 | 67 | tests: 68 | name: tests 69 | runs-on: ubuntu-latest 70 | steps: 71 | - name: Checkout sources 72 | uses: actions/checkout@v2 73 | 74 | - name: Install beta toolchain 75 | uses: actions-rs/toolchain@v1 76 | with: 77 | profile: minimal 78 | toolchain: beta 79 | override: true 80 | 81 | - name: Set up cache 82 | uses: Swatinem/rust-cache@v1 83 | with: 84 | cache-on-failure: true 85 | 86 | - name: Run cargo test 87 | uses: actions-rs/cargo@v1 88 | with: 89 | command: test 90 | args: --all-features -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | **/*.rs.bk 3 | Cargo.lock 4 | 5 | # Flamegraph 6 | *.svg 7 | perf.* 8 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "arrsac" 3 | version = "0.10.0" 4 | authors = ["Geordon Worley "] 5 | edition = "2021" 6 | description = "From the paper \"A Comparative Analysis of RANSAC Techniques Leading to Adaptive Real-Time Random Sample Consensus\"" 7 | documentation = "https://docs.rs/arrsac/" 8 | repository = "https://github.com/rust-cv/arrsac" 9 | keywords = ["ransac", "sample", "consensus"] 10 | categories = ["algorithms", "science", "science::robotics", "no-std"] 11 | license = "MIT" 12 | readme = "README.md" 13 | 14 | [dependencies] 15 | sample-consensus = "1.0.1" 16 | rand_core = "0.6.3" 17 | 18 | [dev-dependencies] 19 | rand = "0.8.4" 20 | rand_xoshiro = "0.6.0" 21 | 22 | [profile.dev] 23 | opt-level = 3 24 | 25 | [profile.release] 26 | debug = true 27 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 rust-cv 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, 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, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # arrsac 2 | 3 | [![Discord][dci]][dcl] [![Crates.io][ci]][cl] [![docs.rs][di]][dl] ![LoC][lo] ![ci][bci] 4 | 5 | [ci]: https://img.shields.io/crates/v/arrsac.svg 6 | [cl]: https://crates.io/crates/arrsac/ 7 | 8 | [di]: https://docs.rs/arrsac/badge.svg 9 | [dl]: https://docs.rs/arrsac/ 10 | 11 | [lo]: https://tokei.rs/b1/github/rust-cv/arrsac?category=code 12 | 13 | [dci]: https://img.shields.io/discord/550706294311485440.svg?logo=discord&colorB=7289DA 14 | [dcl]: https://discord.gg/d32jaam 15 | 16 | [bci]: https://github.com/rust-cv/arrsac/workflows/ci/badge.svg 17 | 18 | Implements the ARRSAC algorithm from the paper "A Comparative Analysis of RANSAC Techniques Leading to Adaptive Real-Time Random Sample Consensus"; 19 | the paper "Randomized RANSAC with Sequential Probability Ratio Test" is also used to implement the SPRT for RANSAC. 20 | 21 | Some things were modified from the original papers which improve corner cases or convenience in regular usage. 22 | 23 | This can be used as a `Consensus` algorithm with the [`sample-consensus`](https://crates.io/crates/sample-consensus) crate. 24 | ARRSAC can replace RANSAC and is almost always a faster solution, given that you are willing to tune the parameters. 25 | -------------------------------------------------------------------------------- /ensure_no_std/.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | Cargo.lock 3 | -------------------------------------------------------------------------------- /ensure_no_std/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "ensure_no_std" 3 | version = "0.1.0" 4 | authors = ["Geordon Worley "] 5 | edition = "2018" 6 | 7 | [dependencies] 8 | arrsac = { path = ".." } 9 | 10 | [profile.dev] 11 | panic = "abort" 12 | 13 | [profile.release] 14 | panic = "abort" 15 | -------------------------------------------------------------------------------- /ensure_no_std/src/main.rs: -------------------------------------------------------------------------------- 1 | // ensure_no_std/src/main.rs 2 | #![no_std] 3 | #![no_main] 4 | 5 | use core::panic::PanicInfo; 6 | 7 | /// This function is called on panic. 8 | #[panic_handler] 9 | fn panic(_info: &PanicInfo) -> ! { 10 | loop {} 11 | } 12 | 13 | #[no_mangle] 14 | pub extern "C" fn _start() -> ! { 15 | loop {} 16 | } 17 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | #![no_std] 2 | 3 | extern crate alloc; 4 | use core::cmp::Reverse; 5 | 6 | use alloc::{vec, vec::Vec}; 7 | use rand_core::RngCore; 8 | use sample_consensus::{Consensus, Estimator, Model}; 9 | 10 | /// The ARRSAC algorithm for sample consensus. 11 | /// 12 | /// Don't forget to shuffle your input data points to avoid bias before 13 | /// using this consensus process. It will not shuffle your data for you. 14 | /// If you do not shuffle, the output will be biased towards data at the beginning 15 | /// of the inputs. 16 | pub struct Arrsac { 17 | initialization_hypotheses: usize, 18 | initialization_blocks: usize, 19 | max_candidate_hypotheses: usize, 20 | estimations_per_block: usize, 21 | block_size: usize, 22 | likelihood_ratio_threshold: f32, 23 | inlier_threshold: f64, 24 | rng: R, 25 | random_samples: Vec, 26 | } 27 | 28 | impl Arrsac 29 | where 30 | R: RngCore, 31 | { 32 | /// `rng` should have the same properties you would want for a Monte Carlo simulation. 33 | /// It should generate random numbers quickly without having any discernable patterns. 34 | /// 35 | /// The `inlier_threshold` is the one parameter that is always specific to your dataset. 36 | /// This must be set to the threshold in which a data point's residual is considered an inlier. 37 | /// Some of the other parameters may need to be configured based on the amount of data, 38 | /// such as `block_size`, `likelihood_ratio_threshold`, and `block_size`. However, 39 | /// `inlier_threshold` has to be set based on the residual function used with the model. 40 | /// 41 | /// `initial_epsilon` must be higher than `initial_delta`. If you modify these values, 42 | /// you need to make sure that within one `block_size` the `likelihood_ratio_threshold` 43 | /// can be reached and a model can be rejected. Basically, make sure that 44 | /// `((1.0 - delta) / (1.0 - epsilon))^block_size >>> likelihood_ratio_threshold`. 45 | /// This must be done to ensure outlier models are rejected during the initial generation 46 | /// phase, which only processes `block_size` datapoints. 47 | /// 48 | /// `initial_epsilon` should also be as large as you can set it where it is still relatively 49 | /// pessimistic. This is so that we can more easily reject a model early in the process 50 | /// to compute an updated value for delta during the adaptive process. This may not be possible 51 | /// and will depend on your data. 52 | pub fn new(inlier_threshold: f64, rng: R) -> Self { 53 | Self { 54 | initialization_hypotheses: 256, 55 | initialization_blocks: 4, 56 | max_candidate_hypotheses: 64, 57 | estimations_per_block: 64, 58 | block_size: 64, 59 | likelihood_ratio_threshold: 1e3, 60 | inlier_threshold, 61 | rng, 62 | random_samples: vec![], 63 | } 64 | } 65 | 66 | /// Number of models generated in the initial step when epsilon and delta are being estimated. 67 | /// 68 | /// Default: `256` 69 | #[must_use] 70 | pub fn initialization_hypotheses(self, initialization_hypotheses: usize) -> Self { 71 | Self { 72 | initialization_hypotheses, 73 | ..self 74 | } 75 | } 76 | 77 | /// Number of data blocks used to compute the initial estimate of delta and epsilon 78 | /// before proceeding with regular block processing. This is used instead of 79 | /// an initial epsilon and delta, which were suggested by the paper. 80 | /// 81 | /// Default: `4` 82 | #[must_use] 83 | pub fn initialization_blocks(self, initialization_blocks: usize) -> Self { 84 | Self { 85 | initialization_blocks, 86 | ..self 87 | } 88 | } 89 | 90 | /// Maximum number of best hypotheses to retain during block processing 91 | /// 92 | /// This number is halved on each block such that on block `n` the number of 93 | /// hypotheses retained is `max_candidate_hypotheses >> n`. 94 | /// 95 | /// Default: `64` 96 | #[must_use] 97 | pub fn max_candidate_hypotheses(self, max_candidate_hypotheses: usize) -> Self { 98 | Self { 99 | max_candidate_hypotheses, 100 | ..self 101 | } 102 | } 103 | 104 | /// Number of estmations (may generate multiple hypotheses) that will be ran 105 | /// for each block of data evaluated 106 | /// 107 | /// Default: `64` 108 | #[must_use] 109 | pub fn estimations_per_block(self, estimations_per_block: usize) -> Self { 110 | Self { 111 | estimations_per_block, 112 | ..self 113 | } 114 | } 115 | 116 | /// Number of data points evaluated before more hypotheses are generated 117 | /// 118 | /// Default: `64` 119 | #[must_use] 120 | pub fn block_size(self, block_size: usize) -> Self { 121 | Self { block_size, ..self } 122 | } 123 | 124 | /// Once a model reaches this level of unlikelihood, it is rejected. Set this 125 | /// higher to make it less restrictive, usually at the cost of more execution time. 126 | /// 127 | /// Increasing this will make it more likely to find a good result. 128 | /// 129 | /// Decreasing this will speed up execution. 130 | /// 131 | /// This ratio is not exposed as a parameter in the original paper, but is instead computed 132 | /// recursively for a few iterations. It is roughly equivalent to the **reciprocal** of the 133 | /// **probability of rejecting a good model**. You can use that to control the probability 134 | /// that a good model is rejected. 135 | /// 136 | /// Default: `1e3` 137 | #[must_use] 138 | pub fn likelihood_ratio_threshold(self, likelihood_ratio_threshold: f32) -> Self { 139 | Self { 140 | likelihood_ratio_threshold, 141 | ..self 142 | } 143 | } 144 | 145 | /// Residual threshold for determining if a data point is an inlier or an outlier of a model 146 | #[must_use] 147 | pub fn inlier_threshold(self, inlier_threshold: f64) -> Self { 148 | Self { 149 | inlier_threshold, 150 | ..self 151 | } 152 | } 153 | 154 | /// Adapted from algorithm 3 from "A Comparative Analysis of RANSAC Techniques Leading to Adaptive 155 | /// Real-Time Random Sample Consensus", but it was effectively rewritten to avoid the need for 156 | /// initial epsilon and delta. 157 | /// 158 | /// Returns the initial models (and their num inliers) sorted by decreasing inliers 159 | /// and `delta` in that order. 160 | fn initial_hypotheses( 161 | &mut self, 162 | estimator: &E, 163 | data: impl Iterator + Clone, 164 | ) -> (Vec<(E::Model, usize)>, f32) 165 | where 166 | E: Estimator, 167 | { 168 | assert!( 169 | self.initialization_blocks > 0, 170 | "ARRSAC must have at least 1 initialization block" 171 | ); 172 | // NOTE: This whole function is different than that specified in the ARRSAC paper. 173 | // The reason is that you needed to provide a good initial guess for epsilon and delta 174 | // otherwise it could lead to delta exceeding epsilon or situations where models could no 175 | // longer be rejected or were almost always rejected. This solution is an imperfect compomise 176 | // that assumes that delta will be roughly equal to the inlier ratio of the worst model generated, 177 | // which both assumes that the worst model is an outlier and that it is actually representative of the 178 | // population. The assumption of epsilon also assumes that the best model is an inlier, but is an 179 | // otherwise good initial guess. The other caveat with this approach is that a sufficiently large 180 | // set of initial datapoints is required to be able to accurately determine epsilon and delta. 181 | // Therefore a new paremeter is added to separate the normal blocks from the initial generation set. 182 | let mut hypotheses = vec![]; 183 | // We don't want more than `block_size` data points to be used to evaluate models initially. 184 | let initial_datapoints = core::cmp::min( 185 | self.initialization_blocks * self.block_size, 186 | data.clone().count(), 187 | ); 188 | // Generate the initial batch of random hypotheses and count their inliers and outliers. 189 | for _ in 0..self.initialization_hypotheses { 190 | for model in self.generate_random_hypotheses(estimator, data.clone()) { 191 | let inliers = self.count_inliers(data.clone().take(initial_datapoints), &model); 192 | hypotheses.push((model, inliers)); 193 | } 194 | } 195 | 196 | // Bail early when no hypothesis was found. 197 | // This will cause execution to terminate. 198 | if hypotheses.is_empty() { 199 | return (hypotheses, 0.0); 200 | } 201 | 202 | // Sort the hypotheses by their inliers. 203 | hypotheses.sort_unstable_by_key(|&(_, inliers)| Reverse(inliers)); 204 | 205 | // Compute epsilon and delta using the best and worst model generated. 206 | let epsilon = hypotheses 207 | .first() 208 | .map(|&(_, inliers)| inliers as f32 / initial_datapoints as f32) 209 | .unwrap_or_default(); 210 | let delta = hypotheses 211 | .last() 212 | .map(|&(_, inliers)| if inliers < E::MIN_SAMPLES {E::MIN_SAMPLES} else {inliers} as f32 / initial_datapoints as f32) 213 | .unwrap_or_default(); 214 | 215 | if epsilon < delta { 216 | // If epsilon is less than delta, then better hypotheses will get rejected and worse accepted, 217 | // which is counter to what we want. In this case, we had a bad initialization, so clear the hypotheses. 218 | // This will cause execution to terminate. 219 | hypotheses.clear(); 220 | return (hypotheses, delta); 221 | } 222 | 223 | // Populate hypotheses with hypotheses generated from the inliers of the best hypothesis. 224 | // This will use the initialization datapoints and filter with SPRT. 225 | self.populate_hypotheses_sprt( 226 | estimator, 227 | &mut hypotheses, 228 | delta, 229 | data, 230 | initial_datapoints, 231 | self.initialization_hypotheses, 232 | ); 233 | 234 | // Sort the hypotheses by their inliers. 235 | hypotheses.sort_unstable_by_key(|&(_, inliers)| Reverse(inliers)); 236 | 237 | // Filter down the hypotheses to just the best ones. 238 | hypotheses.truncate(self.max_candidate_hypotheses >> (self.initialization_blocks - 1)); 239 | 240 | (hypotheses, delta) 241 | } 242 | 243 | /// Populates `self.random_samples` using a len. 244 | fn populate_samples(&mut self, num: usize, len: usize) { 245 | // We can generate no hypotheses if the amout of data is too low. 246 | if len < num { 247 | panic!("cannot use arrsac without having enough samples"); 248 | } 249 | let len = len as u32; 250 | // Threshold generation below adapted from randomize::RandRangeU32. 251 | let threshold = len.wrapping_neg() % len; 252 | self.random_samples.clear(); 253 | for _ in 0..num { 254 | loop { 255 | let mul = u64::from(self.rng.next_u32()).wrapping_mul(u64::from(len)); 256 | if mul as u32 >= threshold { 257 | let s = (mul >> 32) as u32; 258 | if !self.random_samples.contains(&s) { 259 | self.random_samples.push(s); 260 | break; 261 | } 262 | } 263 | } 264 | } 265 | } 266 | 267 | fn populate_hypotheses_sprt( 268 | &mut self, 269 | estimator: &E, 270 | hypotheses: &mut Vec<(E::Model, usize)>, 271 | delta: f32, 272 | data: impl Iterator + Clone, 273 | num_checked: usize, 274 | num_hypotheses: usize, 275 | ) where 276 | E: Estimator, 277 | { 278 | // Update epsilon using the best model. 279 | // Since epsilon can only increase and delta is fixed, we can be sure that these ratios 280 | // will still be valid (epsilon > delta). 281 | let epsilon = hypotheses[0].1 as f32 / num_checked as f32; 282 | // Create the likelihood ratios for inliers and outliers. 283 | let positive_likelihood_ratio = delta / epsilon; 284 | let negative_likelihood_ratio = (1.0 - delta) / (1.0 - epsilon); 285 | // Generate the list of inliers for the best model. 286 | let mut inliers = self.inliers(data.clone().take(num_checked), &hypotheses[0].0); 287 | if inliers.len() <= E::MIN_SAMPLES { 288 | // If we don't have enough samples to generate more models, then we should expand the inliers to 289 | // the entire dataset. 290 | inliers = self.inliers(data.clone().take(num_checked), &hypotheses[0].0); 291 | } 292 | // We generate hypotheses until we reach the initial num hypotheses. 293 | // We can't count the number generated because it could generate 0 hypotheses 294 | // and then the loop would continue indefinitely. 295 | let mut random_hypotheses = Vec::new(); 296 | for _ in 0..num_hypotheses { 297 | random_hypotheses.extend(self.generate_random_hypotheses_subset( 298 | estimator, 299 | data.clone(), 300 | &inliers, 301 | )); 302 | for model in random_hypotheses.drain(..) { 303 | if let Some(inliers) = self.asprt( 304 | data.clone().take(num_checked), 305 | &model, 306 | positive_likelihood_ratio, 307 | negative_likelihood_ratio, 308 | E::MIN_SAMPLES, 309 | ) { 310 | hypotheses.push((model, inliers)); 311 | } 312 | } 313 | } 314 | } 315 | 316 | /// Generates as many hypotheses as one call to `Estimator::estimate()` returns from all data. 317 | fn generate_random_hypotheses( 318 | &mut self, 319 | estimator: &E, 320 | data: impl Iterator + Clone, 321 | ) -> E::ModelIter 322 | where 323 | E: Estimator, 324 | { 325 | self.populate_samples(E::MIN_SAMPLES, data.clone().count()); 326 | estimator.estimate( 327 | self.random_samples 328 | .iter() 329 | .map(|&ix| data.clone().nth(ix as usize).unwrap()), 330 | ) 331 | } 332 | 333 | /// Generates as many hypotheses as one call to `Estimator::estimate()` returns from a subset of the data. 334 | fn generate_random_hypotheses_subset( 335 | &mut self, 336 | estimator: &E, 337 | data: impl Iterator + Clone, 338 | subset: &[usize], 339 | ) -> E::ModelIter 340 | where 341 | E: Estimator, 342 | { 343 | self.populate_samples(E::MIN_SAMPLES, subset.len()); 344 | estimator.estimate( 345 | core::mem::take(&mut self.random_samples) 346 | .iter() 347 | .map(|&ix| data.clone().nth(subset[ix as usize]).unwrap()), 348 | ) 349 | } 350 | 351 | /// Algorithm 1 in "Randomized RANSAC with Sequential Probability Ratio Test". 352 | /// 353 | /// This tests if a model is accepted. Returns `Some(inliers)` if accepted or `None` if rejected. 354 | /// 355 | /// `inlier_threshold` - The model residual error threshold between inliers and outliers 356 | /// `positive_likelihood_ratio` - `δ / ε` 357 | /// `negative_likelihood_ratio` - `(1 - δ) / (1 - ε)` 358 | fn asprt>( 359 | &self, 360 | data: impl Iterator, 361 | model: &M, 362 | positive_likelihood_ratio: f32, 363 | negative_likelihood_ratio: f32, 364 | minimum_samples: usize, 365 | ) -> Option { 366 | let mut likelihood_ratio = 1.0; 367 | let mut inliers = 0; 368 | for data in data { 369 | likelihood_ratio *= if model.residual(&data) < self.inlier_threshold { 370 | inliers += 1; 371 | positive_likelihood_ratio 372 | } else { 373 | negative_likelihood_ratio 374 | }; 375 | 376 | if likelihood_ratio > self.likelihood_ratio_threshold || likelihood_ratio.is_nan() { 377 | return None; 378 | } 379 | } 380 | 381 | (inliers >= minimum_samples).then(|| inliers) 382 | } 383 | 384 | /// Determines the number of inliers a model has. 385 | fn count_inliers>( 386 | &self, 387 | data: impl Iterator, 388 | model: &M, 389 | ) -> usize { 390 | data.filter(|data| model.residual(data) < self.inlier_threshold) 391 | .count() 392 | } 393 | 394 | /// Gets indices of inliers for a model. 395 | fn inliers>( 396 | &self, 397 | data: impl Iterator, 398 | model: &M, 399 | ) -> Vec { 400 | data.enumerate() 401 | .filter(|(_, data)| model.residual(data) < self.inlier_threshold) 402 | .map(|(ix, _)| ix) 403 | .collect() 404 | } 405 | } 406 | 407 | impl Consensus for Arrsac 408 | where 409 | E: Estimator, 410 | R: RngCore, 411 | { 412 | type Inliers = Vec; 413 | 414 | fn model(&mut self, estimator: &E, data: I) -> Option 415 | where 416 | I: Iterator + Clone, 417 | { 418 | self.model_inliers(estimator, data).map(|(model, _)| model) 419 | } 420 | 421 | fn model_inliers(&mut self, estimator: &E, data: I) -> Option<(E::Model, Self::Inliers)> 422 | where 423 | I: Iterator + Clone, 424 | { 425 | // Don't do anything if we don't have enough data. 426 | if data.clone().count() < E::MIN_SAMPLES { 427 | return None; 428 | } 429 | // Generate the initial set of hypotheses. This also gets us an estimate of delta. 430 | let (mut hypotheses, delta) = self.initial_hypotheses(estimator, data.clone()); 431 | 432 | // If there are no initial hypotheses then initialization failed, so exit early. 433 | if hypotheses.is_empty() { 434 | return None; 435 | } 436 | 437 | // Gradually increase how many datapoints we are evaluating until we evaluate them all. 438 | // This starts at the first block that was not evaluated in initial_hypotheses. 439 | 'outer: for block in self.initialization_blocks.. { 440 | let samples_up_to_beginning_of_block = block * self.block_size; 441 | let samples_up_to_end_of_block = samples_up_to_beginning_of_block + self.block_size; 442 | // Score hypotheses with samples. 443 | for sample in samples_up_to_beginning_of_block..samples_up_to_end_of_block { 444 | // Score the hypotheses with the new datapoint. 445 | let new_datapoint = if let Some(datapoint) = data.clone().nth(sample) { 446 | datapoint 447 | } else { 448 | // We reached the last datapoint, so break out of the outer loop. 449 | break 'outer; 450 | }; 451 | for (hypothesis, inlier_count) in hypotheses.iter_mut() { 452 | if hypothesis.residual(&new_datapoint) < self.inlier_threshold { 453 | *inlier_count += 1; 454 | } 455 | } 456 | } 457 | // Sort the hypotheses by their inliers to find the best. 458 | hypotheses.sort_unstable_by_key(|&(_, inliers)| Reverse(inliers)); 459 | // Populate hypotheses with hypotheses that pass SPRT. 460 | self.populate_hypotheses_sprt( 461 | estimator, 462 | &mut hypotheses, 463 | delta, 464 | data.clone(), 465 | samples_up_to_end_of_block, 466 | self.estimations_per_block, 467 | ); 468 | // This will retain at least half of the hypotheses each time 469 | // and gradually decrease as the number of samples we are evaluating increases. 470 | // NOTE: 471 | // The paper says to use a peculiar formula that just results in doing 472 | // this basic right shift below, but as written it contained some apparent errors in 473 | // where it was ran. This seems to be the correct location to do this. 474 | hypotheses.sort_unstable_by_key(|&(_, inliers)| Reverse(inliers)); 475 | hypotheses.truncate(self.max_candidate_hypotheses >> block); 476 | if hypotheses.len() <= 1 { 477 | break 'outer; 478 | } 479 | } 480 | hypotheses 481 | .into_iter() 482 | .max_by_key(|&(_, inliers)| inliers) 483 | .map(|(model, _)| { 484 | let inliers = self.inliers(data.clone(), &model); 485 | (model, inliers) 486 | }) 487 | } 488 | } 489 | -------------------------------------------------------------------------------- /tests/lines.rs: -------------------------------------------------------------------------------- 1 | use arrsac::Arrsac; 2 | use rand::distributions::Uniform; 3 | use rand::{distributions::Distribution, Rng, SeedableRng}; 4 | use rand_xoshiro::Xoshiro256PlusPlus; 5 | use sample_consensus::{Consensus, Estimator, Model}; 6 | 7 | #[derive(Debug, Clone, Copy)] 8 | struct Vector2 { 9 | x: T, 10 | y: T, 11 | } 12 | 13 | impl Vector2 { 14 | fn new(x: f64, y: f64) -> Self { 15 | Self { x, y } 16 | } 17 | fn dot(&self, other: &Self) -> f64 { 18 | self.x * other.x + self.y * other.y 19 | } 20 | fn norm(&self) -> f64 { 21 | (self.x * self.x + self.y * self.y).sqrt() 22 | } 23 | fn normalize(&self) -> Self { 24 | let v_norm = self.norm(); 25 | Self { 26 | x: self.x / v_norm, 27 | y: self.y / v_norm, 28 | } 29 | } 30 | } 31 | 32 | impl core::ops::Mul> for f64 { 33 | type Output = Vector2; 34 | fn mul(self, rhs: Vector2) -> Self::Output { 35 | Vector2 { 36 | x: self * rhs.x, 37 | y: self * rhs.y, 38 | } 39 | } 40 | } 41 | 42 | impl core::ops::Add for Vector2 { 43 | type Output = Self; 44 | fn add(self, rhs: Self) -> Self::Output { 45 | Self { 46 | x: self.x + rhs.x, 47 | y: self.y + rhs.y, 48 | } 49 | } 50 | } 51 | 52 | #[derive(Debug)] 53 | struct Line { 54 | norm: Vector2, 55 | c: f64, 56 | } 57 | 58 | impl Model> for Line { 59 | fn residual(&self, point: &Vector2) -> f64 { 60 | (self.norm.dot(point) + self.c).abs() 61 | } 62 | } 63 | 64 | struct LineEstimator; 65 | 66 | impl Estimator> for LineEstimator { 67 | type Model = Line; 68 | type ModelIter = std::iter::Once; 69 | const MIN_SAMPLES: usize = 2; 70 | 71 | fn estimate(&self, mut data: I) -> Self::ModelIter 72 | where 73 | I: Iterator> + Clone, 74 | { 75 | let a = data.next().unwrap(); 76 | let b = data.next().unwrap(); 77 | let norm = Vector2::new(a.y - b.y, b.x - a.x).normalize(); 78 | let c = -norm.dot(&b); 79 | std::iter::once(Line { norm, c }) 80 | } 81 | } 82 | 83 | #[test] 84 | fn lines() { 85 | let mut rng = Xoshiro256PlusPlus::seed_from_u64(0); 86 | // The max candidate hypotheses had to be increased dramatically to ensure all 1000 cases find a 87 | // good-fitting line. 88 | let mut arrsac = Arrsac::new(3.0, rng.clone()); 89 | 90 | for _ in 0..2000 { 91 | // Generate and normalize. 92 | let norm = Vector2::new(rng.gen_range(-10.0..10.0), rng.gen_range(-10.0..10.0)).normalize(); 93 | // Get parallel ray. 94 | let ray = Vector2::new(norm.y, -norm.x); 95 | // Generate random c. 96 | let c = rng.gen_range(-10.0..10.0); 97 | 98 | // Generate random number of points between 50 and 1000. 99 | let num = rng.gen_range(50..1000); 100 | // The points should be no more than 5.0 away from the line and be evenly distributed away from the line. 101 | let residuals = Uniform::new(-5.0, 5.0); 102 | // The points must be generated along the line, but the distance should be bounded to make it more difficult. 103 | let distances = Uniform::new(-50.0, 50.0); 104 | // Generate the points. 105 | let points: Vec> = (0..num) 106 | .map(|_| { 107 | let residual: f64 = residuals.sample(&mut rng); 108 | let distance: f64 = distances.sample(&mut rng); 109 | let along = distance * ray; 110 | let against = (residual - c) * norm; 111 | along + against 112 | }) 113 | .collect(); 114 | 115 | let model = arrsac 116 | .model(&LineEstimator, points.iter().copied()) 117 | .expect("unable to estimate a model"); 118 | // Check the slope using the cosine distance. 119 | assert!( 120 | model.norm.dot(&norm).abs() > 0.99, 121 | "slope out of expected range" 122 | ); 123 | } 124 | } 125 | -------------------------------------------------------------------------------- /tests/no_solutions.rs: -------------------------------------------------------------------------------- 1 | use arrsac::Arrsac; 2 | use rand::SeedableRng; 3 | use rand_xoshiro::Xoshiro256PlusPlus; 4 | use sample_consensus::{Consensus, Estimator, Model}; 5 | 6 | pub struct Unsolvable(f64); 7 | 8 | impl Model for Unsolvable { 9 | fn residual(&self, _data: &i32) -> f64 { 10 | 0.01 11 | } 12 | } 13 | 14 | pub struct UnsolvableEstimator {} 15 | 16 | impl Estimator for UnsolvableEstimator { 17 | type Model = Unsolvable; 18 | type ModelIter = Option; 19 | const MIN_SAMPLES: usize = 4; 20 | 21 | fn estimate(&self, _data: I) -> Self::ModelIter 22 | where 23 | I: Iterator + Clone, 24 | { 25 | // Simulate all estimations failing. 26 | None 27 | } 28 | } 29 | 30 | /// It handles the case when the estimator fails to produce any solution from the given data 31 | #[test] 32 | pub fn no_valid_hypothesys() { 33 | let rng = Xoshiro256PlusPlus::seed_from_u64(0); 34 | let mut arrsac = Arrsac::new(3.0, rng); 35 | let estimator = UnsolvableEstimator {}; 36 | let result = arrsac.model(&estimator, 1..999); 37 | assert!(result.is_none()); 38 | } 39 | --------------------------------------------------------------------------------