├── .gitignore ├── Cargo.toml ├── LICENSE ├── README.md ├── docs ├── euroc.avif ├── tum_vi.avif └── uzh.avif ├── examples ├── DejaVuSans.ttf ├── single_camera.rs └── stereo_camera.rs └── src ├── image_utilities.rs ├── lib.rs ├── patch.rs └── tracker.rs /.gitignore: -------------------------------------------------------------------------------- 1 | # Generated by Cargo 2 | # will have compiled files and executables 3 | debug/ 4 | target/ 5 | 6 | # Remove Cargo.lock from gitignore if creating an executable, leave it for libraries 7 | # More information here https://doc.rust-lang.org/cargo/guide/cargo-toml-vs-cargo-lock.html 8 | Cargo.lock 9 | 10 | # These are backup files generated by rustfmt 11 | **/*.rs.bk 12 | 13 | # MSVC Windows builds of rustc generate these, which store debugging information 14 | *.pdb 15 | .venv 16 | result 17 | .DS_Store -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "patch-tracker" 3 | version = "0.2.1" 4 | edition = "2021" 5 | authors = ["Powei Lin "] 6 | readme = "README.md" 7 | license = "MIT OR Apache-2.0" 8 | description = "Patch tracker" 9 | homepage = "https://github.com/powei-lin/patch-tracker-rs" 10 | repository = "https://github.com/powei-lin/patch-tracker-rs" 11 | keywords = ["opticalflow"] 12 | categories = ["science", "mathematics"] 13 | exclude = [ 14 | "docs/*.avif", 15 | ] 16 | 17 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 18 | 19 | [dependencies] 20 | image = "0.25.0" 21 | imageproc = "0.25.0" 22 | log = "0.4.22" 23 | nalgebra = "0.33.0" 24 | rayon = "1.10.0" 25 | 26 | [dev-dependencies] 27 | ab_glyph = "0.2.29" 28 | env_logger = "0.11.5" 29 | glob = "0.3.1" 30 | rand = "0.8.5" 31 | rand_chacha = "0.3.1" 32 | show-image = { version = "0.14.0", features = ["image"] } 33 | clap = { version = "4.5.20", features = ["derive"] } 34 | 35 | 36 | [[example]] 37 | name = "single_camera" 38 | path = "examples/single_camera.rs" 39 | 40 | [[example]] 41 | name = "stereo_camera" 42 | path = "examples/stereo_camera.rs" 43 | 44 | [lib] 45 | name = "patch_tracker" 46 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Powei Lin 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 | # patch-tracker-rs 2 | [![crate](https://img.shields.io/crates/v/patch-tracker.svg)](https://crates.io/crates/patch-tracker) 3 | 4 | ```rust 5 | use patch_tracker::PatchTracker; 6 | 7 | let mut point_tracker = PatchTracker::<4>::default(); 8 | point_tracker.process_frame(&img_luma8); 9 | ``` 10 | 11 | # Example 12 | * [EuRoC dataset](https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) 13 | Slow down for show case. 14 | 15 | * [TUM Visual-Inertial Dataset](https://cvg.cit.tum.de/data/datasets/visual-inertial-dataset) 16 | Slow down for show case. 17 | 18 | * [The UZH FPV Dataset](https://fpv.ifi.uzh.ch/datasets) 19 | Slow down for show case. 20 | -------------------------------------------------------------------------------- /docs/euroc.avif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/powei-lin/patch-tracker-rs/9f09f609b83b196d738fa59e90fb192d5fff84dc/docs/euroc.avif -------------------------------------------------------------------------------- /docs/tum_vi.avif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/powei-lin/patch-tracker-rs/9f09f609b83b196d738fa59e90fb192d5fff84dc/docs/tum_vi.avif -------------------------------------------------------------------------------- /docs/uzh.avif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/powei-lin/patch-tracker-rs/9f09f609b83b196d738fa59e90fb192d5fff84dc/docs/uzh.avif -------------------------------------------------------------------------------- /examples/DejaVuSans.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/powei-lin/patch-tracker-rs/9f09f609b83b196d738fa59e90fb192d5fff84dc/examples/DejaVuSans.ttf -------------------------------------------------------------------------------- /examples/single_camera.rs: -------------------------------------------------------------------------------- 1 | use ab_glyph::{FontRef, PxScale}; 2 | use glob::glob; 3 | use image::ImageReader; 4 | use patch_tracker::PatchTracker; 5 | 6 | use image::Rgb; 7 | use imageproc::drawing::{draw_cross_mut, draw_text_mut}; 8 | use rand::prelude::*; 9 | use rand_chacha::ChaCha8Rng; 10 | use show_image::{create_window, event}; 11 | use std::path::PathBuf; 12 | use std::time::{Duration, Instant}; 13 | 14 | use clap::Parser; 15 | 16 | #[derive(Parser, Debug)] 17 | #[command(version, about, long_about = None)] 18 | struct Args { 19 | /// Name of the person to greet 20 | #[arg(short, long)] 21 | folder: String, 22 | } 23 | 24 | fn id_to_color(id: u64) -> [u8; 3] { 25 | let mut rng = ChaCha8Rng::seed_from_u64(id); 26 | let color_num = rng.gen_range(0..2u32.pow(24)); 27 | [ 28 | ((color_num >> 16) % 256) as u8, 29 | ((color_num >> 8) % 256) as u8, 30 | (color_num % 256) as u8, 31 | ] 32 | } 33 | 34 | #[show_image::main] 35 | fn main() { 36 | let args = Args::parse(); 37 | 38 | env_logger::init(); 39 | 40 | let path = args.folder; 41 | let path_list: Vec = glob(format!("{}/*.png", path).as_str()) 42 | .expect("Failed to read glob pattern") 43 | .filter_map(Result::ok) 44 | .collect(); 45 | if path_list.is_empty() { 46 | println!("there's no png in this folder."); 47 | return; 48 | } 49 | let mut point_tracker = PatchTracker::<3>::default(); 50 | 51 | const FPS: u32 = 10; 52 | let window = create_window("image", Default::default()).unwrap(); 53 | 54 | for (i, event) in window.event_channel().unwrap().into_iter().enumerate() { 55 | let start = Instant::now(); 56 | if i >= path_list.len() { 57 | break; 58 | } 59 | let curr_img = ImageReader::open(&path_list[i]).unwrap().decode().unwrap(); 60 | let curr_img_luma8 = curr_img.to_luma8(); 61 | 62 | point_tracker.process_frame(&curr_img_luma8); 63 | 64 | // drawing 65 | let mut curr_img_rgb = curr_img.to_rgb8(); 66 | let font = FontRef::try_from_slice(include_bytes!("DejaVuSans.ttf")).unwrap(); 67 | 68 | let height = 10.0; 69 | let scale = PxScale { 70 | x: height, 71 | y: height, 72 | }; 73 | 74 | for (id, (x, y)) in point_tracker.get_track_points() { 75 | let color = Rgb(id_to_color(id as u64)); 76 | draw_cross_mut(&mut curr_img_rgb, color, x.round() as i32, y.round() as i32); 77 | let text = format!("{}", id); 78 | draw_text_mut( 79 | &mut curr_img_rgb, 80 | color, 81 | x as i32, 82 | y as i32, 83 | scale, 84 | &font, 85 | &text, 86 | ); 87 | } 88 | // let output_name = format!("output/{:05}.png", i); 89 | // let _ = curr_img_rgb.save(output_name); 90 | 91 | window.set_image("image-001", curr_img_rgb).unwrap(); 92 | if let event::WindowEvent::KeyboardInput(event) = event { 93 | println!("{:#?}", event); 94 | if event.input.key_code == Some(event::VirtualKeyCode::Escape) 95 | && event.input.state.is_pressed() 96 | { 97 | break; 98 | } 99 | } 100 | let duration = start.elapsed(); 101 | 102 | let one_frame = Duration::new(0, 1_000_000_000u32 / FPS); 103 | if let Some(rest) = one_frame.checked_sub(duration) { 104 | ::std::thread::sleep(rest); 105 | } 106 | } 107 | } 108 | -------------------------------------------------------------------------------- /examples/stereo_camera.rs: -------------------------------------------------------------------------------- 1 | use ab_glyph::{FontRef, PxScale}; 2 | use glob::glob; 3 | use image::ImageReader; 4 | use patch_tracker::StereoPatchTracker; 5 | 6 | use image::Rgb; 7 | use imageproc::drawing::{draw_cross_mut, draw_text_mut}; 8 | use rand::prelude::*; 9 | use rand_chacha::ChaCha8Rng; 10 | use show_image::{create_window, event}; 11 | use std::path::PathBuf; 12 | use std::time::{Duration, Instant}; 13 | 14 | use clap::Parser; 15 | 16 | #[derive(Parser, Debug)] 17 | #[command(version, about, long_about = None)] 18 | struct Args { 19 | /// Name of the person to greet 20 | #[arg(short, long)] 21 | folder: String, 22 | } 23 | 24 | fn id_to_color(id: u64) -> [u8; 3] { 25 | let mut rng = ChaCha8Rng::seed_from_u64(id); 26 | let color_num = rng.gen_range(0..2u32.pow(24)); 27 | [ 28 | ((color_num >> 16) % 256) as u8, 29 | ((color_num >> 8) % 256) as u8, 30 | (color_num % 256) as u8, 31 | ] 32 | } 33 | 34 | #[show_image::main] 35 | fn main() { 36 | let args = Args::parse(); 37 | 38 | env_logger::init(); 39 | 40 | let path = args.folder; 41 | let path_list0: Vec = glob(format!("{}/mav0/cam0/data/*.png", path).as_str()) 42 | .expect("Failed to read glob pattern") 43 | .filter_map(Result::ok) 44 | .collect(); 45 | if path_list0.is_empty() { 46 | println!("there's no png in this folder."); 47 | return; 48 | } 49 | let path_list1: Vec = glob(format!("{}/mav0/cam1/data/*.png", path).as_str()) 50 | .expect("Failed to read glob pattern") 51 | .filter_map(Result::ok) 52 | .collect(); 53 | if path_list1.is_empty() { 54 | println!("there's no png in this folder."); 55 | return; 56 | } 57 | let mut point_tracker = StereoPatchTracker::<4>::default(); 58 | 59 | const FPS: u32 = 5; 60 | let window = create_window("image", Default::default()).unwrap(); 61 | let font = FontRef::try_from_slice(include_bytes!("DejaVuSans.ttf")).unwrap(); 62 | 63 | for (i, event) in window.event_channel().unwrap().into_iter().enumerate() { 64 | let start = Instant::now(); 65 | if i >= path_list0.len() { 66 | break; 67 | } 68 | let curr_img0 = ImageReader::open(&path_list0[i]).unwrap().decode().unwrap(); 69 | let curr_img1 = ImageReader::open(&path_list1[i]).unwrap().decode().unwrap(); 70 | let curr_img0_luma8 = curr_img0.to_luma8(); 71 | let curr_img1_luma8 = curr_img1.to_luma8(); 72 | 73 | point_tracker.process_frame(&curr_img0_luma8, &curr_img1_luma8); 74 | 75 | // drawing 76 | let mut curr_img_rgb0 = curr_img0.to_rgb8(); 77 | let mut curr_img_rgb1 = curr_img1.to_rgb8(); 78 | 79 | let height = 10.0; 80 | let scale = PxScale { 81 | x: height, 82 | y: height, 83 | }; 84 | 85 | let [tracked_pts0, tracked_pts1] = point_tracker.get_track_points(); 86 | for (id, (x, y)) in tracked_pts0 { 87 | let color = Rgb(id_to_color(id as u64)); 88 | draw_cross_mut(&mut curr_img_rgb0, color, x as i32, y as i32); 89 | let text = format!("{}", id); 90 | draw_text_mut( 91 | &mut curr_img_rgb0, 92 | color, 93 | x as i32, 94 | y as i32, 95 | scale, 96 | &font, 97 | &text, 98 | ); 99 | } 100 | for (id, (x, y)) in tracked_pts1 { 101 | let color = Rgb(id_to_color(id as u64)); 102 | draw_cross_mut(&mut curr_img_rgb1, color, x as i32, y as i32); 103 | let text = format!("{}", id); 104 | draw_text_mut( 105 | &mut curr_img_rgb1, 106 | color, 107 | x as i32, 108 | y as i32, 109 | scale, 110 | &font, 111 | &text, 112 | ); 113 | } 114 | // let output_name = format!("output/{:05}.png", i); 115 | // let _ = curr_img_rgb.save(output_name); 116 | // combine images 117 | let width = curr_img_rgb0.width(); 118 | let height = curr_img_rgb0.height() * 2; 119 | let mut c0 = curr_img_rgb0.to_vec(); 120 | let mut c1 = curr_img_rgb1.to_vec(); 121 | c0.append(&mut c1); 122 | let cur = image::ImageBuffer::, Vec>::from_vec(width, height, c0).unwrap(); 123 | window.set_image("image-001", cur).unwrap(); 124 | if let event::WindowEvent::KeyboardInput(event) = event { 125 | println!("{:#?}", event); 126 | if event.input.key_code == Some(event::VirtualKeyCode::Escape) 127 | && event.input.state.is_pressed() 128 | { 129 | break; 130 | } 131 | } 132 | let duration = start.elapsed(); 133 | 134 | let one_frame = Duration::new(0, 1_000_000_000u32 / FPS); 135 | if let Some(rest) = one_frame.checked_sub(duration) { 136 | ::std::thread::sleep(rest); 137 | } 138 | } 139 | } 140 | -------------------------------------------------------------------------------- /src/image_utilities.rs: -------------------------------------------------------------------------------- 1 | use image::{GenericImageView, GrayImage}; 2 | use imageproc::corners::{corners_fast9, Corner}; 3 | use nalgebra as na; 4 | 5 | pub fn image_grad(grayscale_image: &GrayImage, x: f32, y: f32) -> na::SVector { 6 | // inbound 7 | let ix = x.floor() as u32; 8 | let iy = y.floor() as u32; 9 | 10 | let dx = x - ix as f32; 11 | let dy = y - iy as f32; 12 | 13 | let ddx = 1.0 - dx; 14 | let ddy = 1.0 - dy; 15 | 16 | let px0y0 = grayscale_image.get_pixel(ix, iy).0[0] as f32; 17 | let px1y0 = grayscale_image.get_pixel(ix + 1, iy).0[0] as f32; 18 | let px0y1 = grayscale_image.get_pixel(ix, iy + 1).0[0] as f32; 19 | let px1y1 = grayscale_image.get_pixel(ix + 1, iy + 1).0[0] as f32; 20 | 21 | let res0 = ddx * ddy * px0y0 + ddx * dy * px0y1 + dx * ddy * px1y0 + dx * dy * px1y1; 22 | 23 | let pxm1y0 = grayscale_image.get_pixel(ix - 1, iy).0[0] as f32; 24 | let pxm1y1 = grayscale_image.get_pixel(ix - 1, iy + 1).0[0] as f32; 25 | 26 | let res_mx = ddx * ddy * pxm1y0 + ddx * dy * pxm1y1 + dx * ddy * px0y0 + dx * dy * px0y1; 27 | 28 | let px2y0 = grayscale_image.get_pixel(ix + 2, iy).0[0] as f32; 29 | let px2y1 = grayscale_image.get_pixel(ix + 2, iy + 1).0[0] as f32; 30 | 31 | let res_px = ddx * ddy * px1y0 + ddx * dy * px1y1 + dx * ddy * px2y0 + dx * dy * px2y1; 32 | 33 | let res1 = 0.5 * (res_px - res_mx); 34 | 35 | let px0ym1 = grayscale_image.get_pixel(ix, iy - 1).0[0] as f32; 36 | let px1ym1 = grayscale_image.get_pixel(ix + 1, iy - 1).0[0] as f32; 37 | 38 | let res_my = ddx * ddy * px0ym1 + ddx * dy * px0y0 + dx * ddy * px1ym1 + dx * dy * px1y0; 39 | 40 | let px0y2 = grayscale_image.get_pixel(ix, iy + 2).0[0] as f32; 41 | let px1y2 = grayscale_image.get_pixel(ix + 1, iy + 2).0[0] as f32; 42 | 43 | let res_py = ddx * ddy * px0y1 + ddx * dy * px0y2 + dx * ddy * px1y1 + dx * dy * px1y2; 44 | 45 | let res2 = 0.5 * (res_py - res_my); 46 | 47 | na::SVector::::new(res0, res1, res2) 48 | } 49 | 50 | pub fn point_in_bound(keypoint: &Corner, height: u32, width: u32, radius: u32) -> bool { 51 | keypoint.x >= radius 52 | && keypoint.x <= width - radius 53 | && keypoint.y >= radius 54 | && keypoint.y <= height - radius 55 | } 56 | 57 | pub fn inbound(image: &GrayImage, x: f32, y: f32, radius: u32) -> bool { 58 | let x = x.round() as u32; 59 | let y = y.round() as u32; 60 | 61 | x >= radius && y >= radius && x < image.width() - radius && y < image.height() - radius 62 | } 63 | 64 | pub fn se2_exp_matrix(a: &na::SVector) -> na::SMatrix { 65 | let theta = a[2]; 66 | let mut so2 = na::Rotation2::new(theta); 67 | let sin_theta_by_theta; 68 | let one_minus_cos_theta_by_theta; 69 | 70 | if theta.abs() < f32::EPSILON { 71 | let theta_sq = theta * theta; 72 | sin_theta_by_theta = 1.0f32 - 1.0 / 6.0 * theta_sq; 73 | one_minus_cos_theta_by_theta = 0.5f32 * theta - 1. / 24. * theta * theta_sq; 74 | } else { 75 | let cos = so2.matrix_mut_unchecked().m22; 76 | let sin = so2.matrix_mut_unchecked().m21; 77 | sin_theta_by_theta = sin / theta; 78 | one_minus_cos_theta_by_theta = (1. - cos) / theta; 79 | } 80 | let mut se2_mat = na::SMatrix::::identity(); 81 | se2_mat.m11 = so2.matrix_mut_unchecked().m11; 82 | se2_mat.m12 = so2.matrix_mut_unchecked().m12; 83 | se2_mat.m21 = so2.matrix_mut_unchecked().m21; 84 | se2_mat.m22 = so2.matrix_mut_unchecked().m22; 85 | se2_mat.m13 = sin_theta_by_theta * a[0] - one_minus_cos_theta_by_theta * a[1]; 86 | se2_mat.m23 = one_minus_cos_theta_by_theta * a[0] + sin_theta_by_theta * a[1]; 87 | se2_mat 88 | } 89 | 90 | pub fn detect_key_points( 91 | image: &GrayImage, 92 | grid_size: u32, 93 | current_corners: &Vec, 94 | num_points_in_cell: u32, 95 | ) -> Vec { 96 | const EDGE_THRESHOLD: u32 = 19; 97 | let h = image.height(); 98 | let w = image.width(); 99 | let mut all_corners = vec![]; 100 | let mut grids = 101 | na::DMatrix::::zeros((h / grid_size + 1) as usize, (w / grid_size + 1) as usize); 102 | 103 | let x_start = (w % grid_size) / 2; 104 | let x_stop = x_start + grid_size * (w / grid_size - 1) + 1; 105 | 106 | let y_start = (h % grid_size) / 2; 107 | let y_stop = y_start + grid_size * (h / grid_size - 1) + 1; 108 | 109 | // add existing corners to grid 110 | for corner in current_corners { 111 | if corner.x >= x_start 112 | && corner.y >= y_start 113 | && corner.x < x_stop + grid_size 114 | && corner.y < y_stop + grid_size 115 | { 116 | let x = (corner.x - x_start) / grid_size; 117 | let y = (corner.y - y_start) / grid_size; 118 | 119 | grids[(y as usize, x as usize)] += 1; 120 | } 121 | } 122 | 123 | for x in (x_start..x_stop).step_by(grid_size as usize) { 124 | for y in (y_start..y_stop).step_by(grid_size as usize) { 125 | if grids[( 126 | ((y - y_start) / grid_size) as usize, 127 | ((x - x_start) / grid_size) as usize, 128 | )] > 0 129 | { 130 | continue; 131 | } 132 | 133 | let image_view = image.view(x, y, grid_size, grid_size).to_image(); 134 | let mut points_added = 0; 135 | let mut threshold: u8 = 40; 136 | 137 | while points_added < num_points_in_cell && threshold >= 10 { 138 | let mut fast_corners = corners_fast9(&image_view, threshold); 139 | fast_corners.sort_by(|a, b| a.score.partial_cmp(&b.score).unwrap()); 140 | 141 | for mut point in fast_corners { 142 | if points_added >= num_points_in_cell { 143 | break; 144 | } 145 | point.x += x; 146 | point.y += y; 147 | if point_in_bound(&point, h, w, EDGE_THRESHOLD) { 148 | all_corners.push(point); 149 | points_added += 1; 150 | } 151 | } 152 | threshold -= 5; 153 | } 154 | } 155 | } 156 | all_corners 157 | } 158 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | pub mod image_utilities; 2 | pub mod patch; 3 | pub mod tracker; 4 | 5 | pub use patch::Pattern52; 6 | pub use tracker::*; 7 | -------------------------------------------------------------------------------- /src/patch.rs: -------------------------------------------------------------------------------- 1 | use image::imageops; 2 | use image::GrayImage; 3 | use nalgebra as na; 4 | use std::ops::AddAssign; 5 | 6 | use crate::image_utilities; 7 | 8 | pub const PATTERN52_SIZE: usize = 52; 9 | pub struct Pattern52 { 10 | pub valid: bool, 11 | pub mean: f32, 12 | pub pos: na::SVector, 13 | pub data: [f32; PATTERN52_SIZE], // negative if the point is not valid 14 | pub h_se2_inv_j_se2_t: na::SMatrix, 15 | pub pattern_scale_down: f32, 16 | } 17 | impl Pattern52 { 18 | pub const PATTERN_RAW: [[f32; 2]; PATTERN52_SIZE] = [ 19 | [-3.0, 7.0], 20 | [-1.0, 7.0], 21 | [1.0, 7.0], 22 | [3.0, 7.0], 23 | [-5.0, 5.0], 24 | [-3.0, 5.0], 25 | [-1.0, 5.0], 26 | [1.0, 5.0], 27 | [3.0, 5.0], 28 | [5.0, 5.0], 29 | [-7.0, 3.0], 30 | [-5.0, 3.0], 31 | [-3.0, 3.0], 32 | [-1.0, 3.0], 33 | [1.0, 3.0], 34 | [3.0, 3.0], 35 | [5.0, 3.0], 36 | [7.0, 3.0], 37 | [-7.0, 1.0], 38 | [-5.0, 1.0], 39 | [-3.0, 1.0], 40 | [-1.0, 1.0], 41 | [1.0, 1.0], 42 | [3.0, 1.0], 43 | [5.0, 1.0], 44 | [7.0, 1.0], 45 | [-7.0, -1.0], 46 | [-5.0, -1.0], 47 | [-3.0, -1.0], 48 | [-1.0, -1.0], 49 | [1.0, -1.0], 50 | [3.0, -1.0], 51 | [5.0, -1.0], 52 | [7.0, -1.0], 53 | [-7.0, -3.0], 54 | [-5.0, -3.0], 55 | [-3.0, -3.0], 56 | [-1.0, -3.0], 57 | [1.0, -3.0], 58 | [3.0, -3.0], 59 | [5.0, -3.0], 60 | [7.0, -3.0], 61 | [-5.0, -5.0], 62 | [-3.0, -5.0], 63 | [-1.0, -5.0], 64 | [1.0, -5.0], 65 | [3.0, -5.0], 66 | [5.0, -5.0], 67 | [-3.0, -7.0], 68 | [-1.0, -7.0], 69 | [1.0, -7.0], 70 | [3.0, -7.0], 71 | ]; 72 | 73 | // verified 74 | pub fn set_data_jac_se2( 75 | &mut self, 76 | greyscale_image: &GrayImage, 77 | j_se2: &mut na::SMatrix, 78 | ) { 79 | let mut num_valid_points = 0; 80 | let mut sum: f32 = 0.0; 81 | let mut grad_sum_se2 = na::SVector::::zeros(); 82 | 83 | let mut jw_se2 = na::SMatrix::::identity(); 84 | 85 | for (i, pattern_pos) in Self::PATTERN_RAW.into_iter().enumerate() { 86 | let p = self.pos 87 | + na::SVector::::new( 88 | pattern_pos[0] / self.pattern_scale_down, 89 | pattern_pos[1] / self.pattern_scale_down, 90 | ); 91 | jw_se2[(0, 2)] = -pattern_pos[1] / self.pattern_scale_down; 92 | jw_se2[(1, 2)] = pattern_pos[0] / self.pattern_scale_down; 93 | 94 | if image_utilities::inbound(greyscale_image, p.x, p.y, 2) { 95 | let val_grad = image_utilities::image_grad(greyscale_image, p.x, p.y); 96 | 97 | self.data[i] = val_grad[0]; 98 | sum += val_grad[0]; 99 | let re = val_grad.fixed_rows::<2>(1).transpose() * jw_se2; 100 | j_se2.set_row(i, &re); 101 | grad_sum_se2.add_assign(j_se2.fixed_rows::<1>(i).transpose()); 102 | num_valid_points += 1; 103 | } else { 104 | self.data[i] = -1.0; 105 | } 106 | } 107 | 108 | self.mean = sum / num_valid_points as f32; 109 | 110 | let mean_inv = num_valid_points as f32 / sum; 111 | 112 | for i in 0..Self::PATTERN_RAW.len() { 113 | if self.data[i] >= 0.0 { 114 | let rhs = grad_sum_se2.transpose() * self.data[i] / sum; 115 | j_se2.fixed_rows_mut::<1>(i).add_assign(-rhs); 116 | self.data[i] *= mean_inv; 117 | } else { 118 | j_se2.set_row(i, &na::SMatrix::::zeros()); 119 | } 120 | } 121 | *j_se2 *= mean_inv; 122 | } 123 | pub fn new(greyscale_image: &GrayImage, px: f32, py: f32) -> Pattern52 { 124 | let mut j_se2 = na::SMatrix::::zeros(); 125 | let mut p = Pattern52 { 126 | valid: false, 127 | mean: 1.0, 128 | pos: na::SVector::::new(px, py), 129 | data: [0.0; PATTERN52_SIZE], // negative if the point is not valid 130 | h_se2_inv_j_se2_t: na::SMatrix::::zeros(), 131 | pattern_scale_down: 2.0, 132 | }; 133 | p.set_data_jac_se2(greyscale_image, &mut j_se2); 134 | let h_se2 = j_se2.transpose() * j_se2; 135 | let mut h_se2_inv = na::SMatrix::::identity(); 136 | 137 | if let Some(x) = h_se2.cholesky() { 138 | x.solve_mut(&mut h_se2_inv); 139 | p.h_se2_inv_j_se2_t = h_se2_inv * j_se2.transpose(); 140 | 141 | // NOTE: while it's very unlikely we get a source patch with all black 142 | // pixels, since points are usually selected at corners, it doesn't cost 143 | // much to be safe here. 144 | 145 | // all-black patch cannot be normalized; will result in mean of "zero" and 146 | // H_se2_inv_J_se2_T will contain "NaN" and data will contain "inf" 147 | p.valid = p.mean > f32::EPSILON 148 | && p.h_se2_inv_j_se2_t.iter().all(|x| x.is_finite()) 149 | && p.data.iter().all(|x| x.is_finite()); 150 | } 151 | 152 | p 153 | } 154 | pub fn residual( 155 | &self, 156 | greyscale_image: &GrayImage, 157 | transformed_pattern: &na::SMatrix, 158 | ) -> Option> { 159 | let mut sum: f32 = 0.0; 160 | let mut num_valid_points = 0; 161 | let mut residual = na::SVector::::zeros(); 162 | for i in 0..PATTERN52_SIZE { 163 | if image_utilities::inbound( 164 | greyscale_image, 165 | transformed_pattern[(0, i)], 166 | transformed_pattern[(1, i)], 167 | 2, 168 | ) { 169 | let p = imageops::interpolate_bilinear( 170 | greyscale_image, 171 | transformed_pattern[(0, i)], 172 | transformed_pattern[(1, i)], 173 | ); 174 | residual[i] = p.unwrap().0[0] as f32; 175 | sum += residual[i]; 176 | num_valid_points += 1; 177 | } else { 178 | residual[i] = -1.0; 179 | } 180 | } 181 | 182 | // all-black patch cannot be normalized 183 | if sum < f32::EPSILON { 184 | return None; 185 | } 186 | 187 | let mut num_residuals = 0; 188 | 189 | for i in 0..PATTERN52_SIZE { 190 | if residual[i] >= 0.0 && self.data[i] >= 0.0 { 191 | let val = residual[i]; 192 | residual[i] = num_valid_points as f32 * val / sum - self.data[i]; 193 | num_residuals += 1; 194 | } else { 195 | residual[i] = 0.0; 196 | } 197 | } 198 | if num_residuals > PATTERN52_SIZE / 2 { 199 | Some(residual) 200 | } else { 201 | None 202 | } 203 | } 204 | } 205 | -------------------------------------------------------------------------------- /src/tracker.rs: -------------------------------------------------------------------------------- 1 | use image::{imageops, GrayImage}; 2 | use imageproc::corners::Corner; 3 | use nalgebra as na; 4 | use rayon::prelude::*; 5 | use std::collections::HashMap; 6 | use std::ops::AddAssign; 7 | 8 | use crate::{image_utilities, patch}; 9 | 10 | use log::info; 11 | 12 | #[derive(Default)] 13 | pub struct PatchTracker { 14 | last_keypoint_id: usize, 15 | tracked_points_map: HashMap>, 16 | previous_image_pyramid: Vec, 17 | } 18 | impl PatchTracker { 19 | pub fn process_frame(&mut self, greyscale_image: &GrayImage) { 20 | // build current image pyramid 21 | let current_image_pyramid: Vec = build_image_pyramid(greyscale_image, LEVELS); 22 | 23 | if !self.previous_image_pyramid.is_empty() { 24 | info!("old points {}", self.tracked_points_map.len()); 25 | // track prev points 26 | self.tracked_points_map = track_points::( 27 | &self.previous_image_pyramid, 28 | ¤t_image_pyramid, 29 | &self.tracked_points_map, 30 | ); 31 | info!("tracked old points {}", self.tracked_points_map.len()); 32 | } 33 | // add new points 34 | let new_points = add_points(&self.tracked_points_map, greyscale_image); 35 | for point in &new_points { 36 | let mut v = na::Affine2::::identity(); 37 | 38 | v.matrix_mut_unchecked().m13 = point.x as f32; 39 | v.matrix_mut_unchecked().m23 = point.y as f32; 40 | self.tracked_points_map.insert(self.last_keypoint_id, v); 41 | self.last_keypoint_id += 1; 42 | } 43 | 44 | // update saved image pyramid 45 | self.previous_image_pyramid = current_image_pyramid; 46 | } 47 | pub fn get_track_points(&self) -> HashMap { 48 | self.tracked_points_map 49 | .iter() 50 | .map(|(k, v)| (*k, (v.matrix().m13, v.matrix().m23))) 51 | .collect() 52 | } 53 | pub fn remove_id(&mut self, ids: &[usize]) { 54 | for id in ids { 55 | self.tracked_points_map.remove(id); 56 | } 57 | } 58 | } 59 | 60 | #[derive(Default)] 61 | pub struct StereoPatchTracker { 62 | last_keypoint_id: usize, 63 | tracked_points_map_cam0: HashMap>, 64 | previous_image_pyramid0: Vec, 65 | tracked_points_map_cam1: HashMap>, 66 | previous_image_pyramid1: Vec, 67 | } 68 | 69 | impl StereoPatchTracker { 70 | pub fn process_frame(&mut self, greyscale_image0: &GrayImage, greyscale_image1: &GrayImage) { 71 | // build current image pyramid 72 | let current_image_pyramid0: Vec = build_image_pyramid(greyscale_image0, LEVELS); 73 | let current_image_pyramid1: Vec = build_image_pyramid(greyscale_image1, LEVELS); 74 | 75 | // not initialized 76 | if !self.previous_image_pyramid0.is_empty() { 77 | info!("old points {}", self.tracked_points_map_cam0.len()); 78 | // track prev points 79 | self.tracked_points_map_cam0 = track_points::( 80 | &self.previous_image_pyramid0, 81 | ¤t_image_pyramid0, 82 | &self.tracked_points_map_cam0, 83 | ); 84 | self.tracked_points_map_cam1 = track_points::( 85 | &self.previous_image_pyramid1, 86 | ¤t_image_pyramid1, 87 | &self.tracked_points_map_cam1, 88 | ); 89 | info!("tracked old points {}", self.tracked_points_map_cam0.len()); 90 | } 91 | // add new points 92 | let new_points0 = add_points(&self.tracked_points_map_cam0, greyscale_image0); 93 | let tmp_tracked_points0: HashMap = new_points0 94 | .iter() 95 | .enumerate() 96 | .map(|(i, point)| { 97 | let mut v = na::Affine2::::identity(); 98 | v.matrix_mut_unchecked().m13 = point.x as f32; 99 | v.matrix_mut_unchecked().m23 = point.y as f32; 100 | (i, v) 101 | }) 102 | .collect(); 103 | 104 | let tmp_tracked_points1 = track_points::( 105 | ¤t_image_pyramid0, 106 | ¤t_image_pyramid1, 107 | &tmp_tracked_points0, 108 | ); 109 | 110 | for (key0, pt0) in tmp_tracked_points0 { 111 | if let Some(pt1) = tmp_tracked_points1.get(&key0) { 112 | self.tracked_points_map_cam0 113 | .insert(self.last_keypoint_id, pt0); 114 | self.tracked_points_map_cam1 115 | .insert(self.last_keypoint_id, *pt1); 116 | self.last_keypoint_id += 1; 117 | } 118 | } 119 | 120 | // update saved image pyramid 121 | self.previous_image_pyramid0 = current_image_pyramid0; 122 | self.previous_image_pyramid1 = current_image_pyramid1; 123 | } 124 | pub fn get_track_points(&self) -> [HashMap; 2] { 125 | let tracked_pts0 = self 126 | .tracked_points_map_cam0 127 | .iter() 128 | .map(|(k, v)| (*k, (v.matrix().m13, v.matrix().m23))) 129 | .collect(); 130 | let tracked_pts1 = self 131 | .tracked_points_map_cam1 132 | .iter() 133 | .map(|(k, v)| (*k, (v.matrix().m13, v.matrix().m23))) 134 | .collect(); 135 | [tracked_pts0, tracked_pts1] 136 | } 137 | pub fn remove_id(&mut self, ids: &[usize]) { 138 | for id in ids { 139 | self.tracked_points_map_cam0.remove(id); 140 | self.tracked_points_map_cam1.remove(id); 141 | } 142 | } 143 | } 144 | 145 | fn build_image_pyramid(greyscale_image: &GrayImage, levels: u32) -> Vec { 146 | const FILTER_TYPE: imageops::FilterType = imageops::FilterType::Triangle; 147 | let (w0, h0) = greyscale_image.dimensions(); 148 | (0..levels) 149 | .into_par_iter() 150 | .map(|i| { 151 | let scale_down: u32 = 1 << i; 152 | let (new_w, new_h) = (w0 / scale_down, h0 / scale_down); 153 | imageops::resize(greyscale_image, new_w, new_h, FILTER_TYPE) 154 | }) 155 | .collect() 156 | } 157 | 158 | fn add_points( 159 | tracked_points_map: &HashMap>, 160 | grayscale_image: &GrayImage, 161 | ) -> Vec { 162 | const GRID_SIZE: u32 = 50; 163 | let num_points_in_cell = 1; 164 | let current_corners: Vec = tracked_points_map 165 | .values() 166 | .map(|v| { 167 | Corner::new( 168 | v.matrix().m13.round() as u32, 169 | v.matrix().m23.round() as u32, 170 | 0.0, 171 | ) 172 | }) 173 | .collect(); 174 | // let curr_img_luma8 = DynamicImage::ImageLuma16(grayscale_image.clone()).into_luma8(); 175 | image_utilities::detect_key_points( 176 | grayscale_image, 177 | GRID_SIZE, 178 | ¤t_corners, 179 | num_points_in_cell, 180 | ) 181 | // let mut prev_points = 182 | // Eigen::aligned_vector pts0; 183 | 184 | // for (const auto &kv : observations.at(0)) { 185 | // pts0.emplace_back(kv.second.translation().template cast()); 186 | // } 187 | } 188 | fn track_points( 189 | image_pyramid0: &[GrayImage], 190 | image_pyramid1: &[GrayImage], 191 | transform_maps0: &HashMap>, 192 | ) -> HashMap> { 193 | let transform_maps1: HashMap> = transform_maps0 194 | .par_iter() 195 | .filter_map(|(k, v)| { 196 | if let Some(new_v) = track_one_point::(image_pyramid0, image_pyramid1, v) { 197 | // return Some((k.clone(), new_v)); 198 | if let Some(old_v) = 199 | track_one_point::(image_pyramid1, image_pyramid0, &new_v) 200 | { 201 | if (v.matrix() - old_v.matrix()) 202 | .fixed_view::<2, 1>(0, 2) 203 | .norm_squared() 204 | < 0.4 205 | { 206 | return Some((*k, new_v)); 207 | } 208 | } 209 | } 210 | None 211 | }) 212 | .collect(); 213 | 214 | transform_maps1 215 | } 216 | fn track_one_point( 217 | image_pyramid0: &[GrayImage], 218 | image_pyramid1: &[GrayImage], 219 | transform0: &na::Affine2, 220 | ) -> Option> { 221 | let mut patch_valid = true; 222 | let mut transform1 = na::Affine2::::identity(); 223 | transform1.matrix_mut_unchecked().m13 = transform0.matrix().m13; 224 | transform1.matrix_mut_unchecked().m23 = transform0.matrix().m23; 225 | 226 | for i in (0..LEVELS).rev() { 227 | let scale_down = 1 << i; 228 | 229 | transform1.matrix_mut_unchecked().m13 /= scale_down as f32; 230 | transform1.matrix_mut_unchecked().m23 /= scale_down as f32; 231 | 232 | let pattern = patch::Pattern52::new( 233 | &image_pyramid0[i as usize], 234 | transform0.matrix().m13 / scale_down as f32, 235 | transform0.matrix().m23 / scale_down as f32, 236 | ); 237 | patch_valid &= pattern.valid; 238 | if patch_valid { 239 | // Perform tracking on current level 240 | patch_valid &= 241 | track_point_at_level(&image_pyramid1[i as usize], &pattern, &mut transform1); 242 | if !patch_valid { 243 | return None; 244 | } 245 | } else { 246 | return None; 247 | } 248 | 249 | transform1.matrix_mut_unchecked().m13 *= scale_down as f32; 250 | transform1.matrix_mut_unchecked().m23 *= scale_down as f32; 251 | // transform1.matrix_mut_unchecked().m33 = 1.0; 252 | } 253 | let new_r_mat = transform0.matrix() * transform1.matrix(); 254 | transform1.matrix_mut_unchecked().m11 = new_r_mat.m11; 255 | transform1.matrix_mut_unchecked().m12 = new_r_mat.m12; 256 | transform1.matrix_mut_unchecked().m21 = new_r_mat.m21; 257 | transform1.matrix_mut_unchecked().m22 = new_r_mat.m22; 258 | Some(transform1) 259 | } 260 | 261 | pub fn track_point_at_level( 262 | grayscale_image: &GrayImage, 263 | dp: &patch::Pattern52, 264 | transform: &mut na::Affine2, 265 | ) -> bool { 266 | // let mut patch_valid: bool = false; 267 | let optical_flow_max_iterations = 5; 268 | let patten = na::SMatrix::::from_fn(|i, j| { 269 | patch::Pattern52::PATTERN_RAW[i][j] / dp.pattern_scale_down 270 | }) 271 | .transpose(); 272 | // transform. 273 | // println!("before {}", transform.matrix()); 274 | for _iteration in 0..optical_flow_max_iterations { 275 | let mut transformed_pat = transform.matrix().fixed_view::<2, 2>(0, 0) * patten; 276 | for i in 0..52 { 277 | transformed_pat 278 | .column_mut(i) 279 | .add_assign(transform.matrix().fixed_view::<2, 1>(0, 2)); 280 | } 281 | // println!("{}", smatrix.transpose()); 282 | // let mut res = na::SVector::::zeros(); 283 | if let Some(res) = dp.residual(grayscale_image, &transformed_pat) { 284 | let inc = -dp.h_se2_inv_j_se2_t * res; 285 | 286 | // avoid NaN in increment (leads to SE2::exp crashing) 287 | if !inc.iter().all(|x| x.is_finite()) { 288 | return false; 289 | } 290 | if inc.norm() > 1e6 { 291 | return false; 292 | } 293 | let new_trans = transform.matrix() * image_utilities::se2_exp_matrix(&inc); 294 | *transform = na::Affine2::::from_matrix_unchecked(new_trans); 295 | let filter_margin = 2; 296 | if !image_utilities::inbound( 297 | grayscale_image, 298 | transform.matrix_mut_unchecked().m13, 299 | transform.matrix_mut_unchecked().m23, 300 | filter_margin, 301 | ) { 302 | return false; 303 | } 304 | } 305 | } 306 | 307 | true 308 | } 309 | --------------------------------------------------------------------------------