feat(simple-viewer): adjust view-port correctly when scaling window

Cargo.toml

@@ -53,7 +53,7 @@ image = {version = "0.25", features = [
 ]}
 linked-hash-map = "0.5"
 log = "0.4"
-nalgebra = {version = "0.32", features = ["rand"]}
+nalgebra = {version = "0.33", features = ["rand"]}
 ndarray = {version = "0.15", features = ["approx-0_5"]}
 num-traits = "0.2"
 numpy = "0.21"

crates/sophus/examples/viewer_ex.rs

@@ -8,7 +8,14 @@ use sophus::viewer::actor::ViewerBuilder;
 use sophus::viewer::actor::ViewerCamera;
 use sophus::viewer::actor::ViewerConfig;
 use sophus::viewer::renderables::*;
+use sophus_core::linalg::VecF64;
+use sophus_image::intensity_image::intensity_arc_image::IsIntensityArcImage;
+use sophus_image::mut_image::MutImageF32;
+use sophus_image::mut_image_view::IsMutImageView;
+use sophus_lie::traits::IsTranslationProductGroup;
 use sophus_lie::Isometry3;
+use sophus_sensor::DynCamera;
+use sophus_viewer::interactions::WgpuClippingPlanes;
 use sophus_viewer::renderables::color::Color;
 use sophus_viewer::renderables::renderable2d::Points2;
 use sophus_viewer::renderables::renderable2d::Renderable2d;
@@ -96,8 +103,11 @@ pub struct ContentGeneratorOutbound {
     pub packets: OutboundChannel<Stream<Packets>>,
 }
 
-fn create_view2_packet(image_size: ImageSize) -> Packet {
-    let img = make_example_image(image_size);
+fn create_view2_packet() -> Packet {
+    let img = make_example_image(ImageSize {
+        width: 300,
+        height: 100,
+    });
 
     let mut packet_2d = View2dPacket {
         view_label: "view_2d".to_owned(),
@@ -109,18 +119,47 @@ fn create_view2_packet(image_size: ImageSize) -> Packet {
     let points2 = Points2::make("points2", &[[16.0, 12.0], [32.0, 24.0]], &Color::red(), 5.0);
     packet_2d.renderables2d.push(Renderable2d::Points2(points2));
 
+    let lines2 = Lines2::make("lines2", &[[[0.0, 0.0], [20.0, 20.0]]], &Color::red(), 5.0);
+    packet_2d.renderables2d.push(Renderable2d::Lines2(lines2));
+
+    Packet::View2d(packet_2d)
+}
+
+fn create_tiny_view2_packet() -> Packet {
+    let mut img = MutImageF32::from_image_size_and_val(ImageSize::new(3, 2), 1.0);
+
+    *img.mut_pixel(0, 0) = 0.0;
+    *img.mut_pixel(0, 1) = 0.5;
+
+    *img.mut_pixel(1, 1) = 0.0;
+
+    *img.mut_pixel(2, 0) = 0.3;
+    *img.mut_pixel(2, 1) = 0.6;
+
+    let mut packet_2d = View2dPacket {
+        view_label: "tiny".to_owned(),
+        renderables3d: vec![],
+        renderables2d: vec![],
+        frame: Some(Frame::from_image(&img.to_shared().to_rgba())),
+    };
+
     let lines2 = Lines2::make(
         "lines2",
-        &[[[0.0, 0.0], [100.0, 100.0]]],
+        &[[[-0.5, -0.5], [0.5, 0.5]], [[0.5, -0.5], [-0.5, 0.5]]],
         &Color::red(),
-        5.0,
+        0.3,
     );
     packet_2d.renderables2d.push(Renderable2d::Lines2(lines2));
 
     Packet::View2d(packet_2d)
 }
 
-fn create_view3_packet(initial_camera: ViewerCamera) -> Packet {
+fn create_view3_packet() -> Packet {
+    let initial_camera = ViewerCamera {
+        intrinsics: DynCamera::default_distorted(ImageSize::new(639, 477)),
+        clipping_planes: WgpuClippingPlanes::default(),
+        scene_from_camera: Isometry3::from_t(&VecF64::<3>::new(0.0, 0.0, -5.0)),
+    };
     let mut packet_3d = View3dPacket {
         view_label: "view_3d".to_owned(),
         renderables3d: vec![],
@@ -164,16 +203,12 @@ impl HasOnMessage for ContentGeneratorMessage {
     ) {
         match &self {
             ContentGeneratorMessage::ClockTick(_time_in_seconds) => {
-                let initial_camera = ViewerCamera::default();
-
                 let mut packets = Packets { packets: vec![] };
 
                 if state.counter == 0 {
-                    packets
-                        .packets
-                        .push(create_view2_packet(initial_camera.intrinsics.image_size()));
-
-                    packets.packets.push(create_view3_packet(initial_camera));
+                    packets.packets.push(create_view2_packet());
+                    packets.packets.push(create_tiny_view2_packet());
+                    packets.packets.push(create_view3_packet());
                 }
 
                 state.counter += 1;

crates/sophus/src/lib.rs

@@ -1,9 +1,7 @@
 #![cfg_attr(feature = "simd", feature(portable_simd))]
 #![allow(clippy::needless_range_loop)]
-
 #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))]
 
-
 pub mod examples;
 
 #[doc(inline)]

crates/sophus_sensor/src/distortion_table.rs

@@ -43,62 +43,48 @@ impl DistortTable {
     }
 }
 
-/// Returns the undistortion lookup table
-pub fn undistort_table(cam: &DynCamera<f64, 1>) -> MutImage2F32 {
-    let mut table = MutImage2F32::from_image_size(cam.image_size());
-    let w = cam.image_size().width;
-    let h = cam.image_size().height;
-    for v in 0..h {
-        for u in 0..w {
-            let pixel = cam.undistort(&VecF64::<2>::from_f64_array([u as f64, v as f64]));
-            *table.mut_pixel(u, v) = pixel.cast();
-        }
-    }
-    table
-}
-
 /// Returns the distortion lookup table
 pub fn distort_table(cam: &DynCamera<f64, 1>) -> DistortTable {
-    // first we find min and max values in the proj plane
-    // just test the 4 corners might not be enough
-    // so we will test the image boundary
+    // First we find min and max values in the proj plane.
+    // Just test the 4 corners might not be enough, so we will test the image boundary.
 
     let mut region = Region::<2>::empty();
 
-    let w = cam.image_size().width;
-    let h = cam.image_size().height;
+    let v_top = -0.5;
+    let v_bottom = cam.image_size().height as f64 - 0.5;
+
+    let u_left = -0.5;
+    let u_right = cam.image_size().width as f64 - 0.5;
 
-    for u in 0..cam.image_size().width {
+    for i in 0..=cam.image_size().width {
+        let u = i as f64 - 0.5;
         // top border
-        let v = 0;
-        let point_in_proj = cam.undistort(&VecF64::<2>::from_f64_array([u as f64, v as f64]));
-        region.extend(&point_in_proj);
+        region.extend(&cam.undistort(&VecF64::<2>::new(u, v_top)));
         // bottom border
-        let v = cam.image_size().height - 1;
-        let point_in_proj = cam.undistort(&VecF64::<2>::new(u as f64, v as f64));
-        region.extend(&point_in_proj);
+        region.extend(&cam.undistort(&VecF64::<2>::new(u, v_bottom)));
     }
-    for v in 0..cam.image_size().height {
+    for i in 0..=cam.image_size().height {
+        let v = i as f64 - 0.5;
         // left border
-        let u = 0;
-        let point_in_proj = cam.undistort(&VecF64::<2>::new(u as f64, v as f64));
-        region.extend(&point_in_proj);
+        region.extend(&cam.undistort(&VecF64::<2>::new(u_left, v)));
         // right border
-        let u = cam.image_size().width - 1;
-        let point_in_proj = cam.undistort(&VecF64::<2>::new(u as f64, v as f64));
-        region.extend(&point_in_proj);
+        region.extend(&cam.undistort(&VecF64::<2>::new(u_right, v)));
     }
-    let region = Region::<2>::from_min_max(region.min().cast() * 2.0, region.max().cast() * 2.0);
+
+    let mid = region.mid();
+    let range = region.range();
+
+    let larger_region = Region::<2>::from_min_max(mid - range, mid + range);
 
     let mut distort_table = DistortTable {
         table: ArcImage2F32::from_image_size_and_val(cam.image_size(), SVector::<f32, 2>::zeros()),
-        region,
+        region: larger_region,
     };
 
     let mut table = MutImage2F32::from_image_size(cam.image_size());
 
-    for v in 0..h {
-        for u in 0..w {
+    for v in 0..cam.image_size().height {
+        for u in 0..cam.image_size().width {
             let point_proj = VecF64::<2>::new(
                 distort_table.offset().x + (u as f64) * distort_table.incr().x,
                 distort_table.offset().y + (v as f64) * distort_table.incr().y,
@@ -110,3 +96,96 @@ pub fn distort_table(cam: &DynCamera<f64, 1>) -> DistortTable {
     distort_table.table = table.into();
     distort_table
 }
+
+#[test]
+fn camera_distortion_table_tests() {
+    use crate::distortion_table::distort_table;
+    use crate::distortion_table::DistortTable;
+    use approx::assert_abs_diff_eq;
+    use approx::assert_relative_eq;
+    use sophus_core::calculus::maps::vector_valued_maps::VectorValuedMapFromVector;
+    use sophus_core::linalg::VecF64;
+    use sophus_image::ImageSize;
+
+    type DynCameraF64 = DynCamera<f64, 1>;
+
+    {
+        let mut cameras: Vec<DynCameraF64> = vec![];
+        cameras.push(DynCameraF64::new_pinhole(
+            &VecF64::<4>::new(600.0, 600.0, 1.0, 0.5),
+            ImageSize {
+                width: 3,
+                height: 2,
+            },
+        ));
+
+        cameras.push(DynCamera::new_kannala_brandt(
+            &VecF64::<8>::from_vec(vec![1000.0, 1000.0, 320.0, 280.0, 0.1, 0.01, 0.001, 0.0001]),
+            ImageSize {
+                width: 640,
+                height: 480,
+            },
+        ));
+
+        for camera in cameras {
+            let pixels_in_image = vec![
+                VecF64::<2>::new(0.0, 0.0),
+                VecF64::<2>::new(2.0, 1.0),
+                VecF64::<2>::new(2.9, 1.9),
+                VecF64::<2>::new(2.5, 1.5),
+                VecF64::<2>::new(3.0, 2.0),
+                VecF64::<2>::new(-0.5, -0.5),
+                VecF64::<2>::new(1.0, 400.0),
+                VecF64::<2>::new(320.0, 240.0),
+                VecF64::<2>::new(319.5, 239.5),
+                VecF64::<2>::new(100.0, 40.0),
+                VecF64::<2>::new(639.0, 479.0),
+            ];
+
+            for pixel in pixels_in_image.clone() {
+                if pixel[0] > camera.image_size().width as f64
+                    || pixel[1] > camera.image_size().height as f64
+                {
+                    continue;
+                }
+
+                for d in [1.0, 0.1, 0.5, 1.1, 3.0, 15.0] {
+                    let point_in_camera = camera.cam_unproj_with_z(&pixel, d);
+                    assert_relative_eq!(point_in_camera[2], d, epsilon = 1e-6);
+
+                    let pixel_in_image2 = camera.cam_proj(&point_in_camera);
+                    assert_relative_eq!(pixel_in_image2, pixel, epsilon = 1e-6);
+                }
+                let ab_in_z1plane = camera.undistort(&pixel);
+
+                let pixel_in_image3 = camera.distort(&ab_in_z1plane);
+                assert_relative_eq!(pixel_in_image3, pixel, epsilon = 1e-6);
+
+                let dx = camera.dx_distort_x(&pixel);
+                let numeric_dx = VectorValuedMapFromVector::static_sym_diff_quotient(
+                    |x: VecF64<2>| camera.distort(&x),
+                    pixel,
+                    1e-6,
+                );
+
+                assert_relative_eq!(dx, numeric_dx, epsilon = 1e-4);
+            }
+
+            let table: DistortTable = distort_table(&camera);
+
+            for pixel in pixels_in_image {
+                if pixel[0] >= camera.image_size().width as f64
+                    || pixel[1] >= camera.image_size().height as f64
+                {
+                    continue;
+                }
+                let proj = camera.undistort(&pixel);
+                let analytic_pixel = camera.distort(&proj);
+                let lut_pixel = table.lookup(&proj);
+
+                assert_abs_diff_eq!(analytic_pixel, pixel, epsilon = 1e-3);
+                assert_abs_diff_eq!(analytic_pixel, lut_pixel, epsilon = 1e-3);
+            }
+        }
+    }
+}