Add fast remap (#6)

powei-lin · web-flow · commit 23962ecb8aa1 · 2025-04-13T18:17:51.000-04:00
* bench

* remove par iter

* save

* add doc

* fast remap

* typo

* typo

* fast rgb

* bench

* readme

* update version

* panic
diff --git a/Cargo.toml b/Cargo.toml
@@ -1,6 +1,6 @@
 [package]
 name = "camera-intrinsic-model"
-version = "0.4.1"
+version = "0.5.0"
 edition = "2021"
 authors = ["Powei Lin <poweilin1994@gmail.com>"]
 readme = "README.md"
@@ -10,7 +10,7 @@ homepage = "https://github.com/powei-lin/camera-intrinsic-model-rs"
 repository = "https://github.com/powei-lin/camera-intrinsic-model-rs"
 keywords = ["camera-intrinsic", "intrinsic", "fisheye"]
 categories = ["data-structures", "science", "mathematics", "science::robotics"]
-exclude = ["/.github/*", "*.ipynb", "./scripts/*", "examples/*", "tests/*", "data/*"]
+exclude = ["/.github/*", "*.ipynb", "./scripts/*", "examples/*", "tests/*", "data/*", "benches/*"]
 
 [dependencies]
 image = "0.25.6"
@@ -25,6 +25,11 @@ name = "remap"
 [[example]]
 name = "stereo_rectify"
 
+[[bench]]
+name = "bench"
+harness = false
+
 [dev-dependencies]
+diol = "0.13.0"
 imageproc = "0.25.0"
 rand = "0.9.0"
diff --git a/README.md b/README.md
@@ -25,6 +25,21 @@ cargo run -r --example remap
 cargo run -r --example stereo_rectify
 ```
 
+## Benchmark
+Remapping to 1024x1024 on m4 mac mini.
+```
+╭───────────────────────────────────────────────────────────────────────╮
+│                                 remap                                 │
+├────────────────┬──────┬───────────┬───────────┬───────────┬───────────┤
+│ benchmark      │ args │   fastest │    median │      mean │    stddev │
+├────────────────┼──────┼───────────┼───────────┼───────────┼───────────┤
+│ mono8 normal   │ None │ 732.17 µs │ 827.00 µs │ 858.60 µs │  94.88 µs │
+│ mono8 fast     │ None │ 272.00 µs │ 342.25 µs │ 360.68 µs │ 228.13 µs │
+│ rgb8 normal    │ None │   1.87 ms │   2.00 ms │   2.04 ms │ 143.60 µs │
+│ rgb8 fast      │ None │ 751.67 µs │ 824.54 µs │ 851.30 µs │  79.45 µs │
+╰────────────────┴──────┴───────────┴───────────┴───────────┴───────────╯
+```
+
 ## Acknowledgements
 Links:
 * https://cvg.cit.tum.de/data/datasets/visual-inertial-dataset
diff --git a/benches/bench.rs b/benches/bench.rs
@@ -0,0 +1,81 @@
+use camera_intrinsic_model::{compute_for_fast_remap, fast_remap, model_from_json, remap};
+use diol::prelude::*;
+use image::{DynamicImage, ImageReader};
+
+fn main() -> eyre::Result<()> {
+    let bench = Bench::from_args()?;
+    bench.register_many(
+        "remap",
+        list![
+            bench_remap.with_name("mono8 normal"),
+            bench_remap_fast.with_name("mono8 fast"),
+            bench_remap_rgb.with_name("rgb8 normal"),
+            bench_remap_fast_rgb.with_name("rgb8 fast"),
+        ],
+        [None],
+    );
+    bench.run()?;
+    Ok(())
+}
+
+fn bench_remap(bencher: Bencher, _dummy: Option<bool>) {
+    let model1 = model_from_json("data/eucm0.json");
+    let new_w_h = 1024;
+    let img = ImageReader::open("data/tum_vi_with_chart.png")
+        .unwrap()
+        .decode()
+        .unwrap();
+    let p = model1.estimate_new_camera_matrix_for_undistort(0.0, Some((new_w_h, new_w_h)));
+    let (xmap, ymap) = model1.init_undistort_map(&p, (new_w_h, new_w_h), None);
+    let img_l8 = DynamicImage::ImageLuma8(img.to_luma8());
+    bencher.bench(|| {
+        let _ = remap(&img_l8, &xmap, &ymap);
+    });
+}
+
+fn bench_remap_rgb(bencher: Bencher, _dummy: Option<bool>) {
+    let model1 = model_from_json("data/eucm0.json");
+    let new_w_h = 1024;
+    let img = ImageReader::open("data/tum_vi_with_chart.png")
+        .unwrap()
+        .decode()
+        .unwrap();
+    let p = model1.estimate_new_camera_matrix_for_undistort(0.0, Some((new_w_h, new_w_h)));
+    let (xmap, ymap) = model1.init_undistort_map(&p, (new_w_h, new_w_h), None);
+    let img_rgb8 = DynamicImage::ImageRgb8(img.to_rgb8());
+    bencher.bench(|| {
+        let _ = remap(&img_rgb8, &xmap, &ymap);
+    });
+}
+
+fn bench_remap_fast(bencher: Bencher, _dummy: Option<bool>) {
+    let model1 = model_from_json("data/eucm0.json");
+    let new_w_h = 1024;
+    let img = ImageReader::open("data/tum_vi_with_chart.png")
+        .unwrap()
+        .decode()
+        .unwrap();
+    let p = model1.estimate_new_camera_matrix_for_undistort(0.0, Some((new_w_h, new_w_h)));
+    let (xmap, ymap) = model1.init_undistort_map(&p, (new_w_h, new_w_h), None);
+    let img_l8 = DynamicImage::ImageLuma8(img.to_luma8());
+    let xy_pos_weight = compute_for_fast_remap(&xmap, &ymap);
+    bencher.bench(|| {
+        let _ = fast_remap(&img_l8, (new_w_h, new_w_h), &xy_pos_weight);
+    });
+}
+
+fn bench_remap_fast_rgb(bencher: Bencher, _dummy: Option<bool>) {
+    let model1 = model_from_json("data/eucm0.json");
+    let new_w_h = 1024;
+    let img = ImageReader::open("data/tum_vi_with_chart.png")
+        .unwrap()
+        .decode()
+        .unwrap();
+    let p = model1.estimate_new_camera_matrix_for_undistort(0.0, Some((new_w_h, new_w_h)));
+    let (xmap, ymap) = model1.init_undistort_map(&p, (new_w_h, new_w_h), None);
+    let img_rgb = DynamicImage::ImageRgb8(img.to_rgb8());
+    let xy_pos_weight = compute_for_fast_remap(&xmap, &ymap);
+    bencher.bench(|| {
+        let _ = fast_remap(&img_rgb, (new_w_h, new_w_h), &xy_pos_weight);
+    });
+}
diff --git a/benches/opencv_test.py b/benches/opencv_test.py
@@ -0,0 +1,20 @@
+import cv2
+import numpy as np
+from time import perf_counter
+
+
+def main():
+    img = (255 * np.random.random((512, 512))).astype(np.uint8)
+    mapx = (511 * np.random.random((1024, 1024))).astype(np.float32)
+    mapy = (511 * np.random.random((1024, 1024))).astype(np.float32)
+    # mapx, mapy = cv2.convertMaps(mapx, mapy, cv2.CV_16SC2)
+    tt = 1000
+    t0 = perf_counter()
+    for i in range(tt):
+        cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
+    t1 = perf_counter()
+    print(f"{(t1 - t0) / tt}")
+
+
+if __name__ == "__main__":
+    main()
diff --git a/examples/remap.rs b/examples/remap.rs
@@ -1,5 +1,5 @@
 use camera_intrinsic_model::*;
-use image::ImageReader;
+use image::{DynamicImage, ImageReader};
 use nalgebra as na;
 
 fn main() {
@@ -22,6 +22,15 @@ fn main() {
     let new_w_h = 1024;
     let p = model1.estimate_new_camera_matrix_for_undistort(0.0, Some((new_w_h, new_w_h)));
     let (xmap, ymap) = model1.init_undistort_map(&p, (new_w_h, new_w_h), None);
-    let remaped = remap(&img, &xmap, &ymap);
-    remaped.save("remaped.png").unwrap()
+    let img_l8 = DynamicImage::ImageLuma8(img.to_luma8());
+    let remaped = remap(&img_l8, &xmap, &ymap);
+    remaped.save("remaped0.png").unwrap();
+
+    let xy_pos_weight = compute_for_fast_remap(&xmap, &ymap);
+    let remaped1 = fast_remap(&img_l8, (new_w_h, new_w_h), &xy_pos_weight);
+    remaped1.save("remaped1.png").unwrap();
+
+    let img_rgb8 = DynamicImage::ImageRgb8(img.to_rgb8());
+    let remaped1 = fast_remap(&img_rgb8, (new_w_h, new_w_h), &xy_pos_weight);
+    remaped1.save("remaped2.png").unwrap();
 }
diff --git a/src/generic_functions.rs b/src/generic_functions.rs
@@ -1,5 +1,6 @@
 use super::generic_model::*;
 
+use image::{DynamicImage, GenericImageView, GrayImage, ImageBuffer};
 use nalgebra as na;
 use rayon::prelude::*;
 
@@ -44,7 +45,7 @@ pub fn init_undistort_map(
         .into_par_iter()
         .flat_map(|y| {
             (0..new_w_h.0)
-                .into_par_iter()
+                .into_iter()
                 .map(|x| {
                     rmat_inv * na::Vector3::new((x as f64 - cx) / fx, (y as f64 - cy) / fy, 1.0)
                 })
@@ -53,7 +54,7 @@ pub fn init_undistort_map(
         .collect();
     let p2ds = camera_model.project(&p3ds);
     let (xvec, yvec): (Vec<f32>, Vec<f32>) = p2ds
-        .par_iter()
+        .iter()
         .map(|xy| {
             if let Some(xy) = xy {
                 (xy[0] as f32, xy[1] as f32)
@@ -67,6 +68,123 @@ pub fn init_undistort_map(
     (xmap, ymap)
 }
 
+#[inline]
+fn interpolate_bilinear_weight(x: f32, y: f32) -> (u32, u32) {
+    if x < 0.0 || x > 65535.0 {
+        panic!("x not in [0-65535]");
+    }
+    if y < 0.0 || y > 65535.0 {
+        panic!("y not in [0-65535]");
+    }
+    const UPPER: f32 = u8::MAX as f32;
+    let x_weight = (UPPER * (x.ceil() - x)) as u32;
+    let y_weight = (UPPER * (y.ceil() - y)) as u32;
+    // 0-255
+    (x_weight, y_weight)
+}
+
+pub fn compute_for_fast_remap(
+    xmap: &na::DMatrix<f32>,
+    ymap: &na::DMatrix<f32>,
+) -> Vec<(u32, u32, u32, u32)> {
+    let xy_pos_weight_vec: Vec<_> = xmap
+        .iter()
+        .zip(ymap.iter())
+        .map(|(&x, &y)| {
+            let (xw, yw) = interpolate_bilinear_weight(x, y);
+            let x0 = x.floor() as u32;
+            let y0 = y.floor() as u32;
+            (x0, y0, xw, yw)
+        })
+        .collect();
+    xy_pos_weight_vec
+}
+
+fn reinterpret_vec(input: Vec<[u8; 3]>) -> Vec<u8> {
+    let len = input.len() * 3;
+    let capacity = input.capacity() * 3;
+    let ptr = input.as_ptr() as *mut u8;
+    std::mem::forget(input); // prevent dropping original vec
+    unsafe { Vec::from_raw_parts(ptr, len, capacity) }
+}
+
+pub fn fast_remap(
+    img: &DynamicImage,
+    new_w_h: (u32, u32),
+    xy_pos_weight_vec: &[(u32, u32, u32, u32)],
+) -> DynamicImage {
+    let remaped = match img {
+        DynamicImage::ImageLuma8(image_buffer) => {
+            let val: Vec<u8> = xy_pos_weight_vec
+                .par_iter()
+                .map(|&(x0, y0, xw0, yw0)| {
+                    let p00 = unsafe { image_buffer.unsafe_get_pixel(x0, y0)[0] as u32 };
+                    let p10 = unsafe { image_buffer.unsafe_get_pixel(x0 + 1, y0)[0] as u32 };
+                    let p01 = unsafe { image_buffer.unsafe_get_pixel(x0, y0 + 1)[0] as u32 };
+                    let p11 = unsafe { image_buffer.unsafe_get_pixel(x0 + 1, y0 + 1)[0] as u32 };
+                    let xw1 = 255 - xw0;
+                    let yw1 = 255 - yw0;
+                    const UPPER_UPPER: u32 = 255 * 255;
+                    let p =
+                        ((p00 * xw0 * yw0 + p10 * xw1 * yw0 + p01 * xw0 * yw1 + p11 * xw1 * yw1)
+                            / UPPER_UPPER) as u8;
+                    p
+                })
+                .collect();
+            let img = GrayImage::from_vec(new_w_h.0, new_w_h.1, val).unwrap();
+            DynamicImage::ImageLuma8(img)
+        }
+        DynamicImage::ImageRgb8(image_buffer) => {
+            let val: Vec<[u8; 3]> = xy_pos_weight_vec
+                .par_iter()
+                .map(|&(x0, y0, xw0, yw0)| {
+                    let p00 = unsafe { image_buffer.unsafe_get_pixel(x0, y0) };
+                    let p10 = unsafe { image_buffer.unsafe_get_pixel(x0 + 1, y0) };
+                    let p01 = unsafe { image_buffer.unsafe_get_pixel(x0, y0 + 1) };
+                    let p11 = unsafe { image_buffer.unsafe_get_pixel(x0 + 1, y0 + 1) };
+                    let mut v = [0, 1, 2];
+                    v.iter_mut().for_each(|i| {
+                        let xw1 = 255 - xw0;
+                        let yw1 = 255 - yw0;
+                        let c = *i as usize;
+                        const UPPER_UPPER: u32 = 255 * 255;
+                        *i = ((p00.0[c] as u32 * xw0 * yw0
+                            + p10.0[c] as u32 * xw1 * yw0
+                            + p01.0[c] as u32 * xw0 * yw1
+                            + p11.0[c] as u32 * xw1 * yw1)
+                            / UPPER_UPPER) as u8;
+                    });
+                    v
+                })
+                .collect();
+            let img = ImageBuffer::from_vec(new_w_h.0, new_w_h.1, reinterpret_vec(val)).unwrap();
+            DynamicImage::ImageRgb8(img)
+        }
+        DynamicImage::ImageLuma16(image_buffer) => {
+            let val: Vec<u16> = xy_pos_weight_vec
+                .par_iter()
+                .map(|&(x0, y0, xw0, yw0)| {
+                    let p00 = unsafe { image_buffer.unsafe_get_pixel(x0, y0)[0] as u32 };
+                    let p10 = unsafe { image_buffer.unsafe_get_pixel(x0 + 1, y0)[0] as u32 };
+                    let p01 = unsafe { image_buffer.unsafe_get_pixel(x0, y0 + 1)[0] as u32 };
+                    let p11 = unsafe { image_buffer.unsafe_get_pixel(x0 + 1, y0 + 1)[0] as u32 };
+                    let xw1 = 255 - xw0;
+                    let yw1 = 255 - yw0;
+                    const UPPER_UPPER: u32 = 255 * 255;
+                    let p =
+                        ((p00 * xw0 * yw0 + p10 * xw1 * yw0 + p01 * xw0 * yw1 + p11 * xw1 * yw1)
+                            / UPPER_UPPER) as u16;
+                    p
+                })
+                .collect();
+            let img = ImageBuffer::from_vec(new_w_h.0, new_w_h.1, val).unwrap();
+            DynamicImage::ImageLuma16(img)
+        }
+        _ => panic!("Only mono8, mono16, and rgb8 support fast remap."),
+    };
+    remaped
+}
+
 /// Returns xmap and ymap for remaping
 ///
 /// # Arguments
diff --git a/src/lib.rs b/src/lib.rs
@@ -1,3 +1,14 @@
+//! camera-intrinsic-model is a library for distort/undistort images.
+//! # Examples
+//!
+//! ```
+//! use camera_intrinsic_model::*;
+//! let model = model_from_json("data/eucm0.json");
+//! let new_w_h = 1024;
+//! let p = model.estimate_new_camera_matrix_for_undistort(0.0, Some((new_w_h, new_w_h)));
+//! let (xmap, ymap) = model.init_undistort_map(&p, (new_w_h, new_w_h), None);
+//! // let remaped = remap(&img, &xmap, &ymap);
+//! ```
 pub mod generic_functions;
 pub mod generic_model;
 pub mod io;
