├── .clang-format ├── .github └── workflows │ ├── format.yml │ ├── nightly.yml │ └── stable.yml ├── .gitignore ├── .pre-commit-config.yaml ├── Cargo.toml ├── LICENSE-APACHE ├── LICENSE-MIT ├── README.md ├── crates ├── sophus │ ├── Cargo.toml │ ├── build.rs │ ├── examples │ │ ├── camera_sim.rs │ │ ├── mini_optics_sim.rs │ │ └── viewer_ex.rs │ └── src │ │ ├── examples.rs │ │ ├── examples │ │ ├── optics_sim.rs │ │ ├── optics_sim │ │ │ ├── aperture_stop.rs │ │ │ ├── convex_lens.rs │ │ │ ├── cost.rs │ │ │ ├── detector.rs │ │ │ ├── element.rs │ │ │ ├── light_path.rs │ │ │ └── scene_point.rs │ │ └── viewer_example.rs │ │ └── lib.rs ├── sophus_autodiff │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── dual.rs │ │ ├── dual │ │ ├── dual_batch_matrix.rs │ │ ├── dual_batch_scalar.rs │ │ ├── dual_batch_vector.rs │ │ ├── dual_matrix.rs │ │ ├── dual_scalar.rs │ │ ├── dual_vector.rs │ │ ├── matrix.rs │ │ ├── scalar.rs │ │ └── vector.rs │ │ ├── lib.rs │ │ ├── linalg.rs │ │ ├── linalg │ │ ├── batch_mask.rs │ │ ├── batch_matrix.rs │ │ ├── batch_scalar.rs │ │ ├── batch_vector.rs │ │ ├── bool_mask.rs │ │ ├── matrix.rs │ │ ├── scalar.rs │ │ └── vector.rs │ │ ├── manifold.rs │ │ ├── maps.rs │ │ ├── maps │ │ ├── curves.rs │ │ ├── matrix_valued_maps.rs │ │ ├── scalar_valued_maps.rs │ │ └── vector_valued_maps.rs │ │ ├── params.rs │ │ └── points.rs ├── sophus_geo │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── hyperplane.rs │ │ ├── hypersphere.rs │ │ ├── lib.rs │ │ ├── ray.rs │ │ ├── region.rs │ │ ├── region │ │ ├── box_region.rs │ │ └── interval.rs │ │ └── unit_vector.rs ├── sophus_image │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── arc_image.rs │ │ ├── color_map.rs │ │ ├── image_view.rs │ │ ├── intensity_image.rs │ │ ├── intensity_image │ │ ├── dyn_intensity_image.rs │ │ ├── intensity_arc_image.rs │ │ ├── intensity_image_view.rs │ │ ├── intensity_mut_image.rs │ │ ├── intensity_pixel.rs │ │ └── intensity_scalar.rs │ │ ├── interpolation.rs │ │ ├── io.rs │ │ ├── io │ │ ├── png.rs │ │ └── tiff.rs │ │ ├── lib.rs │ │ ├── mut_image.rs │ │ └── mut_image_view.rs ├── sophus_lie │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── complex.rs │ │ ├── groups.rs │ │ ├── groups │ │ ├── affine_group_template.rs │ │ ├── galilean3.rs │ │ ├── isometry2.rs │ │ ├── isometry3.rs │ │ ├── rotation2.rs │ │ ├── rotation3.rs │ │ └── rotation_boost3.rs │ │ ├── lib.rs │ │ ├── lie_group.rs │ │ ├── lie_group │ │ ├── average.rs │ │ ├── factor_lie_group.rs │ │ ├── group_mul.rs │ │ ├── lie_group_manifold.rs │ │ └── real_lie_group.rs │ │ ├── quaternion.rs │ │ ├── sl2c.rs │ │ └── traits.rs ├── sophus_opt │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── asserts.rs │ │ ├── block.rs │ │ ├── block │ │ ├── block_gradient.rs │ │ ├── block_hessian.rs │ │ ├── block_jacobian.rs │ │ ├── block_sparse_matrix_builder.rs │ │ ├── block_vector.rs │ │ ├── grid.rs │ │ └── symmetric_block_sparse_matrix_builder.rs │ │ ├── example_problems.rs │ │ ├── example_problems │ │ ├── cam_calib.rs │ │ ├── linear_eq_toy_problem.rs │ │ ├── non_linear_eq_toy_problem.rs │ │ ├── pose_circle.rs │ │ └── simple_prior.rs │ │ ├── lib.rs │ │ ├── nlls.rs │ │ ├── nlls │ │ ├── constraint.rs │ │ ├── constraint │ │ │ ├── eq_constraint.rs │ │ │ ├── eq_constraint_fn.rs │ │ │ ├── evaluated_eq_constraint.rs │ │ │ └── evaluated_eq_set.rs │ │ ├── cost.rs │ │ ├── cost │ │ │ ├── compare_idx.rs │ │ │ ├── cost_fn.rs │ │ │ ├── cost_term.rs │ │ │ ├── evaluated_cost.rs │ │ │ └── evaluated_term.rs │ │ ├── functor_library.rs │ │ ├── functor_library │ │ │ ├── costs.rs │ │ │ ├── costs │ │ │ │ ├── example_non_linear_cost.rs │ │ │ │ ├── isometry2_prior.rs │ │ │ │ ├── isometry3_prior.rs │ │ │ │ ├── pinhole_reprojection.rs │ │ │ │ ├── pose_graph.rs │ │ │ │ ├── quadratic1.rs │ │ │ │ └── quadratic2.rs │ │ │ ├── eq_constraints.rs │ │ │ └── eq_constraints │ │ │ │ ├── example_linear_eq.rs │ │ │ │ ├── example_non_linear_eq.rs │ │ │ │ └── spherical_constraint.rs │ │ ├── linear_system.rs │ │ └── linear_system │ │ │ ├── cost_system.rs │ │ │ ├── eq_system.rs │ │ │ ├── solvers.rs │ │ │ └── solvers │ │ │ ├── dense_lu.rs │ │ │ ├── sparse_ldlt.rs │ │ │ ├── sparse_lu.rs │ │ │ └── sparse_qr.rs │ │ ├── robust_kernel.rs │ │ ├── variables.rs │ │ └── variables │ │ ├── var_builder.rs │ │ ├── var_families.rs │ │ ├── var_family.rs │ │ └── var_tuple.rs ├── sophus_renderer │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── camera.rs │ │ ├── camera │ │ ├── clipping_planes.rs │ │ ├── intrinsics.rs │ │ └── properties.rs │ │ ├── lib.rs │ │ ├── offscreen_renderer.rs │ │ ├── pipeline_builder.rs │ │ ├── pixel_renderer.rs │ │ ├── pixel_renderer │ │ ├── pixel_line.rs │ │ └── pixel_point.rs │ │ ├── render_context.rs │ │ ├── renderables.rs │ │ ├── renderables │ │ ├── color.rs │ │ ├── frame.rs │ │ ├── pixel_renderable.rs │ │ ├── scene_renderable.rs │ │ └── scene_renderable │ │ │ └── axes.rs │ │ ├── scene_renderer.rs │ │ ├── scene_renderer │ │ ├── distortion.rs │ │ ├── line.rs │ │ ├── mesh.rs │ │ ├── point.rs │ │ └── textured_mesh.rs │ │ ├── shaders │ │ ├── distortion.wgsl │ │ ├── pixel_line.wgsl │ │ ├── pixel_point.wgsl │ │ ├── scene_line.wgsl │ │ ├── scene_mesh.wgsl │ │ ├── scene_point.wgsl │ │ ├── scene_textured_mesh.wgsl │ │ └── utils.wgsl │ │ ├── textures.rs │ │ ├── textures │ │ ├── depth.rs │ │ ├── depth_image.rs │ │ ├── ndc_z_buffer.rs │ │ ├── rgba.rs │ │ └── visual_depth.rs │ │ ├── types.rs │ │ └── uniform_buffers.rs ├── sophus_sensor │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── camera.rs │ │ ├── camera_enum.rs │ │ ├── camera_enum │ │ ├── general_camera.rs │ │ └── perspective_camera.rs │ │ ├── distortion_table.rs │ │ ├── distortions.rs │ │ ├── distortions │ │ ├── affine.rs │ │ ├── brown_conrady.rs │ │ ├── enhanced_unified.rs │ │ └── kannala_brandt.rs │ │ ├── dyn_camera.rs │ │ ├── lib.rs │ │ ├── projections.rs │ │ ├── projections │ │ ├── orthographic.rs │ │ └── perspective.rs │ │ └── traits.rs ├── sophus_sim │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── camera_simulator.rs │ │ └── lib.rs ├── sophus_spline │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── lib.rs │ │ └── spline_segment.rs ├── sophus_tensor │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ ├── arc_tensor.rs │ │ ├── element.rs │ │ ├── lib.rs │ │ ├── mut_tensor.rs │ │ ├── mut_tensor_view.rs │ │ └── tensor_view.rs ├── sophus_timeseries │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ │ └── lib.rs └── sophus_viewer │ ├── Cargo.toml │ ├── README.md │ ├── build.rs │ └── src │ ├── interactions.rs │ ├── interactions │ ├── inplane_interaction.rs │ └── orbit_interaction.rs │ ├── lib.rs │ ├── packets.rs │ ├── packets │ ├── image_view_packet.rs │ ├── plot_view_packet.rs │ ├── plot_view_packet │ │ ├── curve_vec_with_conf.rs │ │ ├── scalar_curve.rs │ │ └── vec_curve.rs │ └── scene_view_packet.rs │ ├── simple_viewer.rs │ ├── viewer_base.rs │ ├── views.rs │ └── views │ ├── active_view_info.rs │ ├── image_view.rs │ ├── plot_view.rs │ └── scene_view.rs ├── justfile ├── publish_all.sh ├── rustfmt.toml └── sophus-rs.code-workspace /.github/workflows/format.yml: -------------------------------------------------------------------------------- 1 | name: format 2 | on: 3 | push: 4 | branches: 5 | - main 6 | pull_request: 7 | branches: 8 | - main 9 | 10 | jobs: 11 | format: 12 | runs-on: ubuntu-22.04 13 | steps: 14 | - name: Checkout workspace 15 | uses: actions/checkout@v3 16 | - uses: extractions/setup-just@v1 17 | - name: Install pre-commit and install 18 | run: | 19 | pip install pre-commit 20 | pre-commit install 21 | - name: Run pre-commit checks 22 | run: pre-commit run --all-files 23 | - uses: actions-rs/toolchain@v1 24 | with: 25 | toolchain: nightly 26 | - name: cargo fmt 27 | run: | 28 | rustup default nightly 29 | rustup component add rustfmt 30 | just format 31 | - name: cargo doc 32 | run: | 33 | rustup default nightly 34 | rustup component add rustfmt 35 | just doc 36 | -------------------------------------------------------------------------------- /.github/workflows/nightly.yml: -------------------------------------------------------------------------------- 1 | name: nightly 2 | on: 3 | push: 4 | branches: [main] 5 | pull_request: 6 | 7 | workflow_dispatch: 8 | jobs: 9 | nightly: 10 | runs-on: ubuntu-24.04 11 | 12 | env: 13 | SCCACHE_GHA_ENABLED: "true" 14 | RUSTC_WRAPPER: "sccache" 15 | 16 | steps: 17 | - uses: actions/checkout@v3 18 | with: 19 | submodules: "recursive" 20 | - name: Run sccache-cache 21 | uses: mozilla-actions/sccache-action@v0.0.9 22 | - uses: actions-rs/toolchain@v1 23 | with: 24 | toolchain: nightly 25 | - uses: extractions/setup-just@v1 26 | - name: cargo build 27 | env: 28 | SCCACHE_GHA_ENABLED: "true" 29 | RUSTC_WRAPPER: "sccache" 30 | run: | 31 | rustup default nightly 32 | just build-simd 33 | - name: cargo test 34 | env: 35 | SCCACHE_GHA_ENABLED: "true" 36 | RUSTC_WRAPPER: "sccache" 37 | run: | 38 | rustup default nightly 39 | just test-simd 40 | -------------------------------------------------------------------------------- /.github/workflows/stable.yml: -------------------------------------------------------------------------------- 1 | name: stable 2 | on: 3 | push: 4 | branches: [main] 5 | pull_request: 6 | 7 | workflow_dispatch: 8 | jobs: 9 | stable: 10 | runs-on: ubuntu-22.04 11 | 12 | env: 13 | SCCACHE_GHA_ENABLED: "true" 14 | RUSTC_WRAPPER: "sccache" 15 | 16 | steps: 17 | - uses: actions/checkout@v3 18 | with: 19 | submodules: "recursive" 20 | - name: Run sccache-cache 21 | uses: mozilla-actions/sccache-action@v0.0.9 22 | - uses: extractions/setup-just@v1 23 | - uses: actions-rs/toolchain@v1 24 | with: 25 | toolchain: stable 26 | - name: cargo build 27 | env: 28 | SCCACHE_GHA_ENABLED: "true" 29 | RUSTC_WRAPPER: "sccache" 30 | run: | 31 | cargo build --release --all-targets 32 | just build-std 33 | - name: cargo test 34 | env: 35 | SCCACHE_GHA_ENABLED: "true" 36 | RUSTC_WRAPPER: "sccache" 37 | run: | 38 | cargo test --release 39 | - name: cargo clippy 40 | env: 41 | SCCACHE_GHA_ENABLED: "true" 42 | RUSTC_WRAPPER: "sccache" 43 | run: | 44 | rustup component add clippy 45 | cargo clippy --all-targets 46 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | Cargo.lock 3 | .vscode/ 4 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.1.0 4 | hooks: 5 | - id: trailing-whitespace 6 | - id: end-of-file-fixer 7 | exclude: (thirdparty/.*)|(SOPHUS_VERSION)|(.txt)$ 8 | - id: check-yaml 9 | args: ["--unsafe"] 10 | - id: check-json 11 | - repo: https://github.com/codespell-project/codespell 12 | rev: v2.1.0 13 | hooks: 14 | - id: codespell 15 | args: 16 | - --ignore-words-list 17 | - "te,tring,crate" 18 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [workspace] 2 | members = [ 3 | "crates/sophus", 4 | "crates/sophus_autodiff", 5 | "crates/sophus_tensor", 6 | "crates/sophus_image", 7 | "crates/sophus_lie", 8 | "crates/sophus_geo", 9 | "crates/sophus_spline", 10 | "crates/sophus_sensor", 11 | "crates/sophus_opt", 12 | "crates/sophus_timeseries", 13 | "crates/sophus_renderer", 14 | "crates/sophus_viewer", 15 | "crates/sophus_sim", 16 | ] 17 | resolver = "3" 18 | 19 | [workspace.package] 20 | edition = "2024" 21 | include = ["**/*.rs", "**/*.wgsl", "**/*.md", "**/Cargo.toml"] 22 | keywords = ["robotics", "optimization"] 23 | license = "MIT OR Apache-2.0" 24 | repository = "https://github.com/sophus-vision/sophus-rs/" 25 | version = "0.15.0" 26 | rust-version = "1.87.0" 27 | 28 | [workspace.dependencies] 29 | sophus = { path = "crates/sophus", version = "0.15.0" } 30 | sophus_autodiff = { path = "crates/sophus_autodiff", version = "0.15.0" } 31 | sophus_tensor = { path = "crates/sophus_tensor", version = "0.15.0" } 32 | sophus_image = { path = "crates/sophus_image", version = "0.15.0" } 33 | sophus_lie = { path = "crates/sophus_lie", version = "0.15.0" } 34 | sophus_geo = { path = "crates/sophus_geo", version = "0.15.0" } 35 | sophus_spline = { path = "crates/sophus_spline", version = "0.15.0" } 36 | sophus_sensor = { path = "crates/sophus_sensor", version = "0.15.0" } 37 | sophus_opt = { path = "crates/sophus_opt", version = "0.15.0" } 38 | sophus_timeseries = { path = "crates/sophus_timeseries", version = "0.15.0" } 39 | sophus_renderer = { path = "crates/sophus_renderer", version = "0.15.0" } 40 | sophus_sim = { path = "crates/sophus_sim", version = "0.15.0" } 41 | sophus_viewer = { path = "crates/sophus_viewer", version = "0.15.0" } 42 | 43 | approx = "0.5" 44 | as-any = "0.3" 45 | async-trait = "0.1" 46 | bytemuck = { version = "1.20.0", features = ["derive"] } 47 | concat-arrays = "0.1" 48 | dyn-clone = "1.0" 49 | env_logger = "0.11" 50 | faer = { version = "0.22.6" } 51 | linked-hash-map = "0.5" 52 | log = "0.4" 53 | nalgebra = { version = "0.33.2", features = ["rand", "bytemuck"] } 54 | ndarray = { version = "0.16", features = ["approx"] } 55 | pollster = { version = "0.4" } 56 | num-traits = "0.2" 57 | rand = "0.9" 58 | rustc_version = "0.4" 59 | snafu = "0.8.6" 60 | typenum = { version = "1.17", features = ["const-generics"] } 61 | winit = { version = "0.30.11", features = ["android-native-activity"] } 62 | 63 | # viewer 64 | eframe = { version = "0.31.1", features = ["wgpu"] } 65 | egui-wgpu = "0.31.1" 66 | egui_plot = "0.32.1" 67 | egui_extras = "0.31.1" 68 | thingbuf = "0.1.6" 69 | 70 | [profile.release] 71 | debug = 1 72 | -------------------------------------------------------------------------------- /LICENSE-MIT: -------------------------------------------------------------------------------- 1 | Copyright (c) 2023 Hauke Strasdat 2 | Copyright (c) 2023 farm-ng, inc. 3 | 4 | Permission is hereby granted, free of charge, to any 5 | person obtaining a copy of this software and associated 6 | documentation files (the "Software"), to deal in the 7 | Software without restriction, including without 8 | limitation the rights to use, copy, modify, merge, 9 | publish, distribute, sublicense, and/or sell copies of 10 | the Software, and to permit persons to whom the Software 11 | is furnished to do so, subject to the following 12 | conditions: 13 | 14 | The above copyright notice and this permission notice 15 | shall be included in all copies or substantial portions 16 | of the Software. 17 | 18 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 19 | ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 20 | TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 21 | PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT 22 | SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 23 | CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 24 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 25 | IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 26 | DEALINGS IN THE SOFTWARE. 27 | -------------------------------------------------------------------------------- /crates/sophus/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "sophus - geometry for robotics and computer vision" 3 | name = "sophus" 4 | readme = "../../README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_geo.workspace = true 16 | sophus_image.workspace = true 17 | sophus_lie.workspace = true 18 | sophus_opt.workspace = true 19 | sophus_tensor.workspace = true 20 | sophus_sensor.workspace = true 21 | sophus_renderer.workspace = true 22 | sophus_sim.workspace = true 23 | sophus_spline.workspace = true 24 | sophus_timeseries.workspace = true 25 | sophus_viewer.workspace = true 26 | 27 | nalgebra.workspace = true 28 | ndarray.workspace = true 29 | 30 | approx.workspace = true 31 | bytemuck.workspace = true 32 | eframe.workspace = true 33 | egui-wgpu.workspace = true 34 | egui_extras.workspace = true 35 | env_logger.workspace = true 36 | pollster.workspace = true 37 | rand.workspace = true 38 | thingbuf.workspace = true 39 | 40 | [build-dependencies] 41 | rustc_version.workspace = true 42 | 43 | 44 | [features] 45 | simd = [ 46 | "sophus_autodiff/simd", 47 | "sophus_image/simd", 48 | "sophus_lie/simd", 49 | "sophus_opt/simd", 50 | "sophus_sensor/simd", 51 | ] 52 | std = [ 53 | "sophus_autodiff/std", 54 | "sophus_image/std", 55 | "sophus_lie/std", 56 | "sophus_opt/std", 57 | "sophus_sensor/std", 58 | ] 59 | default = ["std"] 60 | 61 | [[example]] 62 | name = "camera_sim" 63 | required-features = ["std"] 64 | 65 | [[example]] 66 | name = "viewer_ex" 67 | required-features = ["std"] 68 | -------------------------------------------------------------------------------- /crates/sophus/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus/examples/camera_sim.rs: -------------------------------------------------------------------------------- 1 | #![cfg(feature = "std")] 2 | 3 | use sophus::{ 4 | lie::Isometry3, 5 | prelude::*, 6 | sim::camera_simulator::CameraSimulator, 7 | }; 8 | use sophus_image::{ 9 | ImageSize, 10 | io::{ 11 | save_as_png, 12 | save_as_tiff, 13 | }, 14 | }; 15 | use sophus_renderer::{ 16 | RenderContext, 17 | camera::RenderCameraProperties, 18 | renderables::{ 19 | Color, 20 | make_line3, 21 | make_mesh3_at, 22 | make_point3, 23 | }, 24 | }; 25 | 26 | pub async fn run_offscreen() { 27 | let render_state = RenderContext::new().await; 28 | 29 | let mut sim = CameraSimulator::new( 30 | &render_state, 31 | &RenderCameraProperties::default_from(ImageSize::new(639, 477)), 32 | ); 33 | 34 | let mut renderables3d = vec![]; 35 | let trig_points = [[0.0, 0.0, -0.1], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]; 36 | renderables3d.push(make_point3("points3", &trig_points, &Color::red(), 5.0)); 37 | renderables3d.push(make_line3( 38 | "lines3", 39 | &[ 40 | [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0]], 41 | [[0.0, 0.0, 0.0], [0.0, 1.0, 0.0]], 42 | [[0.0, 0.0, 0.0], [0.0, 0.0, 1.0]], 43 | ], 44 | &Color::green(), 45 | 5.0, 46 | )); 47 | let blue = Color::blue(); 48 | renderables3d.push(make_mesh3_at( 49 | "mesh", 50 | &[(trig_points, blue)], 51 | Isometry3::trans_z(3.0), 52 | )); 53 | 54 | sim.update_3d_renderables(renderables3d); 55 | 56 | let result = sim.render(Isometry3::trans_z(-5.0)); 57 | 58 | save_as_png(&result.rgba_image.image_view(), "rgba.png").unwrap(); 59 | 60 | let color_mapped_depth = result.depth_image.color_mapped(); 61 | save_as_png(&color_mapped_depth.image_view(), "color_mapped_depth.png").unwrap(); 62 | 63 | save_as_tiff( 64 | &result.depth_image.metric_depth().image_view(), 65 | "depth.tiff", 66 | ) 67 | .unwrap(); 68 | } 69 | 70 | fn main() { 71 | env_logger::init(); 72 | 73 | pollster::block_on(run_offscreen()); 74 | } 75 | -------------------------------------------------------------------------------- /crates/sophus/src/examples.rs: -------------------------------------------------------------------------------- 1 | /// Optics simulation components. 2 | pub mod optics_sim; 3 | /// Viewer example utilities. 4 | pub mod viewer_example; 5 | -------------------------------------------------------------------------------- /crates/sophus/src/examples/optics_sim.rs: -------------------------------------------------------------------------------- 1 | use std::sync::Arc; 2 | 3 | use sophus_autodiff::linalg::VecF64; 4 | use sophus_opt::{ 5 | nlls::{ 6 | CostFn, 7 | CostTerms, 8 | OptParams, 9 | optimize_nlls, 10 | }, 11 | variables::{ 12 | VarBuilder, 13 | VarFamily, 14 | VarKind, 15 | }, 16 | }; 17 | 18 | use crate::examples::optics_sim::{ 19 | convex_lens::BiConvexLens2F64, 20 | cost::ChiefRayCost, 21 | }; 22 | 23 | /// Aperture stop in an optical system. 24 | pub mod aperture_stop; 25 | /// BiConvex lens in an optical system. 26 | pub mod convex_lens; 27 | /// Cost terms for the optimization process. 28 | pub mod cost; 29 | /// Detector in an optical system. 30 | pub mod detector; 31 | /// Elements in an optical system. 32 | pub mod element; 33 | /// The light path of rays through the optical system. 34 | pub mod light_path; 35 | /// Scene points in the optical system. 36 | pub mod scene_point; 37 | 38 | /// Refines the angle of the chief ray using a nonlinear least squares optimization. 39 | pub fn refine_chief_ray_angle( 40 | angle: f64, 41 | scene_point: VecF64<2>, 42 | lens: Arc, 43 | target_point: VecF64<2>, 44 | ) -> f64 { 45 | let family: VarFamily> = VarFamily::new(VarKind::Free, vec![VecF64::<1>::new(angle)]); 46 | let variables = VarBuilder::new().add_family("angle", family).build(); 47 | let mut chief_ray_cost = CostTerms::new(["angle"], vec![]); 48 | chief_ray_cost.collection.push(ChiefRayCost { 49 | scene_point, 50 | aperture: target_point, 51 | entity_indices: [0], 52 | }); 53 | let solution = optimize_nlls( 54 | variables, 55 | vec![CostFn::new_boxed(lens, chief_ray_cost)], 56 | OptParams { 57 | num_iterations: 100, 58 | initial_lm_damping: 1.0, 59 | parallelize: true, 60 | solver: Default::default(), 61 | }, 62 | ) 63 | .unwrap(); 64 | solution.variables.get_members::>("angle")[0][0] 65 | } 66 | -------------------------------------------------------------------------------- /crates/sophus/src/examples/optics_sim/aperture_stop.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::VecF64; 2 | use sophus_geo::LineF64; 3 | use sophus_renderer::renderables::{ 4 | Color, 5 | SceneRenderable, 6 | make_line3, 7 | }; 8 | 9 | use crate::examples::optics_sim::element::Element; 10 | 11 | /// A struct representing the aperture stop in an optical system. 12 | #[derive(Clone)] 13 | pub struct ApertureStop { 14 | /// The x-coordinate of the aperture stop. 15 | pub x: f64, 16 | /// The radius of the aperture stop. 17 | pub radius: f64, 18 | } 19 | 20 | impl ApertureStop { 21 | /// Plane through the aperture stop, perpendicular to the optical axis. 22 | pub fn aperture_plane(&self) -> LineF64 { 23 | let p0 = VecF64::<2>::new(self.x, -0.200); 24 | let p1 = VecF64::<2>::new(self.x, 0.200); 25 | LineF64::from_point_pair(p0, p1) 26 | } 27 | } 28 | 29 | impl Element for ApertureStop { 30 | fn to_renderable3(&self) -> SceneRenderable { 31 | let up = 0.5; 32 | make_line3( 33 | "aperture", 34 | &[ 35 | [ 36 | VecF64::<3>::new(self.x, up, 0.0).cast(), 37 | VecF64::<3>::new(self.x, self.radius, 0.0).cast(), 38 | ], 39 | [ 40 | VecF64::<3>::new(self.x, -up, 0.0).cast(), 41 | VecF64::<3>::new(self.x, -self.radius, 0.0).cast(), 42 | ], 43 | ], 44 | &Color::black(1.0), 45 | 5.0, 46 | ) 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /crates/sophus/src/examples/optics_sim/cost.rs: -------------------------------------------------------------------------------- 1 | use std::sync::Arc; 2 | 3 | use sophus_autodiff::{ 4 | dual::DualScalar, 5 | linalg::VecF64, 6 | maps::VectorValuedVectorMap, 7 | }; 8 | use sophus_geo::{ 9 | LineF64, 10 | Ray, 11 | UnitVector, 12 | }; 13 | use sophus_lie::prelude::IsSingleScalar; 14 | use sophus_opt::{ 15 | nlls::{ 16 | EvaluatedCostTerm, 17 | HasResidualFn, 18 | MakeEvaluatedCostTerm, 19 | }, 20 | robust_kernel::RobustKernel, 21 | variables::VarKind, 22 | }; 23 | 24 | use crate::examples::optics_sim::convex_lens::BiConvexLens2F64; 25 | 26 | /// Cost term for the chief ray. 27 | #[derive(Debug, Clone)] 28 | pub struct ChiefRayCost { 29 | /// The entity indices for the cost term. 30 | pub entity_indices: [usize; 1], 31 | /// The scene point where the ray originates. 32 | pub scene_point: VecF64<2>, 33 | /// The aperture stop of the lens. 34 | pub aperture: VecF64<2>, 35 | } 36 | 37 | impl ChiefRayCost { 38 | /// Residual function for the chief ray cost term. 39 | pub fn residual, const DM: usize, const DN: usize>( 40 | angle_rad: VecF64<1>, 41 | scene_point: &VecF64<2>, 42 | lens: &Arc, 43 | aperture: &VecF64<2>, 44 | ) -> VecF64<1> { 45 | let ray = Ray { 46 | origin: *scene_point, 47 | dir: UnitVector::from_vector_and_normalize(&VecF64::<2>::new( 48 | angle_rad[0].cos(), 49 | angle_rad[0].sin(), 50 | )), 51 | }; 52 | 53 | let out_ray = match lens.refract(ray.clone()) { 54 | Some(r) => r[1].clone(), 55 | None => ray, 56 | }; 57 | 58 | let intersection_point = 59 | LineF64::from_point_pair(*aperture, VecF64::<2>::new(aperture[0], 10.0)) 60 | .rays_intersect(&out_ray) 61 | .unwrap(); 62 | VecF64::<1>::new(intersection_point[1] - aperture[1]) 63 | } 64 | } 65 | 66 | impl HasResidualFn<1, 1, Arc, VecF64<1>> for ChiefRayCost { 67 | fn eval( 68 | &self, 69 | global_constants: &Arc, 70 | idx: [usize; 1], 71 | angle_rad: VecF64<1>, 72 | derivatives: [VarKind; 1], 73 | robust_kernel: Option, 74 | ) -> EvaluatedCostTerm<1, 1> { 75 | let residual = Self::residual::( 76 | angle_rad, 77 | &self.scene_point, 78 | global_constants, 79 | &self.aperture, 80 | ); 81 | 82 | let dx_res_fn = |x: VecF64<1>| -> VecF64<1> { 83 | Self::residual::, 1, 1>( 84 | x, 85 | &self.scene_point, 86 | global_constants, 87 | &self.aperture, 88 | ) 89 | }; 90 | (|| { 91 | VectorValuedVectorMap::::sym_diff_quotient_jacobian(dx_res_fn, angle_rad, 0.001) 92 | },) 93 | .make(idx, derivatives, residual, robust_kernel, None) 94 | } 95 | 96 | fn idx_ref(&self) -> &[usize; 1] { 97 | &self.entity_indices 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /crates/sophus/src/examples/optics_sim/detector.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::VecF64; 2 | use sophus_geo::LineF64; 3 | use sophus_renderer::renderables::{ 4 | Color, 5 | SceneRenderable, 6 | make_line3, 7 | }; 8 | 9 | use crate::examples::optics_sim::element::Element; 10 | 11 | /// Detector in an optical system, i.e., the image plane. 12 | #[derive(Clone)] 13 | pub struct Detector { 14 | /// The x-coordinate of the detector. 15 | pub x: f64, 16 | } 17 | 18 | impl Detector { 19 | const HALF_WIDTH: f64 = 0.300; 20 | 21 | /// Creates a new Detector instance. 22 | pub fn image_plane(&self) -> LineF64 { 23 | let p0 = VecF64::<2>::new(self.x, -Self::HALF_WIDTH); 24 | let p1 = VecF64::<2>::new(self.x, Self::HALF_WIDTH); 25 | 26 | LineF64::from_point_pair(p0, p1) 27 | } 28 | } 29 | 30 | impl Element for Detector { 31 | fn to_renderable3(&self) -> SceneRenderable { 32 | let p0 = VecF64::<3>::new(self.x, -Self::HALF_WIDTH, 0.0); 33 | let p1 = VecF64::<3>::new(self.x, Self::HALF_WIDTH, 0.0); 34 | 35 | make_line3( 36 | "image-plane", 37 | &[[p0.cast(), p1.cast()]], 38 | &Color::blue(), 39 | 1.5, 40 | ) 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /crates/sophus/src/examples/optics_sim/element.rs: -------------------------------------------------------------------------------- 1 | use sophus_renderer::renderables::{ 2 | Color, 3 | SceneRenderable, 4 | }; 5 | 6 | /// A trait for an optical element. 7 | pub trait Element { 8 | /// Converts the element to a renderable 3D object. 9 | fn to_renderable3(&self) -> SceneRenderable; 10 | } 11 | 12 | /// Creates a gray color with RGB values of 0.3. 13 | pub fn gray_color() -> Color { 14 | Color { 15 | r: 0.3, 16 | g: 0.3, 17 | b: 0.3, 18 | a: 1.0, 19 | } 20 | } 21 | -------------------------------------------------------------------------------- /crates/sophus/src/examples/optics_sim/scene_point.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::VecF64; 2 | use sophus_renderer::renderables::{ 3 | Color, 4 | SceneRenderable, 5 | make_point3, 6 | }; 7 | 8 | use crate::examples::optics_sim::{ 9 | element::Element, 10 | light_path::unproj2, 11 | }; 12 | 13 | /// A struct representing a pair of points in 2D space. 14 | pub struct ScenePoints { 15 | /// Point pair. 16 | pub p: [VecF64<2>; 2], 17 | } 18 | 19 | impl Element for ScenePoints { 20 | fn to_renderable3(&self) -> SceneRenderable { 21 | make_point3( 22 | "points", 23 | &[unproj2(self.p[0].cast()), unproj2(self.p[1].cast())], 24 | &Color::blue(), 25 | 5.0, 26 | ) 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /crates/sophus/src/examples/viewer_example.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | linalg::{ 3 | SVec, 4 | VecF64, 5 | }, 6 | prelude::IsVector, 7 | }; 8 | use sophus_image::{ 9 | ArcImage4U8, 10 | ImageSize, 11 | MutImage4U8, 12 | prelude::*, 13 | }; 14 | use sophus_renderer::{ 15 | camera::RenderCameraProperties, 16 | renderables::ImageFrame, 17 | }; 18 | use sophus_sensor::DynCameraF64; 19 | 20 | /// Makes example image of image-size 21 | pub fn make_example_image(image_size: ImageSize) -> ArcImage4U8 { 22 | let mut img = 23 | MutImage4U8::from_image_size_and_val(image_size, SVec::::new(255, 255, 255, 255)); 24 | 25 | let w = image_size.width; 26 | let h = image_size.height; 27 | 28 | for i in 0..10 { 29 | for j in 0..10 { 30 | img.mut_pixel(i, j).copy_from_slice(&[0, 0, 0, 255]); 31 | img.mut_pixel(i, h - j - 1) 32 | .copy_from_slice(&[255, 255, 255, 255]); 33 | img.mut_pixel(w - i - 1, h - j - 1) 34 | .copy_from_slice(&[0, 0, 255, 255]); 35 | } 36 | } 37 | img.to_shared() 38 | } 39 | 40 | /// Creates a distorted image frame with a red and blue grid 41 | pub fn make_distorted_frame() -> ImageFrame { 42 | let focal_length = 500.0; 43 | 44 | let image_size = ImageSize::new(638, 479); 45 | let cx = 320.0; 46 | let cy = 240.0; 47 | 48 | let unified_cam = DynCameraF64::new_enhanced_unified( 49 | VecF64::from_array([focal_length, focal_length, cx, cy, 0.629, 1.22]), 50 | image_size, 51 | ); 52 | 53 | let mut img = 54 | MutImage4U8::from_image_size_and_val(image_size, SVec::::new(255, 255, 255, 255)); 55 | 56 | for v in 0..image_size.height { 57 | for u in 0..image_size.width { 58 | let uv = VecF64::<2>::new(u as f64, v as f64); 59 | let p_on_z1 = unified_cam.cam_unproj(uv); 60 | 61 | if p_on_z1[0].abs() < 0.5 { 62 | *img.mut_pixel(u, v) = SVec::::new(255, 0, 0, 255); 63 | 64 | if p_on_z1[1].abs() < 0.3 { 65 | *img.mut_pixel(u, v) = SVec::::new(0, 0, 255, 255); 66 | } 67 | } 68 | } 69 | } 70 | 71 | ImageFrame { 72 | image: Some(img.to_shared()), 73 | camera_properties: RenderCameraProperties::from_intrinsics(&unified_cam), 74 | } 75 | } 76 | -------------------------------------------------------------------------------- /crates/sophus/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![cfg_attr(feature = "simd", feature(portable_simd))] 2 | #![allow(clippy::needless_range_loop)] 3 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 4 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 5 | #![deny(missing_docs)] 6 | 7 | #[doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 8 | #[cfg(doctest)] 9 | pub struct ReadmeDoctests; 10 | 11 | #[doc(inline)] 12 | pub use sophus_autodiff as autodiff; 13 | #[doc(inline)] 14 | pub use sophus_geo as geo; 15 | #[doc(inline)] 16 | pub use sophus_image as image; 17 | #[doc(inline)] 18 | pub use sophus_lie as lie; 19 | #[doc(inline)] 20 | pub use sophus_opt as opt; 21 | #[doc(inline)] 22 | pub use sophus_renderer as renderer; 23 | #[doc(inline)] 24 | pub use sophus_sensor as sensor; 25 | #[doc(inline)] 26 | pub use sophus_sim as sim; 27 | #[doc(inline)] 28 | pub use sophus_spline as spline; 29 | #[doc(inline)] 30 | pub use sophus_tensor as tensor; 31 | #[doc(inline)] 32 | pub use sophus_timeseries as timeseries; 33 | #[doc(inline)] 34 | pub use sophus_viewer as viewer; 35 | 36 | /// Non-comprehensive list of examples. 37 | /// 38 | /// This is *not a comprehensive list* of examples for the *sophus umbrella crate*. 39 | /// The current focus is on the sophus render and camera simulation modules. Most 40 | /// usage examples are found in the individual sub-crates. In particular, the 41 | /// unit tests in each sub-crate are a good source of examples. 42 | pub mod examples; 43 | pub use eframe; 44 | pub use nalgebra; 45 | pub use ndarray; 46 | pub use thingbuf; 47 | 48 | /// sophus prelude. 49 | /// 50 | /// It is recommended to import this prelude when working with `sophus` types: 51 | /// 52 | /// ``` 53 | /// use sophus::prelude::*; 54 | /// ``` 55 | pub mod prelude { 56 | pub use crate::{ 57 | autodiff::prelude::*, 58 | image::prelude::*, 59 | lie::prelude::*, 60 | opt::prelude::*, 61 | }; 62 | } 63 | -------------------------------------------------------------------------------- /crates/sophus_autodiff/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "automatic differentiation with optional SIMD acceleration" 3 | name = "sophus_autodiff" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | approx.workspace = true 15 | log.workspace = true 16 | nalgebra.workspace = true 17 | ndarray.workspace = true 18 | num-traits.workspace = true 19 | snafu.workspace = true 20 | typenum.workspace = true 21 | 22 | sleef = {version = "0.3", optional = true} 23 | 24 | [build-dependencies] 25 | rustc_version.workspace = true 26 | 27 | [features] 28 | simd = ["sleef"] 29 | std = [] 30 | default = ["std"] 31 | -------------------------------------------------------------------------------- /crates/sophus_autodiff/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_autodiff/src/dual.rs: -------------------------------------------------------------------------------- 1 | #[cfg(feature = "simd")] 2 | mod dual_batch_matrix; 3 | #[cfg(feature = "simd")] 4 | mod dual_batch_scalar; 5 | #[cfg(feature = "simd")] 6 | mod dual_batch_vector; 7 | mod dual_matrix; 8 | mod dual_scalar; 9 | mod dual_vector; 10 | mod matrix; 11 | mod scalar; 12 | mod vector; 13 | 14 | #[cfg(feature = "simd")] 15 | pub use crate::dual::dual_batch_matrix::DualBatchMatrix; 16 | #[cfg(feature = "simd")] 17 | pub use crate::dual::dual_batch_scalar::DualBatchScalar; 18 | #[cfg(feature = "simd")] 19 | pub use crate::dual::dual_batch_vector::DualBatchVector; 20 | pub use crate::dual::{ 21 | dual_matrix::DualMatrix, 22 | dual_scalar::DualScalar, 23 | dual_vector::DualVector, 24 | matrix::*, 25 | scalar::*, 26 | vector::*, 27 | }; 28 | -------------------------------------------------------------------------------- /crates/sophus_autodiff/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![cfg_attr(feature = "simd", feature(portable_simd))] 2 | #![deny(missing_docs)] 3 | #![no_std] 4 | #![allow(clippy::needless_range_loop)] 5 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 6 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 7 | 8 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 9 | #[cfg(doctest)] 10 | pub struct ReadmeDoctests; 11 | 12 | #[cfg(feature = "std")] 13 | extern crate std; 14 | 15 | /// Dual numbers – for automatic differentiation. 16 | /// 17 | /// This module provides forward-mode AD through dual number types, enabling derivative 18 | /// computations for scalars, vectors, and matrices (including batch/`simd` forms). 19 | pub mod dual; 20 | /// Traits for core linear algebra types. 21 | /// 22 | /// Defines abstractions for scalars, vectors, and matrices, along with optional batch/SIMD support. 23 | pub mod linalg; 24 | /// Traits for manifolds. 25 | /// 26 | /// A manifold generalizes vector spaces to curved settings. This module offers interfaces for 27 | /// manifold-based computations, tangent spaces, and related logic. 28 | pub mod manifold; 29 | /// Numerical differentiation on curves, scalar-valued, vector-valued, and matrix-valued maps. 30 | /// 31 | /// Provides finite-difference utilities for computing derivatives of user-defined functions. 32 | pub mod maps; 33 | /// Parameter traits. 34 | /// 35 | /// Provides a uniform interfaces for types which state is internally represented by parameter 36 | /// vectors. 37 | pub mod params; 38 | /// Point traits. 39 | /// 40 | /// Defines interfaces for points in various dimensions, including bounds and clamping. 41 | pub mod points; 42 | 43 | /// sophus_geo prelude. 44 | /// 45 | /// It is recommended to import this prelude when working with `sophus_autodiff` types: 46 | /// 47 | /// ``` 48 | /// use sophus_autodiff::prelude::*; 49 | /// ``` 50 | /// 51 | /// or 52 | /// 53 | /// ```ignore 54 | /// use sophus::prelude::*; 55 | /// ``` 56 | /// 57 | /// to import all preludes when using the `sophus` umbrella crate. 58 | pub mod prelude { 59 | pub use crate::{ 60 | dual::{ 61 | HasJacobian, 62 | IsDualMatrix, 63 | IsDualMatrixFromCurve, 64 | IsDualScalar, 65 | IsDualScalarFromCurve, 66 | IsDualVector, 67 | IsDualVectorFromCurve, 68 | }, 69 | linalg::{ 70 | IsBoolMask, 71 | IsCoreScalar, 72 | IsMatrix, 73 | IsRealMatrix, 74 | IsRealScalar, 75 | IsRealVector, 76 | IsScalar, 77 | IsSingleMatrix, 78 | IsSingleScalar, 79 | IsSingleVector, 80 | IsVector, 81 | }, 82 | manifold::{ 83 | IsManifold, 84 | IsTangent, 85 | IsVariable, 86 | }, 87 | params::{ 88 | HasParams, 89 | IsParamsImpl, 90 | }, 91 | points::{ 92 | IsPoint, 93 | IsUnboundedPoint, 94 | }, 95 | }; 96 | } 97 | 98 | pub use nalgebra; 99 | -------------------------------------------------------------------------------- /crates/sophus_autodiff/src/linalg/batch_mask.rs: -------------------------------------------------------------------------------- 1 | use core::{ 2 | fmt, 3 | fmt::Debug, 4 | simd::{ 5 | LaneCount, 6 | Mask, 7 | SupportedLaneCount, 8 | }, 9 | }; 10 | 11 | use crate::prelude::IsBoolMask; 12 | 13 | /// A lane-wise boolean mask for batch (SIMD) operations. 14 | /// 15 | /// This struct wraps a [`Mask`][core::simd::Mask], storing a boolean value 16 | /// for each of `N` lanes. It implements [`IsBoolMask`] for: 17 | /// 18 | /// - Checking whether all or any lanes are `true`. 19 | /// - Counting the number of `true` lanes. 20 | /// - Creating a fully `true` or fully `false` mask. 21 | /// 22 | /// # Generic Parameters 23 | /// - `N`: The number of lanes in the SIMD mask. Must implement [`SupportedLaneCount`]. 24 | /// 25 | /// # Feature 26 | /// This type is only available when the `"simd"` feature is enabled. 27 | #[cfg(feature = "simd")] 28 | pub struct BatchMask 29 | where 30 | LaneCount: SupportedLaneCount, 31 | { 32 | // The underlying lane-wise mask storing `true` or `false` for each lane. 33 | pub(crate) inner: Mask, 34 | } 35 | 36 | impl Debug for BatchMask 37 | where 38 | LaneCount: SupportedLaneCount, 39 | { 40 | fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { 41 | // Displays the mask in a user-friendly way. 42 | write!(f, "BatchMask({:?})", self.inner) 43 | } 44 | } 45 | 46 | impl IsBoolMask for BatchMask 47 | where 48 | LaneCount: SupportedLaneCount, 49 | { 50 | fn all_true() -> Self { 51 | BatchMask { 52 | inner: Mask::from_array([true; BATCH]), 53 | } 54 | } 55 | 56 | fn all_false() -> Self { 57 | BatchMask { 58 | inner: Mask::from_array([false; BATCH]), 59 | } 60 | } 61 | 62 | fn all(&self) -> bool { 63 | Mask::all(self.inner) 64 | } 65 | 66 | fn any(&self) -> bool { 67 | Mask::any(self.inner) 68 | } 69 | 70 | fn count(&self) -> usize { 71 | self.inner.to_array().iter().filter(|x| **x).count() 72 | } 73 | 74 | fn lanes(&self) -> usize { 75 | BATCH 76 | } 77 | } 78 | -------------------------------------------------------------------------------- /crates/sophus_autodiff/src/linalg/bool_mask.rs: -------------------------------------------------------------------------------- 1 | use core::fmt::Debug; 2 | 3 | /// A generic trait for boolean masks, supporting both single-lane (regular `bool`) and 4 | /// multi-lane (SIMD) usage. 5 | /// 6 | /// A "mask" in this context indicates which elements (or "lanes") of a batch/array 7 | /// are active (`true`) vs. inactive (`false`). This trait provides methods to: 8 | /// 9 | /// - Create fully true/fully false masks. 10 | /// - Check if all or any lanes are true. 11 | /// - Count how many lanes are true. 12 | /// - Get the total number of lanes (usually 1 for a single bool, or more for SIMD). 13 | /// 14 | /// # Implementations 15 | /// - For single-lane booleans, `bool` itself implements `IsBoolMask` trivially. 16 | /// - For SIMD-based multi-lane booleans, see [`BatchMask`](crate::linalg::batch_mask::BatchMask) 17 | /// (when the `"simd"` feature is enabled). 18 | pub trait IsBoolMask: Debug { 19 | /// Creates a mask where all lanes are set to `true`. 20 | fn all_true() -> Self; 21 | 22 | /// Creates a mask where all lanes are set to `false`. 23 | fn all_false() -> Self; 24 | 25 | /// Returns `true` if *all* lanes in the mask are `true`. 26 | fn all(&self) -> bool; 27 | 28 | /// Returns `true` if *any* lane in the mask is `true`. 29 | fn any(&self) -> bool; 30 | 31 | /// Returns the number of lanes that are `true`. 32 | fn count(&self) -> usize; 33 | 34 | /// Returns the total number of lanes in the mask. 35 | fn lanes(&self) -> usize; 36 | } 37 | 38 | // A `bool`-based implementation of [`IsBoolMask`], suitable for single-lane usage. 39 | // 40 | // Since there is only one "lane," the methods reflect simple boolean logic: 41 | // - `all_true()` => `true` 42 | // - `all_false()` => `false` 43 | // - `all()` => returns the boolean itself 44 | // - `any()` => same as `all()` in a single-lane context 45 | // - `count()` => returns `1` if `true`, else `0` 46 | // - `lanes()` => returns `1` 47 | // 48 | // This effectively acts as the trivial, single-lane version of a boolean mask. 49 | impl IsBoolMask for bool { 50 | fn all_true() -> bool { 51 | true 52 | } 53 | 54 | fn all_false() -> bool { 55 | false 56 | } 57 | 58 | fn all(&self) -> bool { 59 | *self 60 | } 61 | 62 | fn any(&self) -> bool { 63 | *self 64 | } 65 | 66 | fn count(&self) -> usize { 67 | if *self { 1 } else { 0 } 68 | } 69 | 70 | fn lanes(&self) -> usize { 71 | 1 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /crates/sophus_autodiff/src/maps.rs: -------------------------------------------------------------------------------- 1 | mod curves; 2 | mod matrix_valued_maps; 3 | mod scalar_valued_maps; 4 | mod vector_valued_maps; 5 | 6 | pub use crate::maps::{ 7 | curves::{ 8 | MatrixValuedCurve, 9 | ScalarValuedCurve, 10 | VectorValuedCurve, 11 | }, 12 | matrix_valued_maps::{ 13 | MatrixValuedMatrixMap, 14 | MatrixValuedVectorMap, 15 | }, 16 | scalar_valued_maps::{ 17 | ScalarValuedMatrixMap, 18 | ScalarValuedVectorMap, 19 | }, 20 | vector_valued_maps::{ 21 | VectorValuedMatrixMap, 22 | VectorValuedVectorMap, 23 | }, 24 | }; 25 | -------------------------------------------------------------------------------- /crates/sophus_geo/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "geometric primitives: rays, hyper-planes, hyper-spheres, axis-aligned bounding boxes" 3 | name = "sophus_geo" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_lie.workspace = true 16 | 17 | approx.workspace = true 18 | 19 | [build-dependencies] 20 | rustc_version.workspace = true 21 | 22 | [features] 23 | simd = ["sophus_lie/simd"] 24 | std = ["sophus_autodiff/std", "sophus_lie/std"] 25 | default = ["std"] 26 | -------------------------------------------------------------------------------- /crates/sophus_geo/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_geo/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![cfg_attr(feature = "simd", feature(portable_simd))] 2 | #![deny(missing_docs)] 3 | #![no_std] 4 | #![allow(clippy::needless_range_loop)] 5 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 6 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 7 | 8 | #[cfg(feature = "std")] 9 | extern crate std; 10 | 11 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 12 | #[cfg(doctest)] 13 | pub struct ReadmeDoctests; 14 | 15 | mod hyperplane; 16 | mod hypersphere; 17 | mod ray; 18 | /// Intervals and box regions. 19 | pub mod region; 20 | mod unit_vector; 21 | 22 | pub use sophus_lie::Quaternion; 23 | 24 | pub use crate::{ 25 | hyperplane::*, 26 | hypersphere::*, 27 | ray::*, 28 | unit_vector::*, 29 | }; 30 | 31 | /// sophus_geo prelude. 32 | /// 33 | /// It is recommended to import this prelude when working with `sophus_geo types: 34 | /// 35 | /// ``` 36 | /// use sophus_geo::prelude::*; 37 | /// ``` 38 | /// 39 | /// or 40 | /// 41 | /// ```ignore 42 | /// use sophus::prelude::*; 43 | /// ``` 44 | /// 45 | /// to import all preludes when using the `sophus` umbrella crate. 46 | pub mod prelude { 47 | pub use sophus_autodiff::prelude::*; 48 | pub use sophus_lie::prelude::*; 49 | 50 | pub use crate::region::{ 51 | IsNonEmptyRegion, 52 | IsRegion, 53 | IsRegionBase, 54 | }; 55 | } 56 | -------------------------------------------------------------------------------- /crates/sophus_geo/src/ray.rs: -------------------------------------------------------------------------------- 1 | use sophus_lie::prelude::{ 2 | IsScalar, 3 | IsSingleScalar, 4 | IsVector, 5 | }; 6 | 7 | use crate::unit_vector::UnitVector; 8 | 9 | /// Ray in `ℝⁿ`. 10 | /// 11 | /// ## Generic parameters: 12 | /// 13 | /// * S 14 | /// - the underlying scalar such as [f64] or [sophus_autodiff::dual::DualScalar]. 15 | /// * DOF 16 | /// - Degrees of freedom of the unit direction vector. 17 | /// * DIM 18 | /// - Dimension of the ray. It holds that DIM == DOF + 1. 19 | /// * BATCH 20 | /// - Batch dimension. If S is [f64] or [sophus_autodiff::dual::DualScalar] then BATCH=1. 21 | /// * DM, DN 22 | /// - DM x DN is the static shape of the Jacobian to be computed if S == DualScalar. If S 23 | /// == f64, then DM==0, DN==0. 24 | #[derive(Clone, Debug)] 25 | pub struct Ray< 26 | S: IsScalar, 27 | const DOF: usize, 28 | const DIM: usize, 29 | const BATCH: usize, 30 | const DM: usize, 31 | const DN: usize, 32 | > { 33 | /// The ray origin. 34 | pub origin: S::Vector, 35 | /// The unit direction vector of the ray. 36 | pub dir: UnitVector, 37 | } 38 | 39 | impl< 40 | S: IsSingleScalar + PartialOrd, 41 | const DOF: usize, 42 | const DIM: usize, 43 | const DM: usize, 44 | const DN: usize, 45 | > Ray 46 | { 47 | /// Returns point on ray. 48 | /// 49 | /// For a given t, it returns "origin + t * dir". 50 | pub fn at(&self, t: S) -> S::Vector { 51 | self.origin + self.dir.vector().scaled(t) 52 | } 53 | } 54 | 55 | /// 2d ray. 56 | pub type Ray2 = Ray; 57 | /// 3d ray. 58 | pub type Ray3 = Ray; 59 | -------------------------------------------------------------------------------- /crates/sophus_image/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "static and dynamic image types" 3 | name = "sophus_image" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_tensor.workspace = true 16 | 17 | approx.workspace = true 18 | bytemuck.workspace = true 19 | nalgebra.workspace = true 20 | ndarray.workspace = true 21 | num-traits.workspace = true 22 | tiff = {version = "0.9.0", optional = true} 23 | png = {version ="0.17", optional = true} 24 | 25 | [build-dependencies] 26 | rustc_version.workspace = true 27 | 28 | [features] 29 | simd = ["sophus_tensor/simd"] 30 | std = ["png", "tiff", "sophus_autodiff/std", "sophus_tensor/std"] 31 | default = ["std"] 32 | -------------------------------------------------------------------------------- /crates/sophus_image/README.md: -------------------------------------------------------------------------------- 1 | ## Static and dynamic image types 2 | 3 | This crates contains a number of statically and dynamically typed images. 4 | Internally, images are represented as ndarray's of scalars such as [f32] for 5 | single channel images or nalgebra vectors for multi-channel images. 6 | 7 | 8 | ## Integration with sophus-rs 9 | 10 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 11 | It re-exports the relevant prelude types under [prelude], so you can 12 | seamlessly interoperate with the rest of the sophus-rs types. 13 | -------------------------------------------------------------------------------- /crates/sophus_image/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_image/src/color_map.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::SVec; 2 | 3 | /// blue to white to red to black 4 | pub struct BlueWhiteRedBlackColorMap; 5 | 6 | impl BlueWhiteRedBlackColorMap { 7 | /// f32 to rgb 8 | pub fn f32_to_rgb(depth: f32) -> SVec { 9 | let depth = depth.clamp(0.0, 1.0); 10 | 11 | let (r, g, b) = if depth < 0.33 { 12 | // Transition from blue to white 13 | let t = depth / 0.33; 14 | (255.0 * t, 255.0 * t, 255.0) 15 | } else if depth < 0.66 { 16 | // Transition from white to red 17 | let t = (depth - 0.33) / 0.33; 18 | ( 19 | 255.0, // Red stays at 255 20 | 255.0 * (1.0 - t), // Green decreases to 0 21 | 255.0 * (1.0 - t), // Blue stays at 0 22 | ) 23 | } else { 24 | // Transition from red to black 25 | let t = (depth - 0.66) / 0.34; 26 | ( 27 | 255.0 * (1.0 - t), // Red decreases to 0 28 | 0.0, // Green stays at 0 29 | 0.0, // Blue stays at 0 30 | ) 31 | }; 32 | 33 | SVec::::new(r as u8, g as u8, b as u8) 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /crates/sophus_image/src/intensity_image.rs: -------------------------------------------------------------------------------- 1 | pub(crate) mod dyn_intensity_image; 2 | pub(crate) mod intensity_arc_image; 3 | #[cfg(feature = "std")] 4 | pub(crate) mod intensity_image_view; 5 | pub(crate) mod intensity_mut_image; 6 | pub(crate) mod intensity_pixel; 7 | pub(crate) mod intensity_scalar; 8 | -------------------------------------------------------------------------------- /crates/sophus_image/src/interpolation.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::SVec; 2 | 3 | use crate::prelude::*; 4 | 5 | /// Bilinear interpolated single-channel f32 image lookup 6 | pub fn interpolate_f32<'a, I: IsImageView<'a, 2, 0, f32, f32, 1, 1>>( 7 | img: &'a I, 8 | uv: SVec, 9 | ) -> f32 { 10 | let image_size = img.image_size(); 11 | 12 | let iu = uv[0].trunc() as usize; 13 | let iv = uv[1].trunc() as usize; 14 | let frac_u: f32 = uv[0].fract(); 15 | let frac_v: f32 = uv[1].fract(); 16 | let u = iu; 17 | let v = iv; 18 | 19 | let u_corner_case = u == image_size.width - 1; 20 | let v_corner_case = v == image_size.height - 1; 21 | 22 | let val00: f32 = img.pixel(u, v); 23 | let val01: f32 = if v_corner_case { 24 | val00 25 | } else { 26 | img.pixel(u, v + 1) 27 | }; 28 | let val10: f32 = if u_corner_case { 29 | val00 30 | } else { 31 | img.pixel(u + 1, v) 32 | }; 33 | let val11: f32 = if u_corner_case || v_corner_case { 34 | val00 35 | } else { 36 | img.pixel(u + 1, v + 1) 37 | }; 38 | 39 | val00 * (1.0 - frac_u) * (1.0 - frac_v) 40 | + val01 * ((1.0 - frac_u) * frac_v) 41 | + val10 * (frac_u * (1.0 - frac_v)) 42 | + val11 * (frac_u * frac_v) 43 | } 44 | 45 | /// Bilinear interpolated multi-channel f32 image lookup 46 | pub fn interpolate_xf32< 47 | 'a, 48 | const ROWS: usize, 49 | I: IsImageView<'a, 3, 1, f32, SVec, ROWS, 1>, 50 | >( 51 | img: &'a I, 52 | uv: SVec, 53 | ) -> SVec { 54 | let image_size = img.image_size(); 55 | 56 | let iu = uv[0].trunc() as usize; 57 | let iv = uv[1].trunc() as usize; 58 | let frac_u: f32 = uv[0].fract(); 59 | let frac_v: f32 = uv[1].fract(); 60 | let u = iu; 61 | let v = iv; 62 | 63 | let u_corner_case = u == image_size.width - 1; 64 | let v_corner_case = v == image_size.height - 1; 65 | 66 | let val00: SVec = img.pixel(u, v); 67 | let val01: SVec = if v_corner_case { 68 | val00 69 | } else { 70 | img.pixel(u, v + 1) 71 | }; 72 | let val10: SVec = if u_corner_case { 73 | val00 74 | } else { 75 | img.pixel(u + 1, v) 76 | }; 77 | let val11: SVec = if u_corner_case || v_corner_case { 78 | val00 79 | } else { 80 | img.pixel(u + 1, v + 1) 81 | }; 82 | 83 | val00 * (1.0 - frac_u) * (1.0 - frac_v) 84 | + val01 * ((1.0 - frac_u) * frac_v) 85 | + val10 * (frac_u * (1.0 - frac_v)) 86 | + val11 * (frac_u * frac_v) 87 | } 88 | -------------------------------------------------------------------------------- /crates/sophus_image/src/io.rs: -------------------------------------------------------------------------------- 1 | #[cfg(feature = "std")] 2 | mod png; 3 | #[cfg(feature = "std")] 4 | mod tiff; 5 | 6 | pub use png::*; 7 | pub use tiff::*; 8 | -------------------------------------------------------------------------------- /crates/sophus_lie/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Lie groups in 2D and 3D: rotations, translations, etc." 3 | name = "sophus_lie" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff = {workspace = true} 15 | 16 | approx.workspace = true 17 | log.workspace = true 18 | nalgebra.workspace = true 19 | num-traits.workspace = true 20 | rand.workspace = true 21 | snafu.workspace = true 22 | 23 | [build-dependencies] 24 | rustc_version.workspace = true 25 | 26 | [features] 27 | simd = ["sophus_autodiff/simd"] 28 | std = ["sophus_autodiff/std"] 29 | default = ["std"] 30 | -------------------------------------------------------------------------------- /crates/sophus_lie/README.md: -------------------------------------------------------------------------------- 1 | ## Lie groups in 2D and 3D: rotations, translations, etc. 2 | 3 | Lie groups crate - part of the sophus-rs project 4 | 5 | ## Integration with sophus-rs 6 | 7 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 8 | It re-exports the relevant prelude types under [prelude], so you can 9 | seamlessly interoperate with the rest of the sophus-rs types. 10 | -------------------------------------------------------------------------------- /crates/sophus_lie/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_lie/src/groups.rs: -------------------------------------------------------------------------------- 1 | pub(crate) mod affine_group_template; 2 | pub(crate) mod galilean3; 3 | pub(crate) mod isometry2; 4 | pub(crate) mod isometry3; 5 | pub(crate) mod rotation2; 6 | pub(crate) mod rotation3; 7 | pub(crate) mod rotation_boost3; 8 | -------------------------------------------------------------------------------- /crates/sophus_lie/src/groups/galilean3.rs: -------------------------------------------------------------------------------- 1 | use super::affine_group_template::AffineGroupTemplateImpl; 2 | use crate::{ 3 | groups::rotation_boost3::RotationBoost3Impl, 4 | lie_group::LieGroup, 5 | }; 6 | 7 | /// 3d **Galilean transformations** – element of the Galilean group **Gal(3)** 8 | pub type Galilean3 = 9 | LieGroup>; 10 | 11 | /// 3d Galilean transformations with f64 scalar type - element of the Galilean group **Gal(3)** 12 | /// 13 | /// See [Galilean3] for details. 14 | pub type Galilei3F64 = Galilean3; 15 | 16 | /// 3d Galilean transformations implementation details 17 | /// 18 | /// See [Galilean3] for details. 19 | pub type Galilei3Impl = 20 | AffineGroupTemplateImpl< 21 | S, 22 | 10, 23 | 11, 24 | 4, 25 | 5, 26 | 6, 27 | 7, 28 | BATCH, 29 | DM, 30 | DN, 31 | RotationBoost3Impl, 32 | >; 33 | 34 | #[test] 35 | fn galilei3_rop_tests() { 36 | #[cfg(feature = "simd")] 37 | use sophus_autodiff::dual::DualBatchScalar; 38 | use sophus_autodiff::dual::DualScalar; 39 | #[cfg(feature = "simd")] 40 | use sophus_autodiff::linalg::BatchScalarF64; 41 | 42 | Galilei3F64::test_suite(); 43 | #[cfg(feature = "simd")] 44 | Galilean3::, 8, 0, 0>::test_suite(); 45 | Galilean3::, 1, 1, 1>::test_suite(); 46 | #[cfg(feature = "simd")] 47 | Galilean3::, 8, 1, 1>::test_suite(); 48 | } 49 | -------------------------------------------------------------------------------- /crates/sophus_lie/src/lie_group/group_mul.rs: -------------------------------------------------------------------------------- 1 | use core::ops::Mul; 2 | 3 | use sophus_autodiff::prelude::IsScalar; 4 | 5 | use crate::{ 6 | IsLieGroupImpl, 7 | lie_group::LieGroup, 8 | }; 9 | 10 | // a * b 11 | impl< 12 | S: IsScalar, 13 | const DOF: usize, 14 | const PARAMS: usize, 15 | const POINT: usize, 16 | const AMBIENT: usize, 17 | const BATCH: usize, 18 | const DM: usize, 19 | const DN: usize, 20 | G: IsLieGroupImpl, 21 | > Mul> 22 | for LieGroup 23 | { 24 | type Output = LieGroup; 25 | 26 | fn mul(self, rhs: Self) -> Self::Output { 27 | self.group_mul(rhs) 28 | } 29 | } 30 | 31 | // a * &b 32 | impl< 33 | S: IsScalar, 34 | const DOF: usize, 35 | const PARAMS: usize, 36 | const POINT: usize, 37 | const AMBIENT: usize, 38 | const BATCH: usize, 39 | const DM: usize, 40 | const DN: usize, 41 | G: IsLieGroupImpl, 42 | > Mul<&LieGroup> 43 | for LieGroup 44 | { 45 | type Output = LieGroup; 46 | 47 | fn mul(self, rhs: &Self) -> Self::Output { 48 | self.group_mul(*rhs) 49 | } 50 | } 51 | 52 | // &a * &b 53 | impl< 54 | S: IsScalar, 55 | const DOF: usize, 56 | const PARAMS: usize, 57 | const POINT: usize, 58 | const AMBIENT: usize, 59 | const BATCH: usize, 60 | const DM: usize, 61 | const DN: usize, 62 | G: IsLieGroupImpl, 63 | > Mul> 64 | for &LieGroup 65 | { 66 | type Output = LieGroup; 67 | 68 | fn mul(self, rhs: LieGroup) -> Self::Output { 69 | self.group_mul(rhs) 70 | } 71 | } 72 | 73 | // a * &b 74 | impl< 75 | S: IsScalar, 76 | const DOF: usize, 77 | const PARAMS: usize, 78 | const POINT: usize, 79 | const AMBIENT: usize, 80 | const BATCH: usize, 81 | const DM: usize, 82 | const DN: usize, 83 | G: IsLieGroupImpl, 84 | > Mul<&LieGroup> 85 | for &LieGroup 86 | { 87 | type Output = LieGroup; 88 | 89 | fn mul(self, rhs: &LieGroup) -> Self::Output { 90 | self.group_mul(*rhs) 91 | } 92 | } 93 | -------------------------------------------------------------------------------- /crates/sophus_lie/src/lie_group/lie_group_manifold.rs: -------------------------------------------------------------------------------- 1 | use core::fmt::Debug; 2 | 3 | use sophus_autodiff::{ 4 | linalg::VecF64, 5 | manifold::IsVariable, 6 | }; 7 | 8 | use crate::{ 9 | IsLieGroupImpl, 10 | lie_group::LieGroup, 11 | prelude::*, 12 | }; 13 | 14 | extern crate alloc; 15 | 16 | // Lie group as Left group manifold 17 | // 18 | // A ⊕ t := exp(t) * A 19 | // A ⊖ B := log(inv(A) * B) 20 | impl< 21 | S: IsScalar, 22 | const DOF: usize, 23 | const PARAMS: usize, 24 | const POINT: usize, 25 | const AMBIENT: usize, 26 | const BATCH: usize, 27 | const DM: usize, 28 | const DN: usize, 29 | G: IsLieGroupImpl + Clone + Debug, 30 | > IsManifold 31 | for LieGroup 32 | { 33 | fn oplus(&self, tangent: &>::Vector) -> Self { 34 | LieGroup::::exp(*tangent) * self 35 | } 36 | 37 | fn ominus(&self, rhs: &Self) -> >::Vector { 38 | (self.inverse() * rhs).log() 39 | } 40 | } 41 | 42 | impl< 43 | const DOF: usize, 44 | const PARAMS: usize, 45 | const POINT: usize, 46 | const AMBIENT: usize, 47 | G: IsLieGroupImpl 48 | + Clone 49 | + Debug 50 | + Send 51 | + Sync 52 | + 'static, 53 | > IsVariable for LieGroup 54 | { 55 | const NUM_DOF: usize = DOF; 56 | 57 | fn update(&mut self, delta: nalgebra::DVectorView) { 58 | assert_eq!(delta.len(), DOF); 59 | let mut tangent = VecF64::::zeros(); 60 | for d in 0..DOF { 61 | tangent[d] = delta[d]; 62 | } 63 | *self = self.oplus(&tangent); 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /crates/sophus_lie/src/traits.rs: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sophus-vision/sophus-rs/d5ce8fb72e2add680f50db48f7dc1af6782e94fd/crates/sophus_lie/src/traits.rs -------------------------------------------------------------------------------- /crates/sophus_opt/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Sparse non-linear least squares optimization" 3 | name = "sophus_opt" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_image.workspace = true 16 | sophus_lie.workspace = true 17 | sophus_sensor.workspace = true 18 | 19 | approx.workspace = true 20 | as-any.workspace = true 21 | dyn-clone.workspace = true 22 | faer.workspace = true 23 | log.workspace = true 24 | nalgebra.workspace = true 25 | ndarray.workspace = true 26 | rand.workspace = true 27 | rayon = "1.10" 28 | snafu.workspace = true 29 | 30 | [build-dependencies] 31 | rustc_version.workspace = true 32 | 33 | [features] 34 | simd = [ 35 | "sophus_autodiff/simd", 36 | "sophus_image/simd", 37 | "sophus_lie/simd", 38 | "sophus_sensor/simd", 39 | ] 40 | std = [ 41 | "sophus_autodiff/std", 42 | "sophus_image/std", 43 | "sophus_lie/std", 44 | "sophus_sensor/std", 45 | ] 46 | default = ["std"] 47 | -------------------------------------------------------------------------------- /crates/sophus_opt/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/asserts.rs: -------------------------------------------------------------------------------- 1 | /// assert that left is less than or equal to right 2 | #[macro_export] 3 | macro_rules! assert_le { 4 | ($left:expr, $right:expr $(,)?) => { 5 | if !($left <= $right) { 6 | panic!( 7 | "assertion failed: `(left <= right)`\n left: `{:?}`, right: `{:?}`", 8 | $left, $right 9 | ); 10 | } 11 | }; 12 | ($left:expr, $right:expr, $($arg:tt)+) => { 13 | if !($left <= $right) { 14 | panic!( 15 | "assertion failed: `(left <= right)`\n left: `{:?}`, right: `{:?}`\n{}", 16 | $left, $right, format_args!($($arg)+) 17 | ); 18 | } 19 | }; 20 | } 21 | 22 | /// assert that left is greater than or equal to right 23 | #[macro_export] 24 | macro_rules! assert_ge { 25 | ($left:expr, $right:expr $(,)?) => { 26 | if !($left >= $right) { 27 | panic!( 28 | "assertion failed: `(left >= right)`\n left: `{:?}`, right: `{:?}`", 29 | $left, $right 30 | ); 31 | } 32 | }; 33 | ($left:expr, $right:expr, $($arg:tt)+) => { 34 | if !($left >= $right) { 35 | panic!( 36 | "assertion failed: `(left >= right)`\n left: `{:?}`, right: `{:?}`\n{}", 37 | $left, $right, format_args!($($arg)+) 38 | ); 39 | } 40 | }; 41 | } 42 | 43 | /// assert that left is less than or equal to right 44 | #[macro_export] 45 | macro_rules! debug_assert_le { 46 | ($($args:tt)*) => { 47 | if cfg!(debug_assertions) { 48 | use $crate::assert_le; 49 | assert_le!($($args)*); 50 | } 51 | }; 52 | } 53 | 54 | /// assert that left is greater than or equal to right 55 | #[macro_export] 56 | macro_rules! debug_assert_ge { 57 | ($($args:tt)*) => { 58 | if cfg!(debug_assertions) { 59 | use $crate::assert_ge; 60 | assert_ge!($($args)*); 61 | } 62 | }; 63 | } 64 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/block.rs: -------------------------------------------------------------------------------- 1 | pub(crate) mod block_gradient; 2 | pub(crate) mod block_hessian; 3 | pub(crate) mod block_jacobian; 4 | pub(crate) mod block_sparse_matrix_builder; 5 | pub(crate) mod block_vector; 6 | pub(crate) mod grid; 7 | pub(crate) mod symmetric_block_sparse_matrix_builder; 8 | 9 | pub use block_gradient::*; 10 | pub use block_hessian::*; 11 | pub use block_jacobian::*; 12 | pub use block_sparse_matrix_builder::*; 13 | pub use block_vector::*; 14 | pub use grid::*; 15 | pub use symmetric_block_sparse_matrix_builder::*; 16 | 17 | /// Range of a block 18 | #[derive(Clone, Debug, Copy, Default)] 19 | pub struct BlockRange { 20 | /// Index of the first element of the block 21 | pub index: i64, 22 | /// Dimension of the block 23 | pub dim: usize, 24 | } 25 | 26 | /// Additional region 27 | #[derive(Debug, Clone)] 28 | pub struct PartitionSpec { 29 | /// num blocks 30 | pub num_blocks: usize, 31 | /// block dim 32 | pub block_dim: usize, 33 | } 34 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/block/block_gradient.rs: -------------------------------------------------------------------------------- 1 | use nalgebra::Const; 2 | use sophus_autodiff::linalg::VecF64; 3 | 4 | use super::BlockRange; 5 | 6 | /// Block gradient vector 7 | /// 8 | /// ```ascii 9 | /// ----------- 10 | /// | | 11 | /// | g_0 | 12 | /// | | 13 | /// |---------| 14 | /// | . | 15 | /// | . | 16 | /// | . | 17 | /// |---------| 18 | /// | | 19 | /// | g_{N-1} | 20 | /// | | 21 | /// ----------- 22 | /// ``` 23 | /// 24 | /// The `INPUT_DIM`-dimensional vector is partitioned into `N` blocks, each of 25 | /// which has an offset and dimension specified by the `ranges` array. 26 | /// 27 | /// Hence, the gradient sub-block `g_i` has a dimensionality of `ranges(i).dim`. 28 | #[derive(Debug, Clone)] 29 | pub struct BlockGradient { 30 | /// vector storage 31 | pub vec: nalgebra::SVector, 32 | /// ranges, one for each block 33 | pub ranges: [BlockRange; N], 34 | } 35 | 36 | impl BlockGradient { 37 | /// create a new block vector 38 | pub fn new(dims: &[usize; N]) -> Self { 39 | debug_assert!(!dims.is_empty()); 40 | 41 | let num_blocks = dims.len(); 42 | 43 | let mut ranges = [BlockRange::default(); N]; 44 | 45 | let mut num_rows: usize = 0; 46 | 47 | for i in 0..num_blocks { 48 | let dim = dims[i]; 49 | ranges[i] = BlockRange { 50 | index: num_rows as i64, 51 | dim, 52 | }; 53 | num_rows += dim; 54 | } 55 | Self { 56 | vec: nalgebra::SVector::zeros(), 57 | ranges, 58 | } 59 | } 60 | 61 | /// Number of blocks 62 | pub fn num_blocks(&self) -> usize { 63 | self.ranges.len() 64 | } 65 | 66 | /// set the ith block 67 | pub fn set_block(&mut self, ith: usize, v: VecF64) { 68 | debug_assert!(ith < self.num_blocks()); 69 | debug_assert_eq!(R, self.ranges[ith].dim); 70 | self.mut_block::(ith).copy_from(&v); 71 | } 72 | 73 | /// get the ith block 74 | pub fn block( 75 | &self, 76 | ith: usize, 77 | ) -> nalgebra::Matrix< 78 | f64, 79 | nalgebra::Dyn, 80 | nalgebra::Const<1>, 81 | nalgebra::ViewStorage< 82 | '_, 83 | f64, 84 | nalgebra::Dyn, 85 | nalgebra::Const<1>, 86 | nalgebra::Const<1>, 87 | Const, 88 | >, 89 | > { 90 | let idx = self.ranges[ith].index as usize; 91 | self.vec.rows(idx, self.ranges[ith].dim) 92 | } 93 | 94 | /// mutable reference to the ith block 95 | pub fn mut_block( 96 | &mut self, 97 | ith: usize, 98 | ) -> nalgebra::Matrix< 99 | f64, 100 | nalgebra::Const, 101 | nalgebra::Const<1>, 102 | nalgebra::ViewStorageMut< 103 | '_, 104 | f64, 105 | nalgebra::Const, 106 | nalgebra::Const<1>, 107 | nalgebra::Const<1>, 108 | Const, 109 | >, 110 | > { 111 | let idx = self.ranges[ith].index as usize; 112 | self.vec.fixed_rows_mut::(idx) 113 | } 114 | } 115 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/block/block_vector.rs: -------------------------------------------------------------------------------- 1 | use super::PartitionSpec; 2 | 3 | /// A region of a block vector. 4 | #[derive(Debug, Clone)] 5 | struct BlockVectorRegion { 6 | scalar_offset: usize, 7 | block_dim: usize, 8 | } 9 | 10 | /// Block vector. 11 | /// 12 | /// ``ascii 13 | /// ----- 14 | /// | A | 15 | /// | . | 16 | /// | . | 17 | /// | A | 18 | /// ----- 19 | /// | B | 20 | /// | . | 21 | /// | . | 22 | /// | B | 23 | /// ----- 24 | /// | | 25 | /// | * | 26 | /// | * | 27 | /// | | 28 | /// ----- 29 | /// | Z | 30 | /// | . | 31 | /// | . | 32 | /// | Z | 33 | /// ----- 34 | /// 35 | /// It ia split into regions and each region is split into a sequence of sub-vectors. 36 | /// Within each region, all sub-vectors have the same dimension. E.g., region 0 contains 37 | /// only A-dimensional sub-vectors, region 1 contains only B-dimensional sub-vectors, etc. 38 | #[derive(Debug, Clone)] 39 | pub struct BlockVector { 40 | partitions: Vec, 41 | vec: nalgebra::DVector, 42 | } 43 | 44 | impl BlockVector { 45 | /// Create a block vector filled with zeros. 46 | /// 47 | /// The shape of the block vector is determined by the provided partition specs. 48 | pub fn zero(partition_specs: &[PartitionSpec]) -> Self { 49 | let mut scalar_offsets = Vec::new(); 50 | let mut offset = 0; 51 | 52 | let mut partitions: Vec = Vec::with_capacity(partition_specs.len()); 53 | 54 | for partition_specs in partition_specs { 55 | scalar_offsets.push(offset); 56 | partitions.push(BlockVectorRegion { 57 | scalar_offset: offset, 58 | block_dim: partition_specs.block_dim, 59 | }); 60 | offset += partition_specs.block_dim * partition_specs.num_blocks; 61 | } 62 | 63 | Self { 64 | partitions, 65 | vec: nalgebra::DVector::zeros(offset), 66 | } 67 | } 68 | 69 | /// Fill vector with a constant value. 70 | pub fn fill(&mut self, value: f64) { 71 | self.vec.fill(value); 72 | } 73 | 74 | /// Move out of self and return scalar vector representation. 75 | pub fn into_scalar_vector(self) -> nalgebra::DVector { 76 | self.vec 77 | } 78 | 79 | /// Get the scalar vector representation. 80 | pub fn scalar_vector(&self) -> &nalgebra::DVector { 81 | &self.vec 82 | } 83 | 84 | /// Get the scalar vector representation. 85 | pub fn scalar_vector_mut(&mut self) -> &mut nalgebra::DVector { 86 | &mut self.vec 87 | } 88 | 89 | /// Add a block to the vector. 90 | pub fn add_block( 91 | &mut self, 92 | partition_idx: usize, 93 | block_index: usize, 94 | block: &nalgebra::DVectorView, 95 | ) { 96 | let partition = &self.partitions[partition_idx]; 97 | assert_eq!(block.shape().0, partition.block_dim); 98 | let scalar_offset = partition.scalar_offset + block_index * partition.block_dim; 99 | use std::ops::AddAssign; 100 | 101 | self.vec 102 | .rows_mut(scalar_offset, partition.block_dim) 103 | .add_assign(block); 104 | } 105 | 106 | /// Get a block 107 | pub fn get_block( 108 | &self, 109 | partition_idx: usize, 110 | block_index: usize, 111 | ) -> nalgebra::DVectorView { 112 | let partition = &self.partitions[partition_idx]; 113 | let scalar_offset = partition.scalar_offset + block_index * partition.block_dim; 114 | self.vec.rows(scalar_offset, partition.block_dim) 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/block/grid.rs: -------------------------------------------------------------------------------- 1 | /// A 2D grid of cells. 2 | #[derive(Debug)] 3 | pub struct Grid { 4 | cells: Vec, 5 | size: [usize; 2], 6 | } 7 | 8 | impl Grid { 9 | /// Create a new grid with the given size and default cell value. 10 | pub fn new(size: [usize; 2], default: Cell) -> Self { 11 | Self { 12 | cells: vec![default; size[0] * size[1]], 13 | size, 14 | } 15 | } 16 | 17 | /// Get cell at given index. 18 | pub fn get(&self, idx: &[usize; 2]) -> &Cell { 19 | &self.cells[idx[0] * self.size[1] + idx[1]] 20 | } 21 | 22 | /// Get mutable cell at given index. 23 | pub fn get_mut(&mut self, idx: &[usize; 2]) -> &mut Cell { 24 | &mut self.cells[idx[0] * self.size[1] + idx[1]] 25 | } 26 | } 27 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/example_problems.rs: -------------------------------------------------------------------------------- 1 | /// Camera calibration example problem 2 | pub mod cam_calib; 3 | /// linear equality constraint toy example 4 | pub mod linear_eq_toy_problem; 5 | /// non-linear equality constraint toy example 6 | pub mod non_linear_eq_toy_problem; 7 | /// Pose graph example problem 8 | pub mod pose_circle; 9 | /// Simple prior example problem 10 | pub mod simple_prior; 11 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/example_problems/linear_eq_toy_problem.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::VecF64; 2 | use sophus_lie::prelude::IsMatrix; 3 | 4 | use crate::{ 5 | nlls::{ 6 | CostFn, 7 | CostTerms, 8 | EqConstraintFn, 9 | EqConstraints, 10 | LinearSolverType, 11 | OptParams, 12 | costs::Quadratic1CostTerm, 13 | eq_constraints::ExampleLinearEqConstraint, 14 | optimize_nlls_with_eq_constraints, 15 | }, 16 | variables::{ 17 | VarBuilder, 18 | VarFamily, 19 | VarKind, 20 | }, 21 | }; 22 | 23 | extern crate alloc; 24 | 25 | /// Simple linear equality constraint problem 26 | pub struct LinearEqToyProblem {} 27 | 28 | impl Default for LinearEqToyProblem { 29 | fn default() -> Self { 30 | Self::new() 31 | } 32 | } 33 | 34 | impl LinearEqToyProblem { 35 | fn new() -> Self { 36 | Self {} 37 | } 38 | 39 | /// Test the linear equality constraint problem 40 | pub fn test(&self, solver: LinearSolverType) { 41 | use sophus_autodiff::linalg::EPS_F64; 42 | 43 | const VAR_X: &str = "x"; 44 | 45 | const EQ_CONSTRAINT_RHS: f64 = 2.0; 46 | let initial_x0 = VecF64::<1>::from_f64(2.0); 47 | let initial_x1 = VecF64::<1>::from_f64(2.0); 48 | 49 | let variables = VarBuilder::new() 50 | .add_family( 51 | VAR_X, 52 | VarFamily::new(VarKind::Free, alloc::vec![initial_x0, initial_x1]), 53 | ) 54 | .build(); 55 | let cost_terms = CostTerms::new( 56 | [VAR_X], 57 | alloc::vec![ 58 | Quadratic1CostTerm { 59 | z: VecF64::<1>::new(1.0), 60 | entity_indices: [0] 61 | }, 62 | Quadratic1CostTerm { 63 | z: VecF64::<1>::new(2.0), 64 | entity_indices: [1] 65 | } 66 | ], 67 | ); 68 | let eq_constraints = EqConstraints::new( 69 | [VAR_X, VAR_X], 70 | vec![ExampleLinearEqConstraint { 71 | lhs: EQ_CONSTRAINT_RHS, 72 | entity_indices: [0, 1], 73 | }], 74 | ); 75 | 76 | // illustrate that the initial guess is not feasible 77 | assert!( 78 | ExampleLinearEqConstraint::residual(initial_x0, initial_x1, EQ_CONSTRAINT_RHS).norm() 79 | > EPS_F64 80 | ); 81 | 82 | let solution = optimize_nlls_with_eq_constraints( 83 | variables, 84 | alloc::vec![CostFn::new_boxed((), cost_terms.clone(),)], 85 | alloc::vec![EqConstraintFn::new_boxed((), eq_constraints,)], 86 | OptParams { 87 | num_iterations: 10, 88 | initial_lm_damping: EPS_F64, 89 | parallelize: true, 90 | solver, 91 | }, 92 | ) 93 | .unwrap(); 94 | let refined_variables = solution.variables; 95 | let refined_x = refined_variables.get_members::>(VAR_X); 96 | 97 | let x0 = refined_x[0]; 98 | let x1 = refined_x[1]; 99 | approx::assert_abs_diff_eq!(x0[0], 0.5, epsilon = 1e-6); 100 | approx::assert_abs_diff_eq!(x1[0], 1.5, epsilon = 1e-6); 101 | 102 | // converged solution should satisfy the equality constraint 103 | approx::assert_abs_diff_eq!( 104 | ExampleLinearEqConstraint::residual(x0, x1, EQ_CONSTRAINT_RHS)[0], 105 | 0.0, 106 | epsilon = 1e-6 107 | ); 108 | } 109 | } 110 | 111 | #[test] 112 | fn normalize_opt_tests() { 113 | for solver in LinearSolverType::indefinite_solvers() { 114 | LinearEqToyProblem::new().test(solver); 115 | } 116 | } 117 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/example_problems/non_linear_eq_toy_problem.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::VecF64; 2 | 3 | use crate::{ 4 | nlls::{ 5 | CostFn, 6 | CostTerms, 7 | EqConstraintFn, 8 | EqConstraints, 9 | LinearSolverType, 10 | OptParams, 11 | costs::ExampleNonLinearCostTerm, 12 | eq_constraints::ExampleNonLinearEqConstraint, 13 | optimize_nlls_with_eq_constraints, 14 | }, 15 | variables::{ 16 | VarBuilder, 17 | VarFamily, 18 | VarKind, 19 | }, 20 | }; 21 | extern crate alloc; 22 | 23 | /// non-linear == constraint toy problem 24 | pub struct NonLinearEqToyProblem {} 25 | 26 | impl Default for NonLinearEqToyProblem { 27 | fn default() -> Self { 28 | Self::new() 29 | } 30 | } 31 | 32 | impl NonLinearEqToyProblem { 33 | fn new() -> Self { 34 | Self {} 35 | } 36 | 37 | /// Test the non-linear equality constraint problem 38 | pub fn test(&self, solver: LinearSolverType) { 39 | use sophus_autodiff::linalg::EPS_F64; 40 | 41 | const VAR_X: &str = "x"; 42 | const EQ_CONSTRAINT_RHS: f64 = 1.0; 43 | 44 | let initial_x = VecF64::<2>::new(2.0, 1.0); 45 | 46 | let variables = VarBuilder::new() 47 | .add_family(VAR_X, VarFamily::new(VarKind::Free, alloc::vec![initial_x])) 48 | .build(); 49 | let cost_terms = CostTerms::new( 50 | [VAR_X], 51 | alloc::vec![ExampleNonLinearCostTerm { 52 | z: VecF64::<2>::new(1.0, 1.0), 53 | entity_indices: [0] 54 | },], 55 | ); 56 | let eq_constraints = EqConstraints::new( 57 | [VAR_X], 58 | vec![ExampleNonLinearEqConstraint { 59 | lhs: EQ_CONSTRAINT_RHS, 60 | entity_indices: [0], 61 | }], 62 | ); 63 | 64 | // illustrate that the initial guess is not feasible 65 | assert!( 66 | ExampleNonLinearEqConstraint::residual(initial_x, EQ_CONSTRAINT_RHS).norm() > EPS_F64 67 | ); 68 | 69 | let refined_variables = optimize_nlls_with_eq_constraints( 70 | variables, 71 | alloc::vec![CostFn::new_boxed((), cost_terms.clone(),)], 72 | alloc::vec![EqConstraintFn::new_boxed((), eq_constraints,)], 73 | OptParams { 74 | num_iterations: 10, 75 | initial_lm_damping: EPS_F64, 76 | parallelize: true, 77 | solver, 78 | }, 79 | ) 80 | .unwrap() 81 | .variables; 82 | let refined_x = refined_variables.get_members::>(VAR_X)[0]; 83 | 84 | approx::assert_abs_diff_eq!(refined_x, VecF64::<2>::new(0.0, 1.0), epsilon = 1e-6); 85 | 86 | // converged solution should satisfy the equality constraint 87 | approx::assert_abs_diff_eq!( 88 | ExampleNonLinearEqConstraint::residual(refined_x, EQ_CONSTRAINT_RHS)[0], 89 | 0.0, 90 | epsilon = 1e-6 91 | ); 92 | } 93 | } 94 | 95 | #[test] 96 | fn normalize_opt_tests() { 97 | for solver in LinearSolverType::indefinite_solvers() { 98 | NonLinearEqToyProblem::new().test(solver); 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![cfg_attr(feature = "simd", feature(portable_simd))] 2 | #![deny(missing_docs)] 3 | #![allow(clippy::needless_range_loop)] 4 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 5 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 6 | 7 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 8 | #[cfg(doctest)] 9 | pub struct ReadmeDoctests; 10 | 11 | mod asserts; 12 | 13 | /// Block vectors, block matrices and utilities 14 | pub mod block; 15 | /// Example problems 16 | pub mod example_problems; 17 | /// Non-linear least squares optimization 18 | pub mod nlls; 19 | /// Robust kernel functions 20 | pub mod robust_kernel; 21 | /// Decision variables 22 | pub mod variables; 23 | /// sophus_opt prelude. 24 | /// 25 | /// It is recommended to import this prelude when working with `sophus_opt types: 26 | /// 27 | /// ``` 28 | /// use sophus_opt::prelude::*; 29 | /// ``` 30 | /// 31 | /// or 32 | /// 33 | /// ```ignore 34 | /// use sophus::prelude::*; 35 | /// ``` 36 | /// 37 | /// to import all preludes when using the `sophus` umbrella crate. 38 | pub mod prelude { 39 | pub use sophus_autodiff::prelude::*; 40 | pub use sophus_image::prelude::*; 41 | pub use sophus_lie::prelude::*; 42 | pub use sophus_sensor::prelude::*; 43 | 44 | pub use crate::{ 45 | nlls::{ 46 | HasEqConstraintResidualFn, 47 | HasResidualFn, 48 | IsCostFn, 49 | IsEqConstraintsFn, 50 | IsEvaluatedCost, 51 | MakeEvaluatedCostTerm, 52 | MakeEvaluatedEqConstraint, 53 | }, 54 | robust_kernel::IsRobustKernel, 55 | }; 56 | } 57 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/constraint.rs: -------------------------------------------------------------------------------- 1 | /// Equality constraint 2 | pub mod eq_constraint; 3 | /// Equality constraint function 4 | pub mod eq_constraint_fn; 5 | /// Equality constraint 6 | pub mod evaluated_eq_constraint; 7 | /// Equality constraint set 8 | pub mod evaluated_eq_set; 9 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/constraint/evaluated_eq_set.rs: -------------------------------------------------------------------------------- 1 | use core::fmt::Debug; 2 | 3 | use dyn_clone::DynClone; 4 | 5 | use super::evaluated_eq_constraint::EvaluatedEqConstraint; 6 | use crate::{ 7 | block::{ 8 | block_vector::BlockVector, 9 | symmetric_block_sparse_matrix_builder::SymmetricBlockSparseMatrixBuilder, 10 | }, 11 | variables::VarFamilies, 12 | }; 13 | 14 | extern crate alloc; 15 | 16 | /// Evaluated equality constraint set: 17 | /// `{c(V⁰₀, V⁰₁, ..., V⁰ₙ₋₁), ..., c(Vⁱ₀, Vⁱ₁, ..., Vⁱₙ₋₁), ...}`. 18 | /// 19 | /// ## Generic parameters 20 | /// 21 | /// * `INPUT_DIM` 22 | /// - Total input dimension of the constraint residual function `c`. It is the sum of argument 23 | /// dimensions: `|Vⁱ₀| + |Vⁱ₁| + ... + |Vⁱₙ₋₁|`. 24 | /// * `N` 25 | /// - Number of arguments of the residual function `c`. 26 | #[derive(Debug, Clone)] 27 | pub struct EvaluatedEqSet { 28 | /// Variable family name for each argument. 29 | pub family_names: [String; N], 30 | /// Collection of evaluated constraints. 31 | pub evaluated_constraints: alloc::vec::Vec>, 32 | } 33 | 34 | impl 35 | EvaluatedEqSet 36 | { 37 | /// Create a new evaluated constraint set. 38 | pub fn new(family_names: [String; N]) -> Self { 39 | EvaluatedEqSet { 40 | family_names, 41 | evaluated_constraints: alloc::vec::Vec::new(), 42 | } 43 | } 44 | } 45 | 46 | /// Evaluated equality constraint set trait. 47 | pub trait IsEvaluatedEqConstraintSet: Debug + DynClone { 48 | /// Return sum of L1 norms of constraint residuals: `∑ᵢ |c(Vⁱ₀, Vⁱ₁, ..., Vⁱₙ₋₁)|`. 49 | fn calc_sum_of_l1_norms(&self) -> f64; 50 | 51 | /// Populate upper triangular KKT matrix. 52 | fn populate_upper_triangular_kkt_mat( 53 | &self, 54 | variables: &VarFamilies, 55 | lambda: &BlockVector, 56 | constraint_idx: usize, 57 | block_triplet: &mut SymmetricBlockSparseMatrixBuilder, 58 | b: &mut BlockVector, 59 | ); 60 | } 61 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/cost.rs: -------------------------------------------------------------------------------- 1 | pub(crate) mod compare_idx; 2 | pub(crate) mod cost_fn; 3 | pub(crate) mod cost_term; 4 | pub(crate) mod evaluated_cost; 5 | pub(crate) mod evaluated_term; 6 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/cost/evaluated_cost.rs: -------------------------------------------------------------------------------- 1 | use core::fmt::Debug; 2 | 3 | use dyn_clone::DynClone; 4 | 5 | use super::evaluated_term::EvaluatedCostTerm; 6 | use crate::{ 7 | block::{ 8 | block_vector::BlockVector, 9 | symmetric_block_sparse_matrix_builder::SymmetricBlockSparseMatrixBuilder, 10 | }, 11 | variables::VarFamilies, 12 | }; 13 | extern crate alloc; 14 | 15 | /// Evaluated non-linear least squares cost. 16 | /// 17 | /// ## Generic parameters 18 | /// 19 | /// * `INPUT_DIM` 20 | /// - Total input dimension of the residual function `g`. It is the sum of argument dimensions: 21 | /// `|Vⁱ₀| + |Vⁱ₁| + ... + |Vⁱₙ₋₁|`. 22 | /// * `N` 23 | /// - Number of arguments of the residual function `g`. 24 | #[derive(Debug, Clone)] 25 | pub struct EvaluatedCost { 26 | /// Variable family name for each argument. 27 | pub family_names: [String; N], 28 | /// Collection of evaluated terms. 29 | pub terms: alloc::vec::Vec>, 30 | } 31 | 32 | impl EvaluatedCost { 33 | /// Create a new evaluated cost. 34 | pub fn new(family_names: [String; N]) -> Self { 35 | EvaluatedCost { 36 | family_names, 37 | terms: alloc::vec::Vec::new(), 38 | } 39 | } 40 | } 41 | 42 | /// Evaluated non-linear least squares cost trait. 43 | pub trait IsEvaluatedCost: Debug + DynClone { 44 | /// Return the sum of squared errors. 45 | fn calc_square_error(&self) -> f64; 46 | 47 | /// Populate upper triangular matrix of the normal equation. 48 | fn populate_upper_triangulatr_normal_equation( 49 | &self, 50 | variables: &VarFamilies, 51 | nu: f64, 52 | hessian_block_triplet: &mut SymmetricBlockSparseMatrixBuilder, 53 | neg_grad: &mut BlockVector, 54 | ); 55 | } 56 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library.rs: -------------------------------------------------------------------------------- 1 | /// Library of cost functors 2 | pub mod costs; 3 | /// Library of equality constraints 4 | pub mod eq_constraints; 5 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/costs.rs: -------------------------------------------------------------------------------- 1 | mod example_non_linear_cost; 2 | mod isometry2_prior; 3 | mod isometry3_prior; 4 | mod pinhole_reprojection; 5 | mod pose_graph; 6 | mod quadratic1; 7 | mod quadratic2; 8 | 9 | pub use example_non_linear_cost::*; 10 | pub use isometry2_prior::*; 11 | pub use isometry3_prior::*; 12 | pub use pinhole_reprojection::*; 13 | pub use pose_graph::*; 14 | pub use quadratic1::*; 15 | pub use quadratic2::*; 16 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/costs/example_non_linear_cost.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::{ 3 | DualScalar, 4 | DualVector, 5 | }, 6 | linalg::VecF64, 7 | }; 8 | 9 | use crate::{ 10 | nlls::cost::evaluated_term::EvaluatedCostTerm, 11 | prelude::*, 12 | robust_kernel::RobustKernel, 13 | variables::VarKind, 14 | }; 15 | 16 | /// Example non-linear residual cost term. 17 | /// 18 | /// ```ascii 19 | /// [[x₀² + x₁]] 20 | /// g(x) = [[ ]] - z 21 | /// [[x₁² - x₀]] 22 | /// ``` 23 | #[derive(Clone, Debug)] 24 | pub struct ExampleNonLinearCostTerm { 25 | /// 2d measurement. 26 | pub z: VecF64<2>, 27 | /// Entity index for `x`. 28 | pub entity_indices: [usize; 1], 29 | } 30 | 31 | impl ExampleNonLinearCostTerm { 32 | /// Compute the residual 33 | pub fn residual, const DM: usize, const DN: usize>( 34 | x: Scalar::Vector<2>, 35 | z: Scalar::Vector<2>, 36 | ) -> Scalar::Vector<2> { 37 | Scalar::Vector::<2>::from_array([ 38 | x.elem(0) * x.elem(0) + x.elem(1), 39 | x.elem(1) * x.elem(1) - x.elem(0), 40 | ]) - z 41 | } 42 | } 43 | 44 | impl HasResidualFn<2, 1, (), VecF64<2>> for ExampleNonLinearCostTerm { 45 | fn idx_ref(&self) -> &[usize; 1] { 46 | &self.entity_indices 47 | } 48 | 49 | fn eval( 50 | &self, 51 | _global_constants: &(), 52 | idx: [usize; 1], 53 | args: VecF64<2>, 54 | var_kinds: [VarKind; 1], 55 | robust_kernel: Option, 56 | ) -> EvaluatedCostTerm<2, 1> { 57 | let residual = Self::residual::(args, self.z); 58 | let dx_res_fn = 59 | |x| Self::residual::, 2, 1>(x, DualVector::from_real_vector(self.z)); 60 | 61 | (|| dx_res_fn(DualVector::var(args)).jacobian(),).make( 62 | idx, 63 | var_kinds, 64 | residual, 65 | robust_kernel, 66 | None, 67 | ) 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/costs/isometry2_prior.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::DualVector, 3 | linalg::{ 4 | MatF64, 5 | VecF64, 6 | }, 7 | }; 8 | use sophus_lie::{ 9 | Isometry2, 10 | Isometry2F64, 11 | }; 12 | 13 | use crate::{ 14 | nlls::cost::evaluated_term::EvaluatedCostTerm, 15 | prelude::*, 16 | robust_kernel, 17 | variables::VarKind, 18 | }; 19 | 20 | /// Isometry2 prior residual cost term. 21 | /// 22 | /// 23 | /// `g(T) = log[T * E(T)⁻¹]` 24 | /// 25 | /// where `T ∈ SE(2)` is the current isometry and `E(T) ∈ SE(2)` is the 26 | /// prior mean isometry. The quadratic cost function is defined as: 27 | /// 28 | /// 29 | /// `f(T) = 0.5 * (g(T)ᵀ * W * g(T)).` 30 | /// 31 | /// where `W` is the prior `3x3` precision matrix, which is the inverse of the prior 32 | /// covariance matrix. 33 | #[derive(Clone, Debug)] 34 | pub struct Isometry2PriorCostTerm { 35 | /// Prior mean, `E(T)` of type [Isometry2F64]. 36 | pub isometry_prior_mean: Isometry2F64, 37 | /// Prior precision, `W`. 38 | pub isometry_prior_precision: MatF64<3, 3>, 39 | /// Entity index for `T`. 40 | pub entity_indices: [usize; 1], 41 | } 42 | 43 | impl Isometry2PriorCostTerm { 44 | /// Compute the residual. 45 | /// 46 | /// `g(T) = log[T * E(T)⁻¹]` 47 | /// 48 | /// Note that `T:= isometry` and `E(T):= isometry_prior_mean` are of type [Isometry2F64]. 49 | pub fn residual, const DM: usize, const DN: usize>( 50 | isometry: Isometry2, 51 | isometry_prior_mean: Isometry2, 52 | ) -> Scalar::Vector<3> { 53 | (isometry * isometry_prior_mean.inverse()).log() 54 | } 55 | } 56 | 57 | impl HasResidualFn<3, 1, (), Isometry2F64> for Isometry2PriorCostTerm { 58 | fn idx_ref(&self) -> &[usize; 1] { 59 | &self.entity_indices 60 | } 61 | 62 | fn eval( 63 | &self, 64 | _global_constants: &(), 65 | idx: [usize; 1], 66 | args: Isometry2F64, 67 | var_kinds: [VarKind; 1], 68 | robust_kernel: Option, 69 | ) -> EvaluatedCostTerm<3, 1> { 70 | let isometry: Isometry2F64 = args; 71 | 72 | let residual = Self::residual(isometry, self.isometry_prior_mean); 73 | let dx_res_fn = |x: DualVector<3, 3, 1>| -> DualVector<3, 3, 1> { 74 | Self::residual( 75 | Isometry2::exp(x) * isometry.to_dual_c(), 76 | self.isometry_prior_mean.to_dual_c(), 77 | ) 78 | }; 79 | 80 | (|| dx_res_fn(DualVector::var(VecF64::<3>::zeros())).jacobian(),).make( 81 | idx, 82 | var_kinds, 83 | residual, 84 | robust_kernel, 85 | Some(self.isometry_prior_precision), 86 | ) 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/costs/isometry3_prior.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::DualVector, 3 | linalg::{ 4 | MatF64, 5 | VecF64, 6 | }, 7 | }; 8 | use sophus_lie::{ 9 | Isometry3, 10 | Isometry3F64, 11 | }; 12 | 13 | use crate::{ 14 | nlls::cost::evaluated_term::EvaluatedCostTerm, 15 | prelude::*, 16 | robust_kernel, 17 | variables::VarKind, 18 | }; 19 | 20 | /// Isometry3 prior residual cost term. 21 | /// 22 | /// 23 | /// `g(T) = log[T * E(T)⁻¹]` 24 | /// 25 | /// where `T ∈ SE(3)` is the current isometry and `E(T) ∈ SE(3)` is the 26 | /// prior mean isometry. The quadratic cost function is defined as: 27 | /// 28 | /// 29 | /// `f(T) = 0.5 * (g(T)ᵀ * W * g(T)).` 30 | /// 31 | /// where `W` is the prior `6x6` precision matrix, which is the inverse of the prior 32 | /// covariance matrix. 33 | #[derive(Clone, Debug)] 34 | pub struct Isometry3PriorCostTerm { 35 | /// Prior mean, `E(T)` of type [Isometry3F64]. 36 | pub isometry_prior_mean: Isometry3F64, 37 | /// Prior precision, `W`. 38 | pub isometry_prior_precision: MatF64<6, 6>, 39 | /// Entity index for `T`. 40 | pub entity_indices: [usize; 1], 41 | } 42 | 43 | impl Isometry3PriorCostTerm { 44 | /// Compute the residual. 45 | /// 46 | /// `g(T) = log[T * E(T)⁻¹]` 47 | /// 48 | /// Note that `T:= isometry` and `E(T):= isometry_prior_mean` are of type [Isometry3F64]. 49 | pub fn residual, const DM: usize, const DN: usize>( 50 | isometry: Isometry3, 51 | isometry_prior_mean: Isometry3, 52 | ) -> Scalar::Vector<6> { 53 | (isometry * isometry_prior_mean.inverse()).log() 54 | } 55 | } 56 | 57 | impl HasResidualFn<6, 1, (), Isometry3F64> for Isometry3PriorCostTerm { 58 | fn idx_ref(&self) -> &[usize; 1] { 59 | &self.entity_indices 60 | } 61 | 62 | fn eval( 63 | &self, 64 | _global_constants: &(), 65 | idx: [usize; 1], 66 | args: Isometry3F64, 67 | var_kinds: [VarKind; 1], 68 | robust_kernel: Option, 69 | ) -> EvaluatedCostTerm<6, 1> { 70 | let isometry: Isometry3F64 = args; 71 | 72 | let residual = Self::residual(isometry, self.isometry_prior_mean); 73 | let dx_res_fn = |x: DualVector<6, 6, 1>| -> DualVector<6, 6, 1> { 74 | Self::residual( 75 | Isometry3::exp(x) * isometry.to_dual_c(), 76 | self.isometry_prior_mean.to_dual_c(), 77 | ) 78 | }; 79 | 80 | (|| dx_res_fn(DualVector::var(VecF64::<6>::zeros())).jacobian(),).make( 81 | idx, 82 | var_kinds, 83 | residual, 84 | robust_kernel, 85 | Some(self.isometry_prior_precision), 86 | ) 87 | } 88 | } 89 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/costs/pose_graph.rs: -------------------------------------------------------------------------------- 1 | use sophus_lie::{ 2 | Isometry2, 3 | Isometry2F64, 4 | }; 5 | 6 | use crate::{ 7 | nlls::cost::evaluated_term::EvaluatedCostTerm, 8 | prelude::*, 9 | robust_kernel, 10 | variables::VarKind, 11 | }; 12 | 13 | /// 2d pose graph prior residual cost term. 14 | /// 15 | /// `g(ʷTₘ, ʷTₙ) = log[ (ʷTₘ)⁻¹ ∙ ʷTₙ ∙ (ᵐTₙ)⁻¹ ]` 16 | /// 17 | /// where `ʷTₘ ∈ SE(2)` is a pose of `m` in the world coordinate frame, `ʷTₙ ∈ SE(2)` is a pose 18 | /// of `n` in the world coordinate frame, and `ᵐTₙ ∈ SE(2)` is the relative pose of `n` in the 19 | /// `m` coordinate frame. 20 | #[derive(Debug, Clone)] 21 | pub struct PoseGraph2CostTerm { 22 | /// 2d relative pose constraint: `ᵐTₙ` of type [Isometry2F64]. 23 | pub pose_m_from_pose_n: Isometry2F64, 24 | /// Entity indices: 25 | /// - 0: `m` pose in the world coordinate frame `ʷTₘ` 26 | /// - 1: `n` pose in the world coordinate frame `ʷTₙ` 27 | pub entity_indices: [usize; 2], 28 | } 29 | 30 | impl PoseGraph2CostTerm { 31 | /// Compute the residual of the pose graph term 32 | /// 33 | /// `g(ʷTₘ, ʷTₙ) = log[ (ʷTₘ)⁻¹ ∙ ʷTₙ ∙ (ᵐTₙ)⁻¹ ]` 34 | /// 35 | /// Note that `ʷTₘ:= world_from_pose_m`, `ʷTₙ:= world_from_pose_n` and 36 | /// `ᵐTₙ:= pose_m_from_pose_n` are of type `Isometry2F64`. 37 | pub fn residual, const DM: usize, const DN: usize>( 38 | world_from_pose_m: Isometry2, 39 | world_from_pose_n: Isometry2, 40 | pose_m_from_pose_n: Isometry2, 41 | ) -> S::Vector<3> { 42 | (world_from_pose_m.inverse() * world_from_pose_n * pose_m_from_pose_n.inverse()).log() 43 | } 44 | } 45 | 46 | impl HasResidualFn<12, 2, (), (Isometry2F64, Isometry2F64)> for PoseGraph2CostTerm { 47 | fn idx_ref(&self) -> &[usize; 2] { 48 | &self.entity_indices 49 | } 50 | 51 | fn eval( 52 | &self, 53 | _global_constants: &(), 54 | idx: [usize; 2], 55 | world_from_pose_x: (Isometry2F64, Isometry2F64), 56 | var_kinds: [VarKind; 2], 57 | robust_kernel: Option, 58 | ) -> EvaluatedCostTerm<12, 2> { 59 | let world_from_pose_m = world_from_pose_x.0; 60 | let world_from_pose_n = world_from_pose_x.1; 61 | 62 | let residual = Self::residual( 63 | world_from_pose_m, 64 | world_from_pose_n, 65 | self.pose_m_from_pose_n, 66 | ); 67 | 68 | ( 69 | || { 70 | -Isometry2::dx_log_a_exp_x_b_at_0( 71 | world_from_pose_m.inverse(), 72 | world_from_pose_n * self.pose_m_from_pose_n.inverse(), 73 | ) 74 | }, 75 | || { 76 | Isometry2::dx_log_a_exp_x_b_at_0( 77 | world_from_pose_m.inverse(), 78 | world_from_pose_n * self.pose_m_from_pose_n.inverse(), 79 | ) 80 | }, 81 | ) 82 | .make(idx, var_kinds, residual, robust_kernel, None) 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/costs/quadratic1.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::{ 3 | DualScalar, 4 | DualVector, 5 | }, 6 | linalg::VecF64, 7 | }; 8 | 9 | use crate::{ 10 | nlls::cost::evaluated_term::EvaluatedCostTerm, 11 | prelude::*, 12 | robust_kernel::RobustKernel, 13 | variables::VarKind, 14 | }; 15 | 16 | /// Linear 1d quadratic residual cost term. 17 | /// 18 | /// `g(x) = x - z`. 19 | /// 20 | /// The quadratic cost function is defined as: 21 | /// 22 | /// `f(x) = 0.5 * (g(x)ᵀ * g(x)).` 23 | #[derive(Clone, Debug)] 24 | pub struct Quadratic1CostTerm { 25 | /// Measurement 26 | pub z: VecF64<1>, 27 | /// entity index 28 | pub entity_indices: [usize; 1], 29 | } 30 | 31 | impl Quadratic1CostTerm { 32 | /// Compute the residual 33 | /// 34 | /// `g(x) = x - z`. 35 | pub fn residual, const DM: usize, const DN: usize>( 36 | x: Scalar::Vector<1>, 37 | z: Scalar::Vector<1>, 38 | ) -> Scalar::Vector<1> { 39 | x - z 40 | } 41 | } 42 | 43 | impl HasResidualFn<1, 1, (), VecF64<1>> for Quadratic1CostTerm { 44 | fn idx_ref(&self) -> &[usize; 1] { 45 | &self.entity_indices 46 | } 47 | 48 | fn eval( 49 | &self, 50 | _global_constants: &(), 51 | idx: [usize; 1], 52 | args: VecF64<1>, 53 | var_kinds: [VarKind; 1], 54 | robust_kernel: Option, 55 | ) -> EvaluatedCostTerm<1, 1> { 56 | let residual = Self::residual::(args, self.z); 57 | let dx_res_fn = |x: DualVector<1, 1, 1>| -> DualVector<1, 1, 1> { 58 | Self::residual::, 1, 1>(x, DualVector::from_real_vector(self.z)) 59 | }; 60 | 61 | (|| dx_res_fn(DualVector::var(args)).jacobian(),).make( 62 | idx, 63 | var_kinds, 64 | residual, 65 | robust_kernel, 66 | None, 67 | ) 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/costs/quadratic2.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::{ 3 | DualScalar, 4 | DualVector, 5 | }, 6 | linalg::VecF64, 7 | }; 8 | 9 | use crate::{ 10 | nlls::cost::evaluated_term::EvaluatedCostTerm, 11 | prelude::*, 12 | robust_kernel::RobustKernel, 13 | variables::VarKind, 14 | }; 15 | 16 | /// Linear 2d quadratic residual cost term. 17 | /// 18 | /// `g(x) = x - z`. 19 | /// 20 | /// The quadratic cost function is defined as: 21 | /// 22 | /// `f(x) = 0.5 * (g(x)ᵀ * g(x)).` 23 | #[derive(Clone, Debug)] 24 | pub struct Quadratic2CostTerm { 25 | /// Measurement 26 | pub z: VecF64<2>, 27 | /// entity index 28 | pub entity_indices: [usize; 1], 29 | } 30 | 31 | impl Quadratic2CostTerm { 32 | /// Compute the residual 33 | /// 34 | /// `g(x) = x - z`. 35 | pub fn residual, const DM: usize, const DN: usize>( 36 | x: Scalar::Vector<2>, 37 | z: Scalar::Vector<2>, 38 | ) -> Scalar::Vector<2> { 39 | x - z 40 | } 41 | } 42 | 43 | impl HasResidualFn<2, 1, (), VecF64<2>> for Quadratic2CostTerm { 44 | fn idx_ref(&self) -> &[usize; 1] { 45 | &self.entity_indices 46 | } 47 | 48 | fn eval( 49 | &self, 50 | _global_constants: &(), 51 | idx: [usize; 1], 52 | args: VecF64<2>, 53 | var_kinds: [VarKind; 1], 54 | robust_kernel: Option, 55 | ) -> EvaluatedCostTerm<2, 1> { 56 | let residual = Self::residual::(args, self.z); 57 | let dx_res_fn = |x: DualVector<2, 2, 1>| -> DualVector<2, 2, 1> { 58 | Self::residual::, 2, 1>( 59 | x, 60 | DualVector::<2, 2, 1>::from_real_vector(self.z), 61 | ) 62 | }; 63 | 64 | (|| dx_res_fn(DualVector::var(args)).jacobian(),).make( 65 | idx, 66 | var_kinds, 67 | residual, 68 | robust_kernel, 69 | None, 70 | ) 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/eq_constraints.rs: -------------------------------------------------------------------------------- 1 | mod example_linear_eq; 2 | mod example_non_linear_eq; 3 | mod spherical_constraint; 4 | 5 | pub use example_linear_eq::*; 6 | pub use example_non_linear_eq::*; 7 | pub use spherical_constraint::*; 8 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/eq_constraints/example_linear_eq.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::{ 3 | DualScalar, 4 | DualVector, 5 | }, 6 | linalg::VecF64, 7 | }; 8 | 9 | use crate::{ 10 | nlls::constraint::eq_constraint::HasEqConstraintResidualFn, 11 | prelude::*, 12 | }; 13 | 14 | /// Linear 1d equality constraint. 15 | /// 16 | /// `x + y = lhs`. 17 | /// 18 | /// The corresponding constraint residual is: 19 | /// 20 | /// `c(x,y) = x + y - lhs`. 21 | #[derive(Clone, Debug)] 22 | pub struct ExampleLinearEqConstraint { 23 | /// Left-hand side of the equality constraint. 24 | pub lhs: f64, 25 | /// Entity indices: 26 | /// - 0: index for `x`. 27 | /// - 1: index for `y`. 28 | pub entity_indices: [usize; 2], 29 | } 30 | 31 | impl ExampleLinearEqConstraint { 32 | /// Compute the residual 33 | /// 34 | /// `c(x,y) = x + y - lhs`. 35 | pub fn residual, const DM: usize, const DN: usize>( 36 | x: Scalar::Vector<1>, 37 | y: Scalar::Vector<1>, 38 | lhs: Scalar, 39 | ) -> Scalar::Vector<1> { 40 | Scalar::Vector::<1>::from_array([(x.elem(0) + y.elem(0)) - lhs]) 41 | } 42 | } 43 | 44 | impl HasEqConstraintResidualFn<1, 2, 2, (), (VecF64<1>, VecF64<1>)> for ExampleLinearEqConstraint { 45 | fn idx_ref(&self) -> &[usize; 2] { 46 | &self.entity_indices 47 | } 48 | 49 | fn eval( 50 | &self, 51 | _global_constants: &(), 52 | idx: [usize; 2], 53 | args: (VecF64<1>, VecF64<1>), 54 | var_kinds: [crate::variables::VarKind; 2], 55 | ) -> crate::nlls::constraint::evaluated_eq_constraint::EvaluatedEqConstraint<1, 2, 2> { 56 | let (x, y) = args; 57 | let residual: VecF64<1> = Self::residual(x, y, self.lhs); 58 | let dx0_res_fn = |a: DualVector<1, 1, 1>| -> DualVector<1, 1, 1> { 59 | let lhs_dual = DualScalar::from_f64(self.lhs); 60 | Self::residual::, 1, 1>(a, DualVector::from_real_vector(y), lhs_dual) 61 | }; 62 | let dx1_res_fn = |a: DualVector<1, 1, 1>| -> DualVector<1, 1, 1> { 63 | let lhs_dual = DualScalar::from_f64(self.lhs); 64 | Self::residual::, 1, 1>(DualVector::from_real_vector(x), a, lhs_dual) 65 | }; 66 | 67 | ( 68 | || dx0_res_fn(DualVector::var(x)).jacobian(), 69 | || dx1_res_fn(DualVector::var(y)).jacobian(), 70 | ) 71 | .make_eq(idx, var_kinds, residual) 72 | } 73 | } 74 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/eq_constraints/example_non_linear_eq.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::{ 3 | DualScalar, 4 | DualVector, 5 | }, 6 | linalg::VecF64, 7 | }; 8 | 9 | use crate::{ 10 | nlls::constraint::eq_constraint::HasEqConstraintResidualFn, 11 | prelude::*, 12 | }; 13 | 14 | /// Linear 1d equality constraint. 15 | /// 16 | /// `x₀² + x₁² = lhs`. 17 | /// 18 | /// The corresponding constraint residual is: 19 | /// 20 | /// `c(x) = x₀² + x₁² - lhs`. 21 | #[derive(Clone, Debug)] 22 | pub struct ExampleNonLinearEqConstraint { 23 | /// Left-hand side of the equality constraint. 24 | pub lhs: f64, 25 | /// Entity index for `x`. 26 | pub entity_indices: [usize; 1], 27 | } 28 | 29 | impl ExampleNonLinearEqConstraint { 30 | /// Compute the constraint residual 31 | /// 32 | /// `c(x) = x₀² + x₁² - lhs`. 33 | pub fn residual, const DM: usize, const DN: usize>( 34 | x: Scalar::Vector<2>, 35 | lhs: Scalar, 36 | ) -> Scalar::Vector<1> { 37 | let r = x.elem(0) * x.elem(0) + x.elem(1) * x.elem(1) - lhs; 38 | Scalar::Vector::<1>::from_array([r]) 39 | } 40 | } 41 | 42 | impl HasEqConstraintResidualFn<1, 2, 1, (), VecF64<2>> for ExampleNonLinearEqConstraint { 43 | fn idx_ref(&self) -> &[usize; 1] { 44 | &self.entity_indices 45 | } 46 | 47 | fn eval( 48 | &self, 49 | _global_constants: &(), 50 | idx: [usize; 1], 51 | x: VecF64<2>, 52 | var_kinds: [crate::variables::VarKind; 1], 53 | ) -> crate::nlls::constraint::evaluated_eq_constraint::EvaluatedEqConstraint<1, 2, 1> { 54 | let residual = Self::residual(x, self.lhs); 55 | let dx0_res_fn = |x: DualVector<2, 2, 1>| -> DualVector<1, 2, 1> { 56 | let lhs_dual = DualScalar::from_f64(self.lhs); 57 | Self::residual::, 2, 1>(x, lhs_dual) 58 | }; 59 | 60 | (|| dx0_res_fn(DualVector::var(x)).jacobian(),).make_eq(idx, var_kinds, residual) 61 | } 62 | } 63 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/functor_library/eq_constraints/spherical_constraint.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::{ 2 | dual::{ 3 | DualScalar, 4 | DualVector, 5 | }, 6 | linalg::VecF64, 7 | }; 8 | 9 | use crate::{ 10 | nlls::constraint::eq_constraint::HasEqConstraintResidualFn, 11 | prelude::*, 12 | }; 13 | 14 | /// Spherical equality constraint. 15 | /// 16 | /// `|x| = r`. 17 | /// 18 | /// where `x` is a 3D vector and `r` is the sphere radius. 19 | /// 20 | /// The corresponding constraint residual is: 21 | /// 22 | /// `c(x) = |x| - r`. 23 | #[derive(Clone, Debug)] 24 | pub struct SphericalConstraint { 25 | /// Sphere radius `r`. 26 | pub radius: f64, 27 | /// Entity indices for `x`. 28 | pub entity_indices: [usize; 1], 29 | } 30 | 31 | impl SphericalConstraint { 32 | /// Compute the constraint residual 33 | /// 34 | /// `c(x) = |x| - r`. 35 | pub fn residual, const DM: usize, const DN: usize>( 36 | vec: Scalar::Vector<3>, 37 | radius: Scalar, 38 | ) -> Scalar::Vector<1> { 39 | let norm = vec.norm(); 40 | Scalar::Vector::<1>::from_array([norm - radius]) 41 | } 42 | } 43 | 44 | impl HasEqConstraintResidualFn<1, 3, 1, (), VecF64<3>> for SphericalConstraint { 45 | fn idx_ref(&self) -> &[usize; 1] { 46 | &self.entity_indices 47 | } 48 | 49 | fn eval( 50 | &self, 51 | _global_constants: &(), 52 | idx: [usize; 1], 53 | vec3: VecF64<3>, 54 | var_kinds: [crate::variables::VarKind; 1], 55 | ) -> crate::nlls::constraint::evaluated_eq_constraint::EvaluatedEqConstraint<1, 3, 1> { 56 | let residual = Self::residual(vec3, self.radius); 57 | let dx_res_fn = |x: DualVector<3, 3, 1>| -> DualVector<1, 3, 1> { 58 | let radius_dual = DualScalar::from_f64(self.radius); 59 | Self::residual::, 3, 1>(x, radius_dual) 60 | }; 61 | 62 | (|| dx_res_fn(DualVector::var(vec3)).jacobian(),).make_eq(idx, var_kinds, residual) 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/linear_system/solvers/dense_lu.rs: -------------------------------------------------------------------------------- 1 | use super::{ 2 | IsSparseSymmetricLinearSystem, 3 | NllsError, 4 | }; 5 | use crate::{ 6 | block::symmetric_block_sparse_matrix_builder::SymmetricBlockSparseMatrixBuilder, 7 | nlls::linear_system::solvers::IsDenseLinearSystem, 8 | }; 9 | 10 | /// Dense LU solver 11 | /// 12 | /// LU decomposition with full pivoting - wrapper around nalgebra's full_piv_lu. 13 | pub struct DenseLu; 14 | 15 | impl IsSparseSymmetricLinearSystem for DenseLu { 16 | fn solve( 17 | &self, 18 | triplets: &SymmetricBlockSparseMatrixBuilder, 19 | b: &nalgebra::DVector, 20 | ) -> Result, NllsError> { 21 | self.solve_dense(triplets.to_symmetric_dense(), b) 22 | } 23 | } 24 | 25 | impl IsDenseLinearSystem for DenseLu { 26 | fn solve_dense( 27 | &self, 28 | mat_a: nalgebra::DMatrix, 29 | b: &nalgebra::DVector, 30 | ) -> Result, NllsError> { 31 | match mat_a.full_piv_lu().solve(b) { 32 | Some(x) => Ok(x), 33 | None => Err(NllsError::DenseLuError), 34 | } 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/linear_system/solvers/sparse_lu.rs: -------------------------------------------------------------------------------- 1 | use faer::{ 2 | MatMut, 3 | prelude::{ 4 | ReborrowMut, 5 | Solve, 6 | }, 7 | sparse::{ 8 | FaerError, 9 | SparseColMat, 10 | linalg::LuError, 11 | }, 12 | }; 13 | 14 | use super::{ 15 | IsSparseSymmetricLinearSystem, 16 | NllsError, 17 | SparseSolverError, 18 | }; 19 | use crate::block::symmetric_block_sparse_matrix_builder::SymmetricBlockSparseMatrixBuilder; 20 | 21 | /// Sparse LU solver 22 | /// 23 | /// Sparse LU decomposition - wrapper around faer's sp_lu implementation. 24 | pub struct SparseLu; 25 | 26 | impl IsSparseSymmetricLinearSystem for SparseLu { 27 | fn solve( 28 | &self, 29 | upper_triangular: &SymmetricBlockSparseMatrixBuilder, 30 | b: &nalgebra::DVector, 31 | ) -> Result, NllsError> { 32 | let dim = upper_triangular.scalar_dimension(); 33 | let csc: SparseColMat = SparseColMat::try_new_from_triplets( 34 | dim, 35 | dim, 36 | &upper_triangular.to_symmetric_scalar_triplets(), 37 | ) 38 | .unwrap(); 39 | let mut x = b.clone(); 40 | let mut x_ref = MatMut::::from_column_major_slice_mut(x.as_mut_slice(), dim, 1); 41 | 42 | match csc.sp_lu() { 43 | Ok(lu) => { 44 | lu.solve_in_place(ReborrowMut::rb_mut(&mut x_ref)); 45 | Ok(x) 46 | } 47 | Err(e) => Err(NllsError::SparseLuError { 48 | details: match e { 49 | LuError::Generic(faer_err) => match faer_err { 50 | FaerError::IndexOverflow => SparseSolverError::IndexOverflow, 51 | FaerError::OutOfMemory => SparseSolverError::OutOfMemory, 52 | _ => SparseSolverError::Unspecific, 53 | }, 54 | LuError::SymbolicSingular { .. } => SparseSolverError::SymbolicSingular, 55 | }, 56 | }), 57 | } 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/nlls/linear_system/solvers/sparse_qr.rs: -------------------------------------------------------------------------------- 1 | use faer::{ 2 | prelude::Solve, 3 | sparse::FaerError, 4 | }; 5 | 6 | use super::{ 7 | IsSparseSymmetricLinearSystem, 8 | NllsError, 9 | SparseSolverError, 10 | }; 11 | use crate::block::symmetric_block_sparse_matrix_builder::SymmetricBlockSparseMatrixBuilder; 12 | 13 | /// Sparse QR solver 14 | /// 15 | /// Sparse QR decomposition - wrapper around faer's sp_qr implementation. 16 | pub struct SparseQr; 17 | 18 | impl IsSparseSymmetricLinearSystem for SparseQr { 19 | fn solve( 20 | &self, 21 | upper_triangular: &SymmetricBlockSparseMatrixBuilder, 22 | b: &nalgebra::DVector, 23 | ) -> Result, NllsError> { 24 | let dim = upper_triangular.scalar_dimension(); 25 | let csc = faer::sparse::SparseColMat::try_new_from_triplets( 26 | dim, 27 | dim, 28 | &upper_triangular.to_symmetric_scalar_triplets(), 29 | ) 30 | .unwrap(); 31 | let mut x = b.clone(); 32 | let x_slice_mut = x.as_mut_slice(); 33 | let mut x_ref = faer::MatMut::::from_column_major_slice_mut(x_slice_mut, dim, 1); 34 | 35 | match csc.sp_qr() { 36 | Ok(qr) => { 37 | qr.solve_in_place(faer::reborrow::ReborrowMut::rb_mut(&mut x_ref)); 38 | Ok(x) 39 | } 40 | Err(e) => Err(NllsError::SparseQrError { 41 | details: match e { 42 | FaerError::IndexOverflow => SparseSolverError::IndexOverflow, 43 | FaerError::OutOfMemory => SparseSolverError::OutOfMemory, 44 | _ => SparseSolverError::Unspecific, 45 | }, 46 | }), 47 | } 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/robust_kernel.rs: -------------------------------------------------------------------------------- 1 | /// Robust kernel functions 2 | #[derive(Debug, Clone, Copy)] 3 | pub struct HuberKernel { 4 | delta: f64, 5 | } 6 | 7 | /// Trait for robust kernels 8 | pub trait IsRobustKernel { 9 | /// apply the kernel 10 | fn apply(&self, x: f64) -> f64; 11 | 12 | /// calculate the weight for a given residual 13 | fn weight(&self, residual_nrm: f64) -> f64 { 14 | if residual_nrm == 0.0 { 15 | return 0.0; 16 | } 17 | self.apply(residual_nrm).sqrt() / residual_nrm 18 | } 19 | } 20 | 21 | impl HuberKernel { 22 | /// create a new Huber kernel 23 | pub fn new(delta: f64) -> Self { 24 | Self { delta } 25 | } 26 | } 27 | 28 | impl IsRobustKernel for HuberKernel { 29 | fn apply(&self, x: f64) -> f64 { 30 | if x.abs() <= self.delta { 31 | x.powi(2) 32 | } else { 33 | 2.0 * self.delta * x.abs() - self.delta.powi(2) 34 | } 35 | } 36 | } 37 | 38 | /// Robust kernel functions 39 | #[derive(Debug, Clone, Copy)] 40 | pub enum RobustKernel { 41 | /// Huber kernel 42 | Huber(HuberKernel), 43 | } 44 | 45 | impl IsRobustKernel for RobustKernel { 46 | fn apply(&self, x: f64) -> f64 { 47 | match self { 48 | RobustKernel::Huber(huber) => huber.apply(x), 49 | } 50 | } 51 | 52 | fn weight(&self, x: f64) -> f64 { 53 | match self { 54 | RobustKernel::Huber(huber) => huber.weight(x), 55 | } 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/variables.rs: -------------------------------------------------------------------------------- 1 | use core::fmt::Debug; 2 | 3 | mod var_builder; 4 | mod var_families; 5 | mod var_family; 6 | mod var_tuple; 7 | 8 | pub use var_builder::*; 9 | pub use var_families::*; 10 | pub use var_family::*; 11 | pub use var_tuple::*; 12 | 13 | extern crate alloc; 14 | 15 | /// Variable kind 16 | #[derive(PartialEq, Debug, Clone, Copy)] 17 | pub enum VarKind { 18 | /// free variable (will be updated during optimization) 19 | Free, 20 | /// conditioned variable (will not be fixed during optimization) 21 | Conditioned, 22 | /// marginalized variable (will be updated during using the Schur complement trick) 23 | Marginalized, 24 | } 25 | -------------------------------------------------------------------------------- /crates/sophus_opt/src/variables/var_builder.rs: -------------------------------------------------------------------------------- 1 | use core::fmt::Debug; 2 | 3 | use sophus_autodiff::manifold::IsVariable; 4 | 5 | use super::{ 6 | var_families::VarFamilies, 7 | var_family::{ 8 | IsVarFamily, 9 | VarFamily, 10 | }, 11 | }; 12 | 13 | extern crate alloc; 14 | 15 | /// Builder for variable families 16 | #[derive(Debug, Clone)] 17 | pub struct VarBuilder { 18 | pub(crate) families: alloc::collections::BTreeMap>, 19 | } 20 | 21 | impl Default for VarBuilder { 22 | fn default() -> Self { 23 | Self::new() 24 | } 25 | } 26 | 27 | impl VarBuilder { 28 | /// create a new set of variable families 29 | pub fn new() -> Self { 30 | Self { 31 | families: alloc::collections::btree_map::BTreeMap::new(), 32 | } 33 | } 34 | 35 | /// add a family of variables to the pool 36 | pub fn add_family( 37 | mut self, 38 | name: &str, 39 | family: VarFamily, 40 | ) -> Self { 41 | self.families 42 | .insert(name.into(), alloc::boxed::Box::new(family)); 43 | self 44 | } 45 | 46 | /// build the variable pool 47 | pub fn build(self) -> VarFamilies { 48 | VarFamilies::new(self) 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /crates/sophus_renderer/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "wgpu-based renderer" 3 | name = "sophus_renderer" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_image.workspace = true 16 | sophus_lie.workspace = true 17 | sophus_opt.workspace = true 18 | sophus_sensor.workspace = true 19 | sophus_tensor.workspace = true 20 | 21 | approx.workspace = true 22 | bytemuck.workspace = true 23 | eframe.workspace = true 24 | egui_extras.workspace = true 25 | env_logger.workspace = true 26 | linked-hash-map.workspace = true 27 | num-traits.workspace = true 28 | snafu.workspace = true 29 | 30 | [build-dependencies] 31 | rustc_version.workspace = true 32 | 33 | [features] 34 | std = [ 35 | "sophus_autodiff/std", 36 | "sophus_image/std", 37 | "sophus_lie/std", 38 | "sophus_opt/std", 39 | "sophus_sensor/std", 40 | "sophus_tensor/std", 41 | ] 42 | default = ["std"] 43 | -------------------------------------------------------------------------------- /crates/sophus_renderer/README.md: -------------------------------------------------------------------------------- 1 | ## wgpu-based renderer 2 | 3 | ## Integration with sophus-rs 4 | 5 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 6 | It re-exports the relevant prelude types under [prelude], so you can 7 | seamlessly interoperate with the rest of the sophus-rs types. 8 | -------------------------------------------------------------------------------- /crates/sophus_renderer/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/camera.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::VecF64; 2 | use sophus_image::ImageSize; 3 | use sophus_lie::{ 4 | Isometry3, 5 | Isometry3F64, 6 | }; 7 | 8 | mod clipping_planes; 9 | mod intrinsics; 10 | mod properties; 11 | 12 | pub use clipping_planes::*; 13 | pub use intrinsics::*; 14 | pub use properties::*; 15 | 16 | /// Render camera configuration. 17 | #[derive(Clone, Debug)] 18 | pub struct RenderCamera { 19 | /// Scene from camera pose 20 | pub scene_from_camera: Isometry3F64, 21 | /// Camera properties 22 | pub properties: RenderCameraProperties, 23 | } 24 | 25 | impl Default for RenderCamera { 26 | fn default() -> Self { 27 | RenderCamera::default_from(ImageSize::new(639, 479)) 28 | } 29 | } 30 | 31 | impl RenderCamera { 32 | /// Create default viewer camera from image size 33 | pub fn default_from(image_size: ImageSize) -> RenderCamera { 34 | RenderCamera { 35 | properties: RenderCameraProperties::default_from(image_size), 36 | scene_from_camera: Isometry3::from_translation(VecF64::<3>::new(0.0, 0.0, -5.0)), 37 | } 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/camera/clipping_planes.rs: -------------------------------------------------------------------------------- 1 | use num_traits::cast; 2 | use sophus_autodiff::linalg::FloatingPointNumber; 3 | 4 | /// Clipping planes for the Wgpu renderer 5 | #[derive(Clone, Copy, Debug)] 6 | pub struct ClippingPlanes { 7 | /// Near clipping plane 8 | pub near: S, 9 | /// Far clipping plane 10 | pub far: S, 11 | } 12 | 13 | impl Default for ClippingPlanesF64 { 14 | fn default() -> Self { 15 | ClippingPlanes { 16 | near: ClippingPlanes::DEFAULT_NEAR, 17 | far: ClippingPlanes::DEFAULT_FAR, 18 | } 19 | } 20 | } 21 | 22 | impl ClippingPlanes { 23 | /// metric z from ndc z 24 | pub fn metric_z_from_ndc_z(&self, ndc_z: S) -> S { 25 | -(self.far * self.near) / (-self.far + ndc_z * self.far - ndc_z * self.near) 26 | } 27 | 28 | /// ndc z from metric z 29 | pub fn ndc_z_from_metric_z(&self, z: S) -> S { 30 | (self.far * (z - self.near)) / (z * (self.far - self.near)) 31 | } 32 | 33 | /// cast 34 | pub fn cast(self) -> ClippingPlanes { 35 | ClippingPlanes { 36 | near: cast(self.near).unwrap(), 37 | far: cast(self.far).unwrap(), 38 | } 39 | } 40 | } 41 | 42 | /// f32 clipping planes 43 | pub type ClippingPlanesF32 = ClippingPlanes; 44 | /// f64 clipping planes 45 | pub type ClippingPlanesF64 = ClippingPlanes; 46 | 47 | impl ClippingPlanesF64 { 48 | /// default near clipping plane 49 | pub const DEFAULT_NEAR: f64 = 1.0; 50 | /// default far clipping plane 51 | pub const DEFAULT_FAR: f64 = 1000.0; 52 | } 53 | 54 | #[test] 55 | fn clipping_plane_tests() { 56 | for near in [0.1, 0.5, 1.0, 7.0] { 57 | for far in [10.0, 5.0, 100.0, 1000.0] { 58 | for ndc_z in [0.0, 0.1, 0.5, 0.7, 0.99, 1.0] { 59 | let clipping_planes = ClippingPlanesF64 { near, far }; 60 | 61 | let metric_z = clipping_planes.metric_z_from_ndc_z(ndc_z); 62 | let roundtrip_ndc_z = clipping_planes.ndc_z_from_metric_z(metric_z); 63 | 64 | approx::assert_abs_diff_eq!(roundtrip_ndc_z, ndc_z, epsilon = 0.0001); 65 | } 66 | } 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/camera/intrinsics.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::VecF64; 2 | use sophus_image::ImageSize; 3 | use sophus_lie::prelude::IsVector; 4 | use sophus_sensor::{ 5 | DynCameraF64, 6 | EnhancedUnifiedCameraF64, 7 | PerspectiveCameraEnum, 8 | PinholeCameraF64, 9 | }; 10 | 11 | /// Camera intrinsics 12 | #[derive(Clone, Debug)] 13 | pub enum RenderIntrinsics { 14 | /// Pinhole camera model 15 | Pinhole(PinholeCameraF64), 16 | /// Unified camera model 17 | UnifiedExtended(EnhancedUnifiedCameraF64), 18 | } 19 | 20 | impl RenderIntrinsics { 21 | /// Create a new RenderIntrinsics from a DynCameraF64 22 | pub fn new(camera: &DynCameraF64) -> RenderIntrinsics { 23 | match camera.model_enum() { 24 | PerspectiveCameraEnum::Pinhole(pinhole) => RenderIntrinsics::Pinhole(*pinhole), 25 | PerspectiveCameraEnum::KannalaBrandt(_camera) => todo!(), 26 | PerspectiveCameraEnum::BrownConrady(_camera) => todo!(), 27 | PerspectiveCameraEnum::EnhancedUnified(camera) => { 28 | RenderIntrinsics::UnifiedExtended(*camera) 29 | } 30 | } 31 | } 32 | 33 | /// Get the image size 34 | pub fn image_size(&self) -> ImageSize { 35 | match self { 36 | RenderIntrinsics::Pinhole(pinhole) => pinhole.image_size(), 37 | RenderIntrinsics::UnifiedExtended(unified) => unified.image_size(), 38 | } 39 | } 40 | 41 | /// Unproject a point from the image plane with a given depth 42 | pub fn cam_unproj_with_z(&self, uv: &VecF64<2>, z: f64) -> VecF64<3> { 43 | match self { 44 | RenderIntrinsics::Pinhole(pinhole) => pinhole.cam_unproj_with_z(uv, z), 45 | RenderIntrinsics::UnifiedExtended(unified) => unified.cam_unproj_with_z(uv, z), 46 | } 47 | } 48 | 49 | /// Return pinhole model 50 | pub fn pinhole_model(&self) -> PinholeCameraF64 { 51 | match self { 52 | RenderIntrinsics::Pinhole(camera) => *camera, 53 | RenderIntrinsics::UnifiedExtended(camera) => PinholeCameraF64::new( 54 | VecF64::<4>::from_array([ 55 | 0.5 * camera.params()[0], 56 | 0.5 * camera.params()[1], 57 | camera.params()[2], 58 | camera.params()[3], 59 | ]), 60 | camera.image_size(), 61 | ), 62 | } 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/camera/properties.rs: -------------------------------------------------------------------------------- 1 | use sophus_image::ImageSize; 2 | use sophus_sensor::DynCameraF64; 3 | 4 | use crate::{ 5 | camera::{ 6 | clipping_planes::{ 7 | ClippingPlanes, 8 | ClippingPlanesF64, 9 | }, 10 | intrinsics::RenderIntrinsics, 11 | }, 12 | uniform_buffers::CameraPropertiesUniform, 13 | }; 14 | 15 | /// Camera properties 16 | #[derive(Clone, Debug)] 17 | pub struct RenderCameraProperties { 18 | /// Camera intrinsics 19 | pub intrinsics: RenderIntrinsics, 20 | /// Clipping planes 21 | pub clipping_planes: ClippingPlanesF64, 22 | } 23 | 24 | impl Default for RenderCameraProperties { 25 | fn default() -> Self { 26 | RenderCameraProperties::default_from(ImageSize::new(639, 479)) 27 | } 28 | } 29 | 30 | impl RenderCameraProperties { 31 | /// Create a new RenderCameraProperties from a DynCameraF64 32 | pub fn new(intrinsics: DynCameraF64, clipping_planes: ClippingPlanesF64) -> Self { 33 | RenderCameraProperties { 34 | intrinsics: RenderIntrinsics::new(&intrinsics), 35 | clipping_planes, 36 | } 37 | } 38 | 39 | /// Create default viewer camera from image size 40 | pub fn default_from(image_size: ImageSize) -> RenderCameraProperties { 41 | RenderCameraProperties { 42 | intrinsics: RenderIntrinsics::Pinhole( 43 | DynCameraF64::default_pinhole(image_size) 44 | .try_get_pinhole() 45 | .unwrap(), 46 | ), 47 | clipping_planes: ClippingPlanes::default(), 48 | } 49 | } 50 | 51 | /// intrinsics 52 | pub fn from_intrinsics(intrinsics: &DynCameraF64) -> Self { 53 | RenderCameraProperties { 54 | intrinsics: RenderIntrinsics::new(intrinsics), 55 | clipping_planes: ClippingPlanes::default(), 56 | } 57 | } 58 | 59 | pub(crate) fn to_uniform(&self) -> CameraPropertiesUniform { 60 | match self.intrinsics { 61 | RenderIntrinsics::Pinhole(pinhole) => CameraPropertiesUniform { 62 | camera_image_width: pinhole.image_size().width as f32, 63 | camera_image_height: pinhole.image_size().height as f32, 64 | near: self.clipping_planes.near as f32, 65 | far: self.clipping_planes.far as f32, 66 | fx: pinhole.params()[0] as f32, 67 | fy: pinhole.params()[1] as f32, 68 | px: pinhole.params()[2] as f32, 69 | py: pinhole.params()[3] as f32, 70 | alpha: 0.0, 71 | beta: 0.0, 72 | }, 73 | RenderIntrinsics::UnifiedExtended(unified) => CameraPropertiesUniform { 74 | camera_image_width: unified.image_size().width as f32, 75 | camera_image_height: unified.image_size().height as f32, 76 | near: self.clipping_planes.near as f32, 77 | far: self.clipping_planes.far as f32, 78 | fx: unified.params()[0] as f32, 79 | fy: unified.params()[1] as f32, 80 | px: unified.params()[2] as f32, 81 | py: unified.params()[3] as f32, 82 | alpha: unified.params()[4] as f32, 83 | beta: unified.params()[5] as f32, 84 | }, 85 | } 86 | } 87 | } 88 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![deny(missing_docs)] 2 | #![allow(clippy::needless_range_loop)] 3 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 4 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 5 | 6 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 7 | #[cfg(doctest)] 8 | pub struct ReadmeDoctests; 9 | 10 | /// Render camera 11 | pub mod camera; 12 | /// The renderable structs. 13 | pub mod renderables; 14 | /// offscreen texture for rendering 15 | pub mod textures; 16 | 17 | /// sophus_renderer prelude. 18 | /// 19 | /// It is recommended to import this prelude when working with `sophus_renderer types: 20 | /// 21 | /// ``` 22 | /// use sophus_renderer::prelude::*; 23 | /// ``` 24 | /// 25 | /// or 26 | /// 27 | /// ```ignore 28 | /// use sophus::prelude::*; 29 | /// ``` 30 | /// 31 | /// to import all preludes when using the `sophus` umbrella crate. 32 | pub mod prelude { 33 | pub(crate) use alloc::{ 34 | collections::btree_map::BTreeMap, 35 | format, 36 | string::{ 37 | String, 38 | ToString, 39 | }, 40 | sync::Arc, 41 | vec, 42 | vec::Vec, 43 | }; 44 | 45 | pub use sophus_autodiff::prelude::*; 46 | pub use sophus_image::prelude::*; 47 | pub use sophus_lie::prelude::*; 48 | pub use sophus_opt::prelude::*; 49 | 50 | extern crate alloc; 51 | } 52 | 53 | mod offscreen_renderer; 54 | mod pipeline_builder; 55 | mod pixel_renderer; 56 | mod render_context; 57 | mod scene_renderer; 58 | mod types; 59 | mod uniform_buffers; 60 | 61 | pub use offscreen_renderer::*; 62 | pub use pipeline_builder::*; 63 | pub use pixel_renderer::*; 64 | pub use render_context::*; 65 | pub use scene_renderer::*; 66 | pub use types::*; 67 | pub use uniform_buffers::*; 68 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/pixel_renderer.rs: -------------------------------------------------------------------------------- 1 | mod pixel_line; 2 | mod pixel_point; 3 | 4 | use eframe::wgpu; 5 | pub use pixel_line::*; 6 | pub use pixel_point::*; 7 | 8 | use crate::{ 9 | RenderContext, 10 | pipeline_builder::{ 11 | PipelineBuilder, 12 | PointVertex2, 13 | TargetTexture, 14 | }, 15 | pixel_renderer::{ 16 | pixel_line::PixelLineRenderer, 17 | pixel_point::PixelPointRenderer, 18 | }, 19 | prelude::*, 20 | types::SceneFocusMarker, 21 | uniform_buffers::VertexShaderUniformBuffers, 22 | }; 23 | 24 | /// Renderer for pixel data 25 | pub struct PixelRenderer { 26 | pub(crate) line_renderer: PixelLineRenderer, 27 | pub(crate) point_renderer: PixelPointRenderer, 28 | pub(crate) pixel_pipeline_builder: PipelineBuilder, 29 | } 30 | 31 | impl PixelRenderer { 32 | /// Create a new pixel renderer 33 | pub fn new(render_context: &RenderContext, uniforms: Arc) -> Self { 34 | let pixel_pipeline_builder = PipelineBuilder::new_pixel( 35 | render_context, 36 | Arc::new(TargetTexture { 37 | rgba_output_format: wgpu::TextureFormat::Rgba8Unorm, 38 | }), 39 | uniforms.clone(), 40 | ); 41 | 42 | Self { 43 | line_renderer: PixelLineRenderer::new(render_context, &pixel_pipeline_builder), 44 | point_renderer: PixelPointRenderer::new(render_context, &pixel_pipeline_builder), 45 | pixel_pipeline_builder, 46 | } 47 | } 48 | 49 | pub(crate) fn show_interaction_marker( 50 | &self, 51 | state: &RenderContext, 52 | marker: &Option, 53 | ) { 54 | *self.point_renderer.show_interaction_marker.lock() = match marker { 55 | Some(marker) => { 56 | let mut vertex_data = vec![]; 57 | 58 | let depth_color = marker.color; 59 | 60 | for _i in 0..6 { 61 | vertex_data.push(PointVertex2 { 62 | _pos: [marker.u, marker.v], 63 | _color: [depth_color.r, depth_color.g, depth_color.b, depth_color.a], 64 | _point_size: 5.0, 65 | }); 66 | } 67 | state.wgpu_queue.write_buffer( 68 | &self.point_renderer.interaction_vertex_buffer, 69 | 0, 70 | bytemuck::cast_slice(&vertex_data), 71 | ); 72 | 73 | true 74 | } 75 | None => false, 76 | }; 77 | } 78 | 79 | pub(crate) fn paint<'rp>( 80 | &'rp self, 81 | command_encoder: &'rp mut wgpu::CommandEncoder, 82 | texture_view: &'rp wgpu::TextureView, 83 | ) { 84 | let mut render_pass = command_encoder.begin_render_pass(&wgpu::RenderPassDescriptor { 85 | label: None, 86 | color_attachments: &[Some(wgpu::RenderPassColorAttachment { 87 | view: texture_view, 88 | resolve_target: None, 89 | ops: wgpu::Operations { 90 | load: wgpu::LoadOp::Load, 91 | store: wgpu::StoreOp::Store, 92 | }, 93 | })], 94 | depth_stencil_attachment: None, 95 | occlusion_query_set: None, 96 | timestamp_writes: None, 97 | }); 98 | render_pass.set_bind_group( 99 | 0, 100 | &self.pixel_pipeline_builder.uniforms.render_bind_group, 101 | &[], 102 | ); 103 | 104 | self.line_renderer.paint(&mut render_pass); 105 | self.point_renderer.paint(&mut render_pass); 106 | } 107 | } 108 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/pixel_renderer/pixel_line.rs: -------------------------------------------------------------------------------- 1 | use eframe::wgpu; 2 | use wgpu::util::DeviceExt; 3 | 4 | use crate::{ 5 | RenderContext, 6 | pipeline_builder::{ 7 | LineVertex2, 8 | PipelineBuilder, 9 | }, 10 | prelude::*, 11 | renderables::LineSegments2, 12 | }; 13 | pub(crate) struct Line2dEntity { 14 | pub(crate) vertex_data: Vec, 15 | pub(crate) vertex_buffer: wgpu::Buffer, 16 | } 17 | 18 | impl Line2dEntity { 19 | pub(crate) fn new(render_context: &RenderContext, lines: &LineSegments2) -> Self { 20 | let mut vertex_data = vec![]; 21 | for line in lines.segments.iter() { 22 | let p0 = line.p0; 23 | let p1 = line.p1; 24 | let d = (p0 - p1).normalize(); 25 | let normal = [d[1], -d[0]]; 26 | 27 | let v0 = LineVertex2 { 28 | _pos: [p0[0], p0[1]], 29 | _normal: normal, 30 | _color: [line.color.r, line.color.g, line.color.b, line.color.a], 31 | _line_width: line.line_width, 32 | }; 33 | let v1 = LineVertex2 { 34 | _pos: [p1[0], p1[1]], 35 | _normal: normal, 36 | _color: [line.color.r, line.color.g, line.color.b, line.color.a], 37 | _line_width: line.line_width, 38 | }; 39 | vertex_data.push(v0); 40 | vertex_data.push(v0); 41 | vertex_data.push(v1); 42 | vertex_data.push(v0); 43 | vertex_data.push(v1); 44 | vertex_data.push(v1); 45 | } 46 | 47 | let vertex_buffer = 48 | render_context 49 | .wgpu_device 50 | .create_buffer_init(&wgpu::util::BufferInitDescriptor { 51 | label: Some(&format!("Pixel line vertex buffer: {}", lines.name)), 52 | contents: bytemuck::cast_slice(&vertex_data), 53 | usage: wgpu::BufferUsages::VERTEX, 54 | }); 55 | 56 | Self { 57 | vertex_data, 58 | vertex_buffer, 59 | } 60 | } 61 | } 62 | 63 | /// Pixel line renderer 64 | pub struct PixelLineRenderer { 65 | pub(crate) pipeline: wgpu::RenderPipeline, 66 | pub(crate) lines_table: BTreeMap, 67 | } 68 | 69 | impl PixelLineRenderer { 70 | /// Create a new pixel line renderer 71 | pub fn new(render_context: &RenderContext, pixel_pipelines: &PipelineBuilder) -> Self { 72 | let device = &render_context.wgpu_device; 73 | 74 | let line_shader = device.create_shader_module(wgpu::ShaderModuleDescriptor { 75 | label: Some("pixel line shader"), 76 | source: wgpu::ShaderSource::Wgsl( 77 | format!( 78 | "{} {}", 79 | include_str!("./../shaders/utils.wgsl"), 80 | include_str!("./../shaders/pixel_line.wgsl") 81 | ) 82 | .into(), 83 | ), 84 | }); 85 | 86 | Self { 87 | lines_table: BTreeMap::new(), 88 | pipeline: pixel_pipelines.create::("line".to_string(), &line_shader, None), 89 | } 90 | } 91 | 92 | pub(crate) fn paint<'rp>(&'rp self, render_pass: &mut wgpu::RenderPass<'rp>) { 93 | render_pass.set_pipeline(&self.pipeline); 94 | for (_name, line) in self.lines_table.iter() { 95 | render_pass.set_vertex_buffer(0, line.vertex_buffer.slice(..)); 96 | render_pass.draw(0..line.vertex_data.len() as u32, 0..1); 97 | } 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/render_context.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | 3 | use eframe::{ 4 | egui_wgpu, 5 | epaint::mutex::RwLock, 6 | wgpu, 7 | }; 8 | 9 | use crate::{ 10 | prelude::*, 11 | types::SOPHUS_RENDER_MULTISAMPLE_COUNT, 12 | }; 13 | 14 | /// The render context 15 | #[derive(Clone)] 16 | pub struct RenderContext { 17 | /// state 18 | pub egui_wgpu_renderer: Arc>, 19 | /// device 20 | pub wgpu_device: Arc, 21 | /// queue 22 | pub wgpu_queue: Arc, 23 | /// adapter 24 | pub wgpu_adapter: Arc, 25 | } 26 | 27 | impl Debug for RenderContext { 28 | fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { 29 | f.debug_struct("RenderContext") 30 | .field("wgpu_device", &self.wgpu_device) 31 | .field("wgpu_queue", &self.wgpu_queue) 32 | .field("wgpu_adapter", &self.wgpu_adapter) 33 | .finish() 34 | } 35 | } 36 | 37 | impl RenderContext { 38 | /// Creates a render context from a egui_wgpu render state 39 | pub fn from_render_state(render_state: &egui_wgpu::RenderState) -> Self { 40 | RenderContext { 41 | wgpu_adapter: Arc::new(render_state.adapter.clone()), 42 | wgpu_device: Arc::new(render_state.device.clone()), 43 | wgpu_queue: Arc::new(render_state.queue.clone()), 44 | egui_wgpu_renderer: render_state.renderer.clone(), 45 | } 46 | } 47 | 48 | /// Creates a new render context, include wgpu device, adapter and queue 49 | /// as well as egui_wgpu renderer. 50 | pub async fn new() -> Self { 51 | let instance = wgpu::Instance::new(&wgpu::InstanceDescriptor { 52 | backends: wgpu::Backends::all(), 53 | ..Default::default() 54 | }); 55 | let adapter = instance 56 | .request_adapter(&wgpu::RequestAdapterOptions { 57 | power_preference: wgpu::PowerPreference::default(), 58 | compatible_surface: None, 59 | force_fallback_adapter: false, 60 | }) 61 | .await 62 | .unwrap(); 63 | let (device, queue) = adapter 64 | .request_device(&Default::default(), None) 65 | .await 66 | .unwrap(); 67 | 68 | const DITHERING: bool = false; 69 | 70 | let renderer = egui_wgpu::Renderer::new( 71 | &device, 72 | wgpu::TextureFormat::Rgba8Unorm, 73 | None, 74 | SOPHUS_RENDER_MULTISAMPLE_COUNT, 75 | DITHERING, 76 | ); 77 | 78 | RenderContext { 79 | egui_wgpu_renderer: Arc::new(RwLock::new(renderer)), 80 | wgpu_device: device.into(), 81 | wgpu_queue: queue.into(), 82 | wgpu_adapter: adapter.into(), 83 | } 84 | } 85 | 86 | /// Creates a render context from an eframe creation context. 87 | pub fn from_egui_cc(cc: &eframe::CreationContext) -> Self { 88 | RenderContext::from_render_state(cc.wgpu_render_state.as_ref().unwrap()) 89 | } 90 | } 91 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/renderables.rs: -------------------------------------------------------------------------------- 1 | mod color; 2 | mod frame; 3 | mod pixel_renderable; 4 | mod scene_renderable; 5 | 6 | pub use color::*; 7 | pub use frame::*; 8 | pub use pixel_renderable::*; 9 | pub use scene_renderable::*; 10 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/renderables/frame.rs: -------------------------------------------------------------------------------- 1 | use sophus_image::{ 2 | ArcImage4U8, 3 | ImageSize, 4 | }; 5 | use sophus_sensor::DynCameraF64; 6 | 7 | use crate::{ 8 | camera::{ 9 | ClippingPlanes, 10 | RenderCameraProperties, 11 | RenderIntrinsics, 12 | }, 13 | prelude::*, 14 | }; 15 | 16 | /// Frame to hold content 17 | /// 18 | /// Invariants: 19 | /// - The image, if available, must have the same size as the intrinsics. 20 | /// - The image size must be non-zero. 21 | #[derive(Clone, Debug)] 22 | pub struct ImageFrame { 23 | /// Image, if available must have the same size as the intrinsics 24 | pub image: Option, 25 | /// Intrinsics 26 | pub camera_properties: RenderCameraProperties, 27 | } 28 | 29 | impl ImageFrame { 30 | /// Try to create a new frame from an image and camera properties d 31 | /// 32 | /// Returns None if the image size is zero or the image size does not match the intrinsics. 33 | pub fn try_from( 34 | image: &ArcImage4U8, 35 | camera_properties: &RenderCameraProperties, 36 | ) -> Option { 37 | if camera_properties.intrinsics.image_size().area() == 0 { 38 | return None; 39 | } 40 | if image.image_size() != camera_properties.intrinsics.image_size() { 41 | return None; 42 | } 43 | Some(ImageFrame { 44 | image: Some(image.clone()), 45 | camera_properties: camera_properties.clone(), 46 | }) 47 | } 48 | 49 | /// Create a new frame from an image 50 | /// 51 | /// Precondition: The image size must be non-zero. 52 | pub fn from_image(image: &ArcImage4U8) -> ImageFrame { 53 | assert!(image.image_size().area() > 0); 54 | 55 | let camera_properties = RenderCameraProperties::default_from(image.image_size()); 56 | 57 | ImageFrame { 58 | image: Some(image.clone()), 59 | camera_properties, 60 | } 61 | } 62 | 63 | /// Create a new frame from intrinsics 64 | /// 65 | /// Precondition: The image size must be non-zero. 66 | pub fn from_intrinsics(intrinsics: &DynCameraF64) -> ImageFrame { 67 | assert!(intrinsics.image_size().area() > 0); 68 | ImageFrame { 69 | image: None, 70 | camera_properties: RenderCameraProperties { 71 | intrinsics: RenderIntrinsics::new(intrinsics), 72 | clipping_planes: ClippingPlanes::default(), 73 | }, 74 | } 75 | } 76 | 77 | /// Create a new frame from camera properties 78 | pub fn from_camera_properties(camera_properties: &RenderCameraProperties) -> ImageFrame { 79 | ImageFrame { 80 | image: None, 81 | camera_properties: camera_properties.clone(), 82 | } 83 | } 84 | 85 | /// Create a new frame from image size 86 | /// 87 | /// Precondition: The image size must be non-zero. 88 | pub fn from_size(view_size: &ImageSize) -> ImageFrame { 89 | assert!(view_size.area() > 0); 90 | ImageFrame { 91 | image: None, 92 | camera_properties: RenderCameraProperties::default_from(*view_size), 93 | } 94 | } 95 | 96 | /// Camera properties 97 | pub fn camera_properties(&self) -> &RenderCameraProperties { 98 | &self.camera_properties 99 | } 100 | 101 | /// Image, if available 102 | pub fn maybe_image(&self) -> Option<&ArcImage4U8> { 103 | self.image.as_ref() 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/renderables/pixel_renderable.rs: -------------------------------------------------------------------------------- 1 | use sophus_autodiff::linalg::SVec; 2 | 3 | use crate::{ 4 | prelude::*, 5 | renderables::color::Color, 6 | }; 7 | 8 | /// pixel renderable 9 | #[derive(Clone, Debug)] 10 | pub enum PixelRenderable { 11 | /// 2D line segments 12 | Line(LineSegments2), 13 | /// 2D point cloud 14 | Point(PointCloud2), 15 | } 16 | 17 | /// named line segments 18 | pub fn named_line2(name: impl ToString, segments: Vec) -> PixelRenderable { 19 | PixelRenderable::Line(LineSegments2 { 20 | name: name.to_string(), 21 | segments, 22 | }) 23 | } 24 | 25 | /// named point cloud 26 | pub fn named_point2(name: impl ToString, points: Vec) -> PixelRenderable { 27 | PixelRenderable::Point(PointCloud2 { 28 | name: name.to_string(), 29 | points, 30 | }) 31 | } 32 | 33 | /// make lines 2d 34 | pub fn make_line2( 35 | name: impl ToString, 36 | arr: &[[impl HasToVec2F32; 2]], 37 | color: &Color, 38 | line_width: f32, 39 | ) -> PixelRenderable { 40 | let mut line_segments = LineSegments2 { 41 | name: name.to_string(), 42 | segments: vec![], 43 | }; 44 | 45 | for tuple in arr { 46 | line_segments.segments.push(LineSegment2 { 47 | p0: tuple[0].to_vec2(), 48 | p1: tuple[1].to_vec2(), 49 | color: *color, 50 | line_width, 51 | }); 52 | } 53 | 54 | PixelRenderable::Line(line_segments) 55 | } 56 | 57 | /// make 2d point cloud 58 | pub fn make_point2( 59 | name: impl ToString, 60 | arr: &[impl HasToVec2F32], 61 | color: &Color, 62 | point_size: f32, 63 | ) -> PixelRenderable { 64 | let mut cloud = PointCloud2 { 65 | name: name.to_string(), 66 | points: vec![], 67 | }; 68 | 69 | for p in arr { 70 | cloud.points.push(Point2 { 71 | p: p.to_vec2(), 72 | color: *color, 73 | point_size, 74 | }); 75 | } 76 | PixelRenderable::Point(cloud) 77 | } 78 | 79 | /// Can be converted to Vec2F32 80 | pub trait HasToVec2F32 { 81 | /// returns Vec2F32 82 | fn to_vec2(&self) -> SVec; 83 | } 84 | 85 | impl HasToVec2F32 for [f32; 2] { 86 | fn to_vec2(&self) -> SVec { 87 | SVec::::new(self[0], self[1]) 88 | } 89 | } 90 | 91 | impl HasToVec2F32 for &[f32; 2] { 92 | fn to_vec2(&self) -> SVec { 93 | SVec::::new(self[0], self[1]) 94 | } 95 | } 96 | 97 | impl HasToVec2F32 for SVec { 98 | fn to_vec2(&self) -> SVec { 99 | *self 100 | } 101 | } 102 | 103 | /// 2D line 104 | #[derive(Clone, Debug)] 105 | pub struct LineSegment2 { 106 | /// Start point 107 | pub p0: SVec, 108 | /// End point 109 | pub p1: SVec, 110 | /// Color 111 | pub color: Color, 112 | /// Line width 113 | pub line_width: f32, 114 | } 115 | 116 | /// 2D point 117 | #[derive(Clone, Debug)] 118 | pub struct Point2 { 119 | /// Point 120 | pub p: SVec, 121 | /// Color 122 | pub color: Color, 123 | /// Point size in pixels 124 | pub point_size: f32, 125 | } 126 | 127 | /// 2D line segments 128 | #[derive(Clone, Debug)] 129 | pub struct LineSegments2 { 130 | /// Name of the entity 131 | pub name: String, 132 | /// List of line segments 133 | pub segments: Vec, 134 | } 135 | 136 | /// 2D point cloud 137 | #[derive(Clone, Debug)] 138 | pub struct PointCloud2 { 139 | /// Name of the entity 140 | pub name: String, 141 | /// List of points 142 | pub points: Vec, 143 | } 144 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/shaders/pixel_line.wgsl: -------------------------------------------------------------------------------- 1 | @group(0) @binding(1) 2 | var zoom_2d: Zoom2d; 3 | 4 | @group(0) @binding(2) 5 | var ortho_camera: PinholeModel; 6 | 7 | struct VertexOut { 8 | @location(0) color: vec4, 9 | @builtin(position) position: vec4, 10 | }; 11 | 12 | @vertex 13 | fn vs_main( 14 | @location(0) position: vec2, 15 | @location(1) color: vec4, 16 | @location(2) normal: vec2, 17 | @location(3) line_width: f32, 18 | @builtin(vertex_index) idx: u32)-> VertexOut 19 | { 20 | var out: VertexOut; 21 | 22 | var line_half_width = 0.5 * line_width * ortho_camera.viewport_scale; 23 | var p = position; 24 | var mod6 = idx % 6u; 25 | if mod6 == 0u { 26 | // p0 27 | p += normal * line_half_width; 28 | } else if mod6 == 1u { 29 | // p0 30 | p -= normal * line_half_width; 31 | } else if mod6 == 2u { 32 | // p1 33 | p += normal * line_half_width; 34 | } else if mod6 == 3u { 35 | // p1 36 | p -= normal * line_half_width; 37 | } else if mod6 == 4u { 38 | // p1 39 | p += normal * line_half_width; 40 | } else if mod6 == 5u { 41 | // p0 42 | p -= normal * line_half_width; 43 | } 44 | 45 | out.position = ortho_pixel_and_z_to_clip(p, zoom_2d, ortho_camera); 46 | out.color = color; 47 | 48 | return out; 49 | } 50 | 51 | @fragment 52 | fn fs_main(in: VertexOut) -> @location(0) vec4 { 53 | return in.color; 54 | } 55 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/shaders/pixel_point.wgsl: -------------------------------------------------------------------------------- 1 | @group(0) @binding(1) 2 | var zoom_2d: Zoom2d; 3 | 4 | @group(0) @binding(2) 5 | var ortho_camera: PinholeModel; 6 | 7 | struct VertexOut { 8 | @location(0) color: vec4, 9 | @builtin(position) position: vec4, 10 | }; 11 | 12 | @vertex 13 | fn vs_main( 14 | @location(0) position: vec2, 15 | @location(1) point_size: f32, 16 | @location(2) color: vec4, 17 | @builtin(vertex_index) idx: u32)-> VertexOut 18 | { 19 | var out: VertexOut; 20 | var point_radius = 0.5 * point_size * ortho_camera.viewport_scale; 21 | 22 | var u = position.x; 23 | var v = position.y; 24 | var mod4 = idx % 6u; 25 | if mod4 == 0u { 26 | u -= point_radius; 27 | v -= point_radius; 28 | } else if mod4 == 1u || mod4 == 3u { 29 | u += point_radius; 30 | v -= point_radius; 31 | } else if mod4 == 2u || mod4 == 4u { 32 | u -= point_radius; 33 | v +=point_radius; 34 | } else if mod4 == 5u { 35 | u += point_radius; 36 | v += point_radius; 37 | } 38 | 39 | out.position = ortho_pixel_and_z_to_clip(vec2(u, v), zoom_2d, ortho_camera); 40 | out.color = color; 41 | 42 | return out; 43 | } 44 | 45 | @fragment 46 | fn fs_main(in: VertexOut) -> @location(0) vec4 { 47 | return in.color; 48 | } 49 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/shaders/scene_line.wgsl: -------------------------------------------------------------------------------- 1 | @group(0) @binding(0) 2 | var camera: CameraProperties; 3 | @group(0) @binding(1) 4 | var zoom: Zoom2d; 5 | @group(0) @binding(2) 6 | var pinhole: PinholeModel; 7 | @group(0) @binding(3) 8 | var view_uniform: CameraPose; 9 | 10 | 11 | struct VertexOut { 12 | @location(0) rgba: vec4, 13 | @builtin(position) position: vec4, 14 | }; 15 | 16 | 17 | @vertex 18 | fn vs_main( 19 | @location(0) p0: vec3, 20 | @location(1) p1: vec3, 21 | @location(2) color: vec4, 22 | @location(3) line_width: f32, 23 | @builtin(vertex_index) idx: u32)-> VertexOut 24 | { 25 | let projection0 = project_point(p0, view_uniform, pinhole, camera, zoom); 26 | let projection1 = project_point(p1, view_uniform, pinhole, camera, zoom); 27 | let depth0 = projection0.z; 28 | let depth1 = projection1.z; 29 | let uv0 = projection0.uv_undistorted; 30 | let uv1 = projection1.uv_undistorted; 31 | 32 | var d = normalize(uv0 - uv1); 33 | var n = vec2(-d.y, d.x); 34 | var line_half_width = 0.5 * line_width; 35 | var uv = vec2(0.0, 0.0); 36 | var z = 0.0; 37 | var mod6 = idx % 6u; 38 | if mod6 == 0u { 39 | uv = uv0 + line_half_width * n; 40 | z = depth0; 41 | } else if mod6 == 1u { 42 | uv = uv0 - line_half_width * n; 43 | z = depth0; 44 | } else if mod6 == 2u { 45 | uv = uv1 + line_half_width * n; 46 | z = depth1; 47 | } else if mod6 == 3u { 48 | uv = uv1 - line_half_width * n; 49 | z = depth1; 50 | } 51 | else if mod6 == 4u { 52 | uv = uv1 + line_half_width * n; 53 | z = depth1; 54 | } else if mod6 == 5u { 55 | uv = uv0 - line_half_width * n; 56 | z = depth0; 57 | } 58 | 59 | var out: VertexOut; 60 | out.position = pixel_and_z_to_clip(uv, z, camera, zoom); 61 | out.rgba = color; 62 | return out; 63 | } 64 | 65 | @fragment 66 | fn fs_main(in: VertexOut) -> @location(0) vec4 { 67 | return in.rgba; 68 | } 69 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/shaders/scene_mesh.wgsl: -------------------------------------------------------------------------------- 1 | @group(0) @binding(0) 2 | var camera: CameraProperties; 3 | @group(0) @binding(1) 4 | var zoom: Zoom2d; 5 | @group(0) @binding(2) 6 | var pinhole: PinholeModel; 7 | @group(0) @binding(3) 8 | var view_uniform: CameraPose; 9 | 10 | struct VertexOut { 11 | @location(0) rgba: vec4, 12 | @builtin(position) position: vec4, 13 | }; 14 | 15 | @vertex 16 | fn vs_main( 17 | @location(0) position: vec3, 18 | @location(1) normal: vec3, 19 | @location(2) color: vec4)-> VertexOut 20 | { 21 | let projection = project_point(position, view_uniform, pinhole, camera, zoom); 22 | var out: VertexOut; 23 | out.position = pixel_and_z_to_clip(projection.uv_undistorted, projection.z, camera, zoom); 24 | out.rgba = color; 25 | return out; 26 | } 27 | 28 | @fragment 29 | fn fs_main(frag: VertexOut) -> @location(0) vec4 { 30 | return frag.rgba; 31 | } 32 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/shaders/scene_point.wgsl: -------------------------------------------------------------------------------- 1 | @group(0) @binding(0) 2 | var camera: CameraProperties; 3 | @group(0) @binding(1) 4 | var zoom: Zoom2d; 5 | @group(0) @binding(2) 6 | var pinhole: PinholeModel; 7 | @group(0) @binding(3) 8 | var view_uniform: CameraPose; 9 | 10 | 11 | struct VertexOut { 12 | @location(0) rgbd: vec4, 13 | @builtin(position) position: vec4, 14 | }; 15 | 16 | @vertex 17 | fn vs_main( 18 | @location(0) position: vec3, 19 | @location(1) point_size: f32, 20 | @location(2) color: vec4, 21 | @builtin(vertex_index) idx: u32)-> VertexOut 22 | { 23 | let projection = project_point(position, view_uniform, pinhole, camera, zoom); 24 | var u = projection.uv_undistorted.x; 25 | var v = projection.uv_undistorted.y; 26 | 27 | var point_radius = 0.5 * point_size; 28 | var mod4 = idx % 6u; 29 | if mod4 == 0u { 30 | u -= point_radius; 31 | v -= point_radius; 32 | } else if mod4 == 1u || mod4 == 3u { 33 | u += point_radius; 34 | v -= point_radius; 35 | } else if mod4 == 2u || mod4 == 4u { 36 | u -= point_radius; 37 | v += point_radius; 38 | } else if mod4 == 5u { 39 | u += point_radius; 40 | v += point_radius; 41 | } 42 | var out: VertexOut; 43 | out.position = pixel_and_z_to_clip(vec2(u, v), projection.z, camera, zoom); 44 | out.rgbd = color; 45 | return out; 46 | } 47 | 48 | @fragment 49 | fn fs_main(in: VertexOut) -> @location(0) vec4 { 50 | return in.rgbd; 51 | } 52 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/shaders/scene_textured_mesh.wgsl: -------------------------------------------------------------------------------- 1 | @group(0) @binding(0) 2 | var camera: CameraProperties; 3 | @group(0) @binding(1) 4 | var zoom: Zoom2d; 5 | @group(0) @binding(2) 6 | var pinhole: PinholeModel; 7 | @group(0) @binding(3) 8 | var view_uniform: CameraPose; 9 | 10 | 11 | struct VertexOut { 12 | @location(0) texCoords: vec2, 13 | @builtin(position) position: vec4, 14 | }; 15 | 16 | 17 | @group(1) @binding(0) 18 | var mesh_texture: texture_2d; 19 | 20 | @group(1) @binding(1) 21 | var mesh_texture_sampler: sampler; 22 | 23 | 24 | 25 | @vertex 26 | fn vs_main( 27 | @location(0) position: vec3, 28 | @location(1) tex_coords: vec2 29 | ) -> VertexOut { 30 | let projection = project_point(position, view_uniform, pinhole, camera, zoom); 31 | 32 | var out: VertexOut; 33 | out.position = pixel_and_z_to_clip(projection.uv_undistorted, projection.z, camera, zoom); 34 | out.texCoords = tex_coords; 35 | return out; 36 | } 37 | 38 | @fragment 39 | fn fs_main(in: VertexOut) -> @location(0) vec4 { 40 | return textureSample(mesh_texture, mesh_texture_sampler, in.texCoords); 41 | } 42 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/textures.rs: -------------------------------------------------------------------------------- 1 | mod depth; 2 | mod depth_image; 3 | mod ndc_z_buffer; 4 | mod rgba; 5 | mod visual_depth; 6 | 7 | pub(crate) use depth::DepthTextures; 8 | pub use depth_image::*; 9 | pub use rgba::*; 10 | use sophus_image::ImageSize; 11 | 12 | use crate::RenderContext; 13 | 14 | #[derive(Debug)] 15 | pub(crate) struct Textures { 16 | pub(crate) view_port_size: ImageSize, 17 | pub(crate) rgbd: RgbdTexture, 18 | pub depth: DepthTextures, 19 | } 20 | 21 | impl Textures { 22 | pub(crate) fn new(render_state: &RenderContext, view_port_size: &ImageSize) -> Self { 23 | Self { 24 | view_port_size: *view_port_size, 25 | rgbd: RgbdTexture::new(render_state, view_port_size), 26 | depth: DepthTextures::new(render_state, view_port_size), 27 | } 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/textures/depth_image.rs: -------------------------------------------------------------------------------- 1 | use eframe::egui::mutex::Mutex; 2 | use sophus_autodiff::linalg::SVec; 3 | use sophus_image::{ 4 | ArcImage4U8, 5 | ArcImageF32, 6 | MutImage4U8, 7 | MutImageF32, 8 | color_map::BlueWhiteRedBlackColorMap, 9 | prelude::*, 10 | }; 11 | 12 | use crate::camera::ClippingPlanesF32; 13 | 14 | /// depth image 15 | pub struct DepthImage { 16 | /// ndc depth values 17 | pub ndc_z_image: ArcImageF32, 18 | /// clipping planes 19 | pub clipping_planes: ClippingPlanesF32, 20 | /// color mapped depth image cache 21 | color_mapped_cache: Mutex>, 22 | /// metric depth image cache 23 | metric_depth_cache: Mutex>, 24 | } 25 | 26 | /// ndc z to color 27 | pub fn ndc_z_to_color(ndc_z: f32) -> SVec { 28 | if ndc_z == 1.0 { 29 | // map background to pitch black 30 | return SVec::::new(0, 0, 0, 255); 31 | } 32 | // scale to [0.0 - 0.9] range, so that far away [dark red] differs 33 | // from background [pitch black]. 34 | let z = 0.9 * ndc_z; 35 | // z is squared to get higher dynamic range for far objects. 36 | let rgb = BlueWhiteRedBlackColorMap::f32_to_rgb(z * z); 37 | SVec::::new(rgb[0], rgb[1], rgb[2], 255) 38 | } 39 | 40 | impl DepthImage { 41 | /// new depth image 42 | pub fn new(ndc_z_image: ArcImageF32, clipping_planes: ClippingPlanesF32) -> Self { 43 | DepthImage { 44 | ndc_z_image, 45 | clipping_planes, 46 | color_mapped_cache: Mutex::new(None), 47 | metric_depth_cache: Mutex::new(None), 48 | } 49 | } 50 | 51 | /// return color mapped depth 52 | pub fn color_mapped(&self) -> ArcImage4U8 { 53 | let mut cached_image = self.color_mapped_cache.lock(); 54 | 55 | match cached_image.as_mut() { 56 | Some(cached_image) => cached_image.clone(), 57 | None => { 58 | let mut image_rgba = MutImage4U8::from_image_size(self.ndc_z_image.image_size()); 59 | 60 | for v in 0..image_rgba.image_size().height { 61 | for u in 0..image_rgba.image_size().width { 62 | let z = self.ndc_z_image.pixel(u, v); 63 | *image_rgba.mut_pixel(u, v) = ndc_z_to_color(z); 64 | } 65 | } 66 | let shared = ArcImage4U8::from(image_rgba); 67 | *cached_image = Some(shared.clone()); 68 | shared 69 | } 70 | } 71 | } 72 | 73 | /// return metric depth image 74 | pub fn metric_depth(&self) -> ArcImageF32 { 75 | let mut cached_image = self.metric_depth_cache.lock(); 76 | 77 | match cached_image.as_mut() { 78 | Some(cached_image) => cached_image.clone(), 79 | None => { 80 | let mut depth = MutImageF32::from_image_size(self.ndc_z_image.image_size()); 81 | for v in 0..depth.image_size().height { 82 | for u in 0..depth.image_size().width { 83 | let z = self.ndc_z_image.pixel(u, v); 84 | 85 | *depth.mut_pixel(u, v) = self.clipping_planes.metric_z_from_ndc_z(z); 86 | } 87 | } 88 | let shared = ArcImageF32::from(depth); 89 | *cached_image = Some(shared.clone()); 90 | shared 91 | } 92 | } 93 | } 94 | } 95 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/textures/ndc_z_buffer.rs: -------------------------------------------------------------------------------- 1 | use eframe::wgpu; 2 | use sophus_image::ImageSize; 3 | 4 | use crate::{ 5 | RenderContext, 6 | types::SOPHUS_RENDER_MULTISAMPLE_COUNT, 7 | }; 8 | 9 | #[derive(Debug)] 10 | pub(crate) struct NdcZBuffer { 11 | pub(crate) _multisample_texture: wgpu::Texture, 12 | pub(crate) multisample_texture_view: wgpu::TextureView, 13 | 14 | pub(crate) final_texture: wgpu::Texture, 15 | pub(crate) final_texture_view: wgpu::TextureView, 16 | } 17 | 18 | impl NdcZBuffer { 19 | pub(crate) fn new(render_state: &RenderContext, view_port_size: &ImageSize) -> Self { 20 | let size = wgpu::Extent3d { 21 | width: view_port_size.width as u32, 22 | height: view_port_size.height as u32, 23 | depth_or_array_layers: 1, 24 | }; 25 | let desc = wgpu::TextureDescriptor { 26 | label: Some("ndc depth multisample texture"), 27 | size, 28 | mip_level_count: 1, 29 | sample_count: SOPHUS_RENDER_MULTISAMPLE_COUNT, 30 | dimension: wgpu::TextureDimension::D2, 31 | format: wgpu::TextureFormat::Depth32Float, 32 | usage: wgpu::TextureUsages::RENDER_ATTACHMENT 33 | | wgpu::TextureUsages::COPY_SRC 34 | | wgpu::TextureUsages::TEXTURE_BINDING, 35 | view_formats: &[], 36 | }; 37 | let multisample_texture = render_state.wgpu_device.create_texture(&desc); 38 | let multisample_texture_view = 39 | multisample_texture.create_view(&wgpu::TextureViewDescriptor::default()); 40 | 41 | let desc = wgpu::TextureDescriptor { 42 | label: Some("depth final texture"), 43 | size, 44 | mip_level_count: 1, 45 | sample_count: 1, 46 | dimension: wgpu::TextureDimension::D2, 47 | format: wgpu::TextureFormat::R32Float, 48 | usage: wgpu::TextureUsages::RENDER_ATTACHMENT 49 | | wgpu::TextureUsages::COPY_SRC 50 | | wgpu::TextureUsages::TEXTURE_BINDING 51 | | wgpu::TextureUsages::STORAGE_BINDING, 52 | view_formats: &[], 53 | }; 54 | let final_texture = render_state.wgpu_device.create_texture(&desc); 55 | let final_texture_view = final_texture.create_view(&wgpu::TextureViewDescriptor::default()); 56 | NdcZBuffer { 57 | _multisample_texture: multisample_texture, 58 | multisample_texture_view, 59 | final_texture, 60 | final_texture_view, 61 | } 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/textures/visual_depth.rs: -------------------------------------------------------------------------------- 1 | use eframe::{ 2 | egui, 3 | wgpu, 4 | }; 5 | use sophus_image::ImageSize; 6 | 7 | use crate::RenderContext; 8 | 9 | #[derive(Debug)] 10 | pub(crate) struct VisualDepthTexture { 11 | pub visual_texture: wgpu::Texture, 12 | pub(crate) egui_tex_id: egui::TextureId, 13 | pub(crate) render_state: RenderContext, 14 | } 15 | 16 | impl Drop for VisualDepthTexture { 17 | fn drop(&mut self) { 18 | self.render_state 19 | .egui_wgpu_renderer 20 | .write() 21 | .free_texture(&self.egui_tex_id); 22 | } 23 | } 24 | 25 | impl VisualDepthTexture { 26 | /// Create a new depth renderer. 27 | pub fn new(render_state: &RenderContext, view_port_size: &ImageSize) -> Self { 28 | let w = view_port_size.width as u32; 29 | let h = view_port_size.height as u32; 30 | 31 | let visual_texture = render_state 32 | .wgpu_device 33 | .create_texture(&wgpu::TextureDescriptor { 34 | label: None, 35 | size: wgpu::Extent3d { 36 | width: w, 37 | height: h, 38 | depth_or_array_layers: 1, 39 | }, 40 | mip_level_count: 1, 41 | sample_count: 1, 42 | dimension: wgpu::TextureDimension::D2, 43 | format: wgpu::TextureFormat::Rgba8UnormSrgb, 44 | usage: wgpu::TextureUsages::COPY_DST | wgpu::TextureUsages::TEXTURE_BINDING, 45 | view_formats: &[wgpu::TextureFormat::Rgba8UnormSrgb], 46 | }); 47 | 48 | let visual_texture_view = 49 | visual_texture.create_view(&wgpu::TextureViewDescriptor::default()); 50 | let egui_tex_id = render_state 51 | .egui_wgpu_renderer 52 | .write() 53 | .register_native_texture( 54 | render_state.wgpu_device.as_ref(), 55 | &visual_texture_view, 56 | wgpu::FilterMode::Linear, 57 | ); 58 | Self { 59 | visual_texture, 60 | egui_tex_id, 61 | render_state: render_state.clone(), 62 | } 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /crates/sophus_renderer/src/types.rs: -------------------------------------------------------------------------------- 1 | use eframe::egui; 2 | use sophus_autodiff::linalg::VecF64; 3 | use sophus_image::ArcImage4U8; 4 | 5 | use crate::{ 6 | offscreen_renderer::OffscreenRenderer, 7 | renderables::Color, 8 | textures::DepthImage, 9 | }; 10 | 11 | /// Render result 12 | pub struct RenderResult { 13 | /// rgba image 14 | pub rgba_image: Option, 15 | 16 | /// rgba egui texture id 17 | pub rgba_egui_tex_id: egui::TextureId, 18 | 19 | /// depth image 20 | pub depth_image: DepthImage, 21 | 22 | /// depth egui texture id 23 | pub depth_egui_tex_id: egui::TextureId, 24 | } 25 | 26 | /// aspect ratio 27 | pub trait HasAspectRatio { 28 | /// return aspect ratio 29 | fn aspect_ratio(&self) -> f32; 30 | } 31 | 32 | impl HasAspectRatio for OffscreenRenderer { 33 | fn aspect_ratio(&self) -> f32 { 34 | self.camera_properties 35 | .intrinsics 36 | .image_size() 37 | .aspect_ratio() 38 | } 39 | } 40 | 41 | #[repr(C)] 42 | #[derive(Debug, Copy, Clone, bytemuck::Pod, bytemuck::Zeroable)] 43 | pub(crate) struct Zoom2dPod { 44 | pub(crate) translation_x: f32, 45 | pub(crate) translation_y: f32, 46 | pub(crate) scaling_x: f32, 47 | pub(crate) scaling_y: f32, 48 | } 49 | 50 | impl Default for Zoom2dPod { 51 | fn default() -> Self { 52 | Zoom2dPod { 53 | translation_x: 0.0, 54 | translation_y: 0.0, 55 | scaling_x: 1.0, 56 | scaling_y: 1.0, 57 | } 58 | } 59 | } 60 | 61 | /// Translation and scaling 62 | /// 63 | /// todo: move to sophus_lie 64 | #[derive(Clone, Copy, Debug)] 65 | pub struct TranslationAndScaling { 66 | /// translation 67 | pub translation: VecF64<2>, 68 | /// scaling 69 | pub scaling: VecF64<2>, 70 | } 71 | 72 | impl TranslationAndScaling { 73 | /// identity 74 | pub fn identity() -> Self { 75 | TranslationAndScaling { 76 | translation: VecF64::<2>::zeros(), 77 | scaling: VecF64::<2>::new(1.0, 1.0), 78 | } 79 | } 80 | 81 | /// apply translation and scaling 82 | pub fn apply(&self, xy: VecF64<2>) -> VecF64<2> { 83 | VecF64::<2>::new( 84 | xy[0] * self.scaling[0] + self.translation[0], 85 | xy[1] * self.scaling[1] + self.translation[1], 86 | ) 87 | } 88 | } 89 | 90 | /// focus point to overlay 91 | pub struct SceneFocusMarker { 92 | /// color 93 | pub color: Color, 94 | /// u viewport pixel 95 | pub u: f32, 96 | /// v viewport pixel 97 | pub v: f32, 98 | /// ndc_z 99 | pub ndc_z: f32, 100 | } 101 | 102 | /// multisample count 103 | pub const SOPHUS_RENDER_MULTISAMPLE_COUNT: u32 = 4; 104 | -------------------------------------------------------------------------------- /crates/sophus_sensor/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Camera models for computer vision" 3 | name = "sophus_sensor" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_geo.workspace = true 16 | sophus_image.workspace = true 17 | 18 | approx.workspace = true 19 | log.workspace = true 20 | nalgebra.workspace = true 21 | ndarray.workspace = true 22 | num-traits.workspace = true 23 | 24 | [build-dependencies] 25 | rustc_version.workspace = true 26 | 27 | [features] 28 | simd = ["sophus_autodiff/simd", "sophus_image/simd"] 29 | std = [ 30 | "sophus_autodiff/std", 31 | "sophus_geo/std", 32 | "sophus_image/std", 33 | ] 34 | default = ["std"] 35 | -------------------------------------------------------------------------------- /crates/sophus_sensor/README.md: -------------------------------------------------------------------------------- 1 | ## Camera models for computer vision 2 | 3 | ## Integration with sophus-rs 4 | 5 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 6 | It re-exports the relevant prelude types under [prelude], so you can 7 | seamlessly interoperate with the rest of the sophus-rs types. 8 | -------------------------------------------------------------------------------- /crates/sophus_sensor/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_sensor/src/camera_enum.rs: -------------------------------------------------------------------------------- 1 | mod general_camera; 2 | mod perspective_camera; 3 | 4 | pub use crate::camera_enum::{ 5 | general_camera::*, 6 | perspective_camera::*, 7 | }; 8 | -------------------------------------------------------------------------------- /crates/sophus_sensor/src/distortions.rs: -------------------------------------------------------------------------------- 1 | mod affine; 2 | mod brown_conrady; 3 | mod enhanced_unified; 4 | mod kannala_brandt; 5 | 6 | pub use affine::*; 7 | pub use brown_conrady::*; 8 | pub use enhanced_unified::*; 9 | pub use kannala_brandt::*; 10 | -------------------------------------------------------------------------------- /crates/sophus_sensor/src/distortions/affine.rs: -------------------------------------------------------------------------------- 1 | use core::{ 2 | borrow::Borrow, 3 | marker::PhantomData, 4 | }; 5 | 6 | use sophus_autodiff::params::IsParamsImpl; 7 | 8 | use crate::{ 9 | prelude::*, 10 | traits::IsCameraDistortionImpl, 11 | }; 12 | 13 | extern crate alloc; 14 | 15 | /// Affine "distortion" implementation 16 | /// 17 | /// This is not a distortion in the traditional sense, but rather a simple affine transformation. 18 | #[derive(Debug, Clone, Copy)] 19 | pub struct AffineDistortionImpl< 20 | S: IsScalar, 21 | const BATCH: usize, 22 | const DM: usize, 23 | const DN: usize, 24 | > { 25 | phantom: PhantomData, 26 | } 27 | 28 | impl, const BATCH: usize, const DM: usize, const DN: usize> 29 | IsParamsImpl for AffineDistortionImpl 30 | { 31 | fn are_params_valid(_params: S::Vector<4>) -> S::Mask { 32 | S::Mask::all_true() 33 | } 34 | 35 | fn params_examples() -> alloc::vec::Vec> { 36 | alloc::vec![S::Vector::<4>::from_f64_array([1.0, 1.0, 0.0, 0.0])] 37 | } 38 | 39 | fn invalid_params_examples() -> alloc::vec::Vec> { 40 | alloc::vec![ 41 | S::Vector::<4>::from_f64_array([0.0, 1.0, 0.0, 0.0]), 42 | S::Vector::<4>::from_f64_array([1.0, 0.0, 0.0, 0.0]), 43 | ] 44 | } 45 | } 46 | 47 | impl, const BATCH: usize, const DM: usize, const DN: usize> 48 | IsCameraDistortionImpl for AffineDistortionImpl 49 | { 50 | fn distort(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2> 51 | where 52 | PA: Borrow>, 53 | PO: Borrow>, 54 | { 55 | let params = params.borrow(); 56 | let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); 57 | S::Vector::<2>::from_array([ 58 | proj_point_in_camera_z1_plane.elem(0) * params.elem(0) + params.elem(2), 59 | proj_point_in_camera_z1_plane.elem(1) * params.elem(1) + params.elem(3), 60 | ]) 61 | } 62 | 63 | fn undistort(params: PA, distorted_point: PO) -> S::Vector<2> 64 | where 65 | PA: Borrow>, 66 | PO: Borrow>, 67 | { 68 | let params = params.borrow(); 69 | let distorted_point = distorted_point.borrow(); 70 | 71 | S::Vector::<2>::from_array([ 72 | (distorted_point.elem(0) - params.elem(2)) / params.elem(0), 73 | (distorted_point.elem(1) - params.elem(3)) / params.elem(1), 74 | ]) 75 | } 76 | 77 | fn dx_distort_x(params: PA, _proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2> 78 | where 79 | PA: Borrow>, 80 | PO: Borrow>, 81 | { 82 | let params = params.borrow(); 83 | 84 | S::Matrix::<2, 2>::from_array2([[params.elem(0), S::zeros()], [S::zeros(), params.elem(1)]]) 85 | } 86 | 87 | fn dx_distort_params(_params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 4> 88 | where 89 | PA: Borrow>, 90 | PO: Borrow>, 91 | { 92 | let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); 93 | 94 | S::Matrix::<2, 4>::from_array2([ 95 | [ 96 | proj_point_in_camera_z1_plane.elem(0), 97 | S::zeros(), 98 | S::ones(), 99 | S::zeros(), 100 | ], 101 | [ 102 | S::zeros(), 103 | proj_point_in_camera_z1_plane.elem(1), 104 | S::zeros(), 105 | S::ones(), 106 | ], 107 | ]) 108 | } 109 | } 110 | -------------------------------------------------------------------------------- /crates/sophus_sensor/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![cfg_attr(feature = "simd", feature(portable_simd))] 2 | #![deny(missing_docs)] 3 | #![no_std] 4 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 5 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 6 | 7 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 8 | #[cfg(doctest)] 9 | pub struct ReadmeDoctests; 10 | 11 | #[cfg(feature = "std")] 12 | extern crate std; 13 | 14 | /// Distortion models 15 | pub mod distortions; 16 | /// Projection models 17 | pub mod projections; 18 | /// Sensor traits 19 | pub mod traits; 20 | /// sophus_sensor prelude. 21 | /// 22 | /// It is recommended to import this prelude when working with `sophus_sensor types: 23 | /// 24 | /// ``` 25 | /// use sophus_sensor::prelude::*; 26 | /// ``` 27 | /// 28 | /// or 29 | /// 30 | /// ```ignore 31 | /// use sophus::prelude::*; 32 | /// ``` 33 | /// 34 | /// to import all preludes when using the `sophus` umbrella crate. 35 | pub mod prelude { 36 | pub use sophus_autodiff::prelude::*; 37 | pub use sophus_geo::prelude::*; 38 | pub use sophus_image::prelude::*; 39 | 40 | pub use crate::traits::{ 41 | IsCamera, 42 | IsPerspectiveCamera, 43 | IsProjection, 44 | }; 45 | } 46 | 47 | mod camera; 48 | mod camera_enum; 49 | mod distortion_table; 50 | mod dyn_camera; 51 | 52 | pub use crate::{ 53 | camera::*, 54 | camera_enum::{ 55 | BrownConradyCamera, 56 | KannalaBrandtCamera, 57 | PinholeCamera, 58 | *, 59 | }, 60 | distortion_table::*, 61 | dyn_camera::*, 62 | }; 63 | -------------------------------------------------------------------------------- /crates/sophus_sensor/src/projections.rs: -------------------------------------------------------------------------------- 1 | mod orthographic; 2 | mod perspective; 3 | 4 | pub use orthographic::*; 5 | pub use perspective::*; 6 | -------------------------------------------------------------------------------- /crates/sophus_sensor/src/projections/orthographic.rs: -------------------------------------------------------------------------------- 1 | use core::{ 2 | borrow::Borrow, 3 | marker::PhantomData, 4 | }; 5 | 6 | use sophus_autodiff::linalg::{ 7 | IsScalar, 8 | IsVector, 9 | }; 10 | 11 | use crate::{ 12 | camera::Camera, 13 | distortions::AffineDistortionImpl, 14 | traits::IsProjection, 15 | }; 16 | 17 | /// Orthographic projection implementation 18 | #[derive(Debug, Clone)] 19 | pub struct OrthographisProjectionImpl< 20 | S: IsScalar, 21 | const BATCH: usize, 22 | const DM: usize, 23 | const DN: usize, 24 | > { 25 | phantom: PhantomData, 26 | } 27 | 28 | impl, const BATCH: usize, const DM: usize, const DN: usize> 29 | IsProjection for OrthographisProjectionImpl 30 | { 31 | fn proj

(point_in_camera: P) -> S::Vector<2> 32 | where 33 | P: Borrow>, 34 | { 35 | point_in_camera.borrow().get_fixed_subvec::<2>(0) 36 | } 37 | 38 | fn unproj

(point_in_camera: P, extension: S) -> S::Vector<3> 39 | where 40 | P: Borrow>, 41 | { 42 | let point_in_camera = point_in_camera.borrow(); 43 | S::Vector::<3>::from_array([point_in_camera.elem(0), point_in_camera.elem(1), extension]) 44 | } 45 | 46 | fn dx_proj_x

(_point_in_camera: P) -> S::Matrix<2, 3> 47 | where 48 | P: Borrow>, 49 | { 50 | unimplemented!("dx_proj_x not implemented for ProjectionOrtho") 51 | } 52 | } 53 | 54 | /// Orthographic camera 55 | pub type OrthographicCamera = Camera< 56 | S, 57 | 0, 58 | 4, 59 | BATCH, 60 | DM, 61 | DN, 62 | AffineDistortionImpl, 63 | OrthographisProjectionImpl, 64 | >; 65 | -------------------------------------------------------------------------------- /crates/sophus_sensor/src/projections/perspective.rs: -------------------------------------------------------------------------------- 1 | use core::borrow::Borrow; 2 | 3 | use sophus_autodiff::{ 4 | linalg::{ 5 | IsMatrix, 6 | IsScalar, 7 | IsVector, 8 | }, 9 | prelude::IsSingleScalar, 10 | }; 11 | 12 | use crate::traits::IsProjection; 13 | 14 | /// Perspective camera projection - using z=1 plane 15 | /// 16 | /// Projects a 3D point in the camera frame to a 2D point in the z=1 plane 17 | #[derive(Debug, Clone, Copy)] 18 | pub struct PerspectiveProjectionImpl< 19 | S: IsScalar, 20 | const BATCH: usize, 21 | const DM: usize, 22 | const DN: usize, 23 | > { 24 | phantom: core::marker::PhantomData, 25 | } 26 | 27 | impl, const BATCH: usize, const DM: usize, const DN: usize> 28 | IsProjection for PerspectiveProjectionImpl 29 | { 30 | fn proj

(point_in_camera: P) -> S::Vector<2> 31 | where 32 | P: Borrow>, 33 | { 34 | let point_in_camera = point_in_camera.borrow(); 35 | S::Vector::<2>::from_array([ 36 | point_in_camera.elem(0) / point_in_camera.elem(2), 37 | point_in_camera.elem(1) / point_in_camera.elem(2), 38 | ]) 39 | } 40 | 41 | fn unproj

(point_in_camera: P, extension: S) -> S::Vector<3> 42 | where 43 | P: Borrow>, 44 | { 45 | let point_in_camera = point_in_camera.borrow(); 46 | 47 | S::Vector::<3>::from_array([ 48 | point_in_camera.elem(0) * extension, 49 | point_in_camera.elem(1) * extension, 50 | extension, 51 | ]) 52 | } 53 | 54 | fn dx_proj_x

(point_in_camera: P) -> S::Matrix<2, 3> 55 | where 56 | P: Borrow>, 57 | { 58 | let point_in_camera = point_in_camera.borrow(); 59 | 60 | S::Matrix::<2, 3>::from_array2([ 61 | [ 62 | S::ones() / point_in_camera.elem(2), 63 | S::zeros(), 64 | -point_in_camera.elem(0) / (point_in_camera.elem(2) * point_in_camera.elem(2)), 65 | ], 66 | [ 67 | S::zeros(), 68 | S::ones() / point_in_camera.elem(2), 69 | -point_in_camera.elem(1) / (point_in_camera.elem(2) * point_in_camera.elem(2)), 70 | ], 71 | ]) 72 | } 73 | } 74 | 75 | /// Perspective projection for single scalar 76 | pub fn perspect_proj< 77 | S: IsSingleScalar + 'static + Send + Sync, 78 | const DM: usize, 79 | const DN: usize, 80 | >( 81 | point_in_camera: &S::Vector<3>, 82 | ) -> S::Vector<2> { 83 | PerspectiveProjectionImpl::::proj(point_in_camera) 84 | } 85 | -------------------------------------------------------------------------------- /crates/sophus_sim/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Basic camera simulation" 3 | name = "sophus_sim" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_image.workspace = true 16 | sophus_lie.workspace = true 17 | sophus_opt.workspace = true 18 | sophus_renderer.workspace = true 19 | sophus_sensor.workspace = true 20 | 21 | approx.workspace = true 22 | bytemuck.workspace = true 23 | eframe.workspace = true 24 | egui_extras.workspace = true 25 | env_logger.workspace = true 26 | linked-hash-map.workspace = true 27 | num-traits.workspace = true 28 | 29 | [build-dependencies] 30 | rustc_version.workspace = true 31 | 32 | [features] 33 | std = [ 34 | "sophus_autodiff/std", 35 | "sophus_image/std", 36 | "sophus_lie/std", 37 | "sophus_opt/std", 38 | "sophus_renderer/std", 39 | "sophus_sensor/std", 40 | ] 41 | default = ["std"] 42 | -------------------------------------------------------------------------------- /crates/sophus_sim/README.md: -------------------------------------------------------------------------------- 1 | ## Basic camera simulation 2 | 3 | ## Integration with sophus-rs 4 | 5 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 6 | It re-exports the relevant prelude types under [prelude], so you can 7 | seamlessly interoperate with the rest of the sophus-rs types. 8 | -------------------------------------------------------------------------------- /crates/sophus_sim/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_sim/src/camera_simulator.rs: -------------------------------------------------------------------------------- 1 | use alloc::vec::Vec; 2 | 3 | use sophus_image::ArcImage4U8; 4 | use sophus_lie::Isometry3F64; 5 | use sophus_renderer::{ 6 | OffscreenRenderer, 7 | RenderContext, 8 | camera::RenderCameraProperties, 9 | renderables::SceneRenderable, 10 | textures::DepthImage, 11 | }; 12 | 13 | extern crate alloc; 14 | 15 | /// camera simulator 16 | pub struct CameraSimulator { 17 | renderer: OffscreenRenderer, 18 | } 19 | 20 | /// Simulated image 21 | pub struct SimulatedImage { 22 | /// rgba 23 | pub rgba_image: ArcImage4U8, 24 | /// depth 25 | pub depth_image: DepthImage, 26 | } 27 | 28 | impl CameraSimulator { 29 | /// new simulator from context and camera intrinsics 30 | pub fn new(render_state: &RenderContext, camera_properties: &RenderCameraProperties) -> Self { 31 | CameraSimulator { 32 | renderer: OffscreenRenderer::new(render_state, camera_properties), 33 | } 34 | } 35 | 36 | /// update scene renderables 37 | pub fn update_3d_renderables(&mut self, renderables: Vec) { 38 | self.renderer.update_scene(renderables); 39 | } 40 | 41 | /// render 42 | pub fn render(&mut self, scene_from_camera: Isometry3F64) -> SimulatedImage { 43 | let view_port_size = self.renderer.intrinsics().image_size(); 44 | 45 | let result = self 46 | .renderer 47 | .render_params(&view_port_size, &scene_from_camera) 48 | .download_rgba(true) 49 | .render(); 50 | 51 | SimulatedImage { 52 | rgba_image: result.rgba_image.unwrap(), 53 | depth_image: result.depth_image, 54 | } 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /crates/sophus_sim/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![deny(missing_docs)] 2 | #![allow(clippy::needless_range_loop)] 3 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 4 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 5 | 6 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 7 | #[cfg(doctest)] 8 | pub struct ReadmeDoctests; 9 | 10 | /// camera simulator - camera image renderer 11 | pub mod camera_simulator; 12 | 13 | /// sophus_sim prelude. 14 | /// 15 | /// It is recommended to import this prelude when working with `sophus_sim types: 16 | /// 17 | /// ``` 18 | /// use sophus_sim::prelude::*; 19 | /// ``` 20 | /// 21 | /// or 22 | /// 23 | /// ```ignore 24 | /// use sophus::prelude::*; 25 | /// ``` 26 | /// 27 | /// to import all preludes when using the `sophus` umbrella crate. 28 | pub mod prelude { 29 | pub use sophus_autodiff::prelude::*; 30 | pub use sophus_image::prelude::*; 31 | pub use sophus_lie::prelude::*; 32 | pub use sophus_opt::prelude::*; 33 | } 34 | -------------------------------------------------------------------------------- /crates/sophus_spline/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Spline interpolation" 3 | name = "sophus_spline" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | 16 | approx.workspace = true 17 | log.workspace = true 18 | num-traits.workspace = true 19 | 20 | [build-dependencies] 21 | rustc_version.workspace = true 22 | 23 | [features] 24 | simd = ["sophus_autodiff/simd"] 25 | std = [ 26 | "sophus_autodiff/std", 27 | ] 28 | default = ["std"] 29 | -------------------------------------------------------------------------------- /crates/sophus_spline/README.md: -------------------------------------------------------------------------------- 1 | ## Spline interpolation using B-splines 2 | 3 | ## Integration with sophus-rs 4 | 5 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 6 | It re-exports the relevant prelude types under [prelude], so you can 7 | seamlessly interoperate with the rest of the sophus-rs types. 8 | -------------------------------------------------------------------------------- /crates/sophus_spline/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_tensor/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Dynamic-size tensors of static-size tensors" 3 | name = "sophus_tensor" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | 16 | approx.workspace = true 17 | bytemuck.workspace = true 18 | nalgebra.workspace = true 19 | log.workspace = true 20 | ndarray.workspace = true 21 | num-traits.workspace = true 22 | concat-arrays.workspace = true 23 | 24 | [build-dependencies] 25 | rustc_version.workspace = true 26 | 27 | [features] 28 | simd = ["sophus_autodiff/simd"] 29 | std = ["sophus_autodiff/std"] 30 | default = ["std"] 31 | -------------------------------------------------------------------------------- /crates/sophus_tensor/README.md: -------------------------------------------------------------------------------- 1 | ## Tensor as a thin wrapper around [`ndarray`](https://docs.rs/ndarray) and [`nalgebra`](https://docs.rs/nalgebra) 2 | 3 | In general, we have dynamic-size tensor (ndarrays's) of static-size 4 | tensors (nalgebra's SMat, SVec, etc.). Note this crate is merely an 5 | implementation detail of the `sophus_image` crate and might be absorbed into 6 | that crate in the future. 7 | 8 | ## Integration with sophus-rs 9 | 10 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 11 | It re-exports the relevant prelude types under [prelude], so you can 12 | seamlessly interoperate with the rest of the sophus-rs types. 13 | -------------------------------------------------------------------------------- /crates/sophus_tensor/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_tensor/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![cfg_attr(feature = "simd", feature(portable_simd))] 2 | #![deny(missing_docs)] 3 | #![no_std] 4 | #![allow(clippy::needless_range_loop)] 5 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 6 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 7 | 8 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 9 | #[cfg(doctest)] 10 | pub struct ReadmeDoctests; 11 | 12 | #[cfg(feature = "std")] 13 | extern crate std; 14 | 15 | mod arc_tensor; 16 | mod element; 17 | mod mut_tensor; 18 | mod mut_tensor_view; 19 | mod tensor_view; 20 | 21 | /// sophus_tensor prelude. 22 | /// 23 | /// It is recommended to import this prelude when working with `sophus_tensor types: 24 | /// 25 | /// ``` 26 | /// use sophus_tensor::prelude::*; 27 | /// ``` 28 | /// 29 | /// or 30 | /// 31 | /// ```ignore 32 | /// use sophus::prelude::*; 33 | /// ``` 34 | /// 35 | /// to import all preludes when using the `sophus` umbrella crate. 36 | pub mod prelude { 37 | pub use sophus_autodiff::prelude::*; 38 | 39 | pub use crate::{ 40 | element::IsStaticTensor, 41 | mut_tensor::{ 42 | InnerScalarToVec, 43 | InnerVecToMat, 44 | }, 45 | mut_tensor_view::IsMutTensorLike, 46 | tensor_view::{ 47 | IsTensorLike, 48 | IsTensorView, 49 | }, 50 | }; 51 | } 52 | 53 | pub use arc_tensor::*; 54 | pub use element::*; 55 | pub use mut_tensor::*; 56 | pub use mut_tensor_view::*; 57 | pub use tensor_view::*; 58 | -------------------------------------------------------------------------------- /crates/sophus_timeseries/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Time series data structure, e.g. for sensor fusion" 3 | name = "sophus_timeseries" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_geo.workspace = true 15 | 16 | approx.workspace = true 17 | log.workspace = true 18 | num-traits.workspace = true 19 | 20 | [build-dependencies] 21 | rustc_version.workspace = true 22 | 23 | [features] 24 | simd = ["sophus_geo/simd"] 25 | std = ["sophus_geo/std"] 26 | default = ["std"] 27 | -------------------------------------------------------------------------------- /crates/sophus_timeseries/README.md: -------------------------------------------------------------------------------- 1 | ## Time series data structure, e.g. for sensor fusion 2 | 3 | ## Integration with sophus-rs 4 | 5 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 6 | It re-exports the relevant prelude types under [prelude], so you can 7 | seamlessly interoperate with the rest of the sophus-rs types. 8 | -------------------------------------------------------------------------------- /crates/sophus_timeseries/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_viewer/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | description = "Basic 2D / 3D visualization, e.g. for computer vision applications" 3 | name = "sophus_viewer" 4 | readme = "README.md" 5 | 6 | edition.workspace = true 7 | include.workspace = true 8 | keywords.workspace = true 9 | license.workspace = true 10 | repository.workspace = true 11 | version.workspace = true 12 | 13 | [dependencies] 14 | sophus_autodiff.workspace = true 15 | sophus_image.workspace = true 16 | sophus_lie.workspace = true 17 | sophus_opt.workspace = true 18 | sophus_renderer.workspace = true 19 | sophus_sensor.workspace = true 20 | 21 | approx.workspace = true 22 | bytemuck.workspace = true 23 | eframe.workspace = true 24 | egui_extras.workspace = true 25 | egui_plot.workspace = true 26 | env_logger.workspace = true 27 | linked-hash-map.workspace = true 28 | log.workspace = true 29 | num-traits.workspace = true 30 | thingbuf.workspace = true 31 | 32 | [build-dependencies] 33 | rustc_version.workspace = true 34 | 35 | [features] 36 | std = [ 37 | "sophus_autodiff/std", 38 | "sophus_image/std", 39 | "sophus_lie/std", 40 | "sophus_opt/std", 41 | "sophus_renderer/std", 42 | "sophus_sensor/std", 43 | ] 44 | default = ["std"] 45 | -------------------------------------------------------------------------------- /crates/sophus_viewer/README.md: -------------------------------------------------------------------------------- 1 | ## Basic 2D / 3D visualization, e.g. for computer vision applications 2 | 3 | ## Integration with sophus-rs 4 | 5 | This crate is part of the [sophus umbrella crate](https://crates.io/crates/sophus). 6 | It re-exports the relevant prelude types under [prelude], so you can 7 | seamlessly interoperate with the rest of the sophus-rs types. 8 | -------------------------------------------------------------------------------- /crates/sophus_viewer/build.rs: -------------------------------------------------------------------------------- 1 | fn main() { 2 | println!("cargo:rustc-check-cfg=cfg(nightly)"); // Declares 'nightly' as a valid cfg condition 3 | 4 | let is_nightly = 5 | rustc_version::version_meta().unwrap().channel == rustc_version::Channel::Nightly; 6 | 7 | if is_nightly { 8 | println!("cargo:rustc-cfg=nightly"); 9 | } 10 | } 11 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/lib.rs: -------------------------------------------------------------------------------- 1 | #![deny(missing_docs)] 2 | #![allow(clippy::needless_range_loop)] 3 | #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] 4 | #![cfg_attr(nightly, feature(doc_auto_cfg))] 5 | 6 | #[doc = include_str!(concat!("../", core::env!("CARGO_PKG_README")))] 7 | #[cfg(doctest)] 8 | pub struct ReadmeDoctests; 9 | 10 | mod interactions; 11 | 12 | pub use interactions::{ 13 | inplane_interaction::*, 14 | orbit_interaction::*, 15 | *, 16 | }; 17 | 18 | /// The view packets. 19 | pub mod packets; 20 | /// Views. 21 | pub mod views; 22 | /// sophus_viewer prelude. 23 | /// 24 | /// It is recommended to import this prelude when working with `sophus_viewer types: 25 | /// 26 | /// ``` 27 | /// use sophus_viewer::prelude::*; 28 | /// ``` 29 | /// 30 | /// or 31 | /// 32 | /// ```ignore 33 | /// use sophus::prelude::*; 34 | /// ``` 35 | /// 36 | /// to import all preludes when using the `sophus` umbrella crate. 37 | pub mod prelude { 38 | pub(crate) use alloc::{ 39 | boxed::Box, 40 | collections::{ 41 | btree_map::BTreeMap, 42 | vec_deque::VecDeque, 43 | }, 44 | string::{ 45 | String, 46 | ToString, 47 | }, 48 | vec::Vec, 49 | }; 50 | 51 | pub use sophus_autodiff::prelude::*; 52 | pub use sophus_image::prelude::*; 53 | pub use sophus_lie::prelude::*; 54 | pub use sophus_opt::prelude::*; 55 | 56 | extern crate alloc; 57 | } 58 | 59 | mod simple_viewer; 60 | mod viewer_base; 61 | 62 | pub use simple_viewer::*; 63 | pub use viewer_base::*; 64 | pub use views::*; 65 | 66 | /// eframe native options - recommended for use with the sophus 67 | pub fn recommened_eframe_native_options() -> eframe::NativeOptions { 68 | eframe::NativeOptions { 69 | viewport: eframe::egui::ViewportBuilder::default().with_inner_size([850.0, 480.0]), 70 | renderer: eframe::Renderer::Wgpu, 71 | multisampling: sophus_renderer::SOPHUS_RENDER_MULTISAMPLE_COUNT as u16, 72 | ..Default::default() 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/packets.rs: -------------------------------------------------------------------------------- 1 | use sophus_lie::Isometry3F64; 2 | use sophus_renderer::{ 3 | camera::RenderCamera, 4 | renderables::{ 5 | ImageFrame, 6 | PixelRenderable, 7 | SceneRenderable, 8 | }, 9 | }; 10 | 11 | use crate::prelude::*; 12 | 13 | mod image_view_packet; 14 | mod plot_view_packet; 15 | mod scene_view_packet; 16 | 17 | pub use image_view_packet::*; 18 | pub use plot_view_packet::*; 19 | pub use scene_view_packet::*; 20 | 21 | /// Packet of renderables 22 | #[derive(Clone, Debug)] 23 | pub enum Packet { 24 | /// scene view packet 25 | Scene(SceneViewPacket), 26 | /// image view packet 27 | Image(ImageViewPacket), 28 | /// plot view packet 29 | Plot(Vec), 30 | } 31 | 32 | /// Create a image packet 33 | pub fn make_image_packet( 34 | view_label: &str, 35 | frame: Option, 36 | pixel_renderables: Vec, 37 | scene_renderables: Vec, 38 | ) -> Packet { 39 | Packet::Image(ImageViewPacket { 40 | frame, 41 | pixel_renderables, 42 | scene_renderables, 43 | view_label: view_label.to_string(), 44 | }) 45 | } 46 | 47 | /// Create a scene packet 48 | pub fn create_scene_packet( 49 | view_label: &str, 50 | initial_camera: RenderCamera, 51 | locked_to_birds_eye_orientation: bool, 52 | ) -> Packet { 53 | Packet::Scene(SceneViewPacket { 54 | view_label: view_label.to_string(), 55 | content: SceneViewPacketContent::Creation(SceneViewCreation { 56 | initial_camera, 57 | locked_to_birds_eye_orientation, 58 | }), 59 | }) 60 | } 61 | 62 | /// Append to scene packet 63 | pub fn append_to_scene_packet(view_label: &str, scene_renderables: Vec) -> Packet { 64 | Packet::Scene(SceneViewPacket { 65 | view_label: view_label.to_string(), 66 | content: SceneViewPacketContent::Renderables(scene_renderables), 67 | }) 68 | } 69 | 70 | /// Create world-from-scene update, scene packet 71 | pub fn world_from_scene_update_packet( 72 | view_label: &str, 73 | world_from_scene_update: Isometry3F64, 74 | ) -> Packet { 75 | Packet::Scene(SceneViewPacket { 76 | view_label: view_label.to_string(), 77 | content: SceneViewPacketContent::WorldFromSceneUpdate(world_from_scene_update), 78 | }) 79 | } 80 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/packets/image_view_packet.rs: -------------------------------------------------------------------------------- 1 | use sophus_renderer::renderables::{ 2 | ImageFrame, 3 | PixelRenderable, 4 | SceneRenderable, 5 | }; 6 | 7 | use crate::prelude::*; 8 | 9 | /// Packet to populate an image view 10 | #[derive(Clone, Debug)] 11 | pub struct ImageViewPacket { 12 | /// Frame to hold content 13 | /// 14 | /// 1. For each `view_label`, content (i.e. pixel_renderables, scene_renderables) will be 15 | /// added to the existing frame. If no frame exists yet, e.g. frame was always None for 16 | /// `view_label`, the content is ignored. 17 | /// 2. If we have a new frame, that is `frame == Some(...)`, all previous content is deleted, 18 | /// but content from this packet will be added. 19 | pub frame: Option, 20 | /// List of 2d renderables 21 | pub pixel_renderables: Vec, 22 | /// List of scene renderables 23 | pub scene_renderables: Vec, 24 | /// Name of the view 25 | pub view_label: String, 26 | } 27 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/packets/plot_view_packet/curve_vec_with_conf.rs: -------------------------------------------------------------------------------- 1 | use sophus_renderer::renderables::Color; 2 | 3 | use crate::{ 4 | packets::{ 5 | VerticalLine, 6 | plot_view_packet::{ 7 | ClearCondition, 8 | CurveTrait, 9 | }, 10 | }, 11 | prelude::*, 12 | }; 13 | 14 | /// Vector of curves with confidence intervals 15 | #[derive(Clone, Debug)] 16 | pub struct CurveVecWithConf { 17 | /// data 18 | pub data: DataVecDeque, 19 | /// style 20 | pub style: CurveVecWithConfStyle, 21 | /// clear condition 22 | pub clear_cond: ClearCondition, 23 | /// vertical line 24 | pub v_line: Option, 25 | } 26 | 27 | /// style of CurveVecWithConf 28 | #[derive(Copy, Clone, Debug)] 29 | pub struct CurveVecWithConfStyle { 30 | /// colors, one for each curve 31 | pub colors: [Color; N], 32 | } 33 | 34 | /// vec conf curve data 35 | pub type DataVecDeque = VecDeque<(f64, ([f64; N], [f64; N]))>; 36 | 37 | impl CurveVecWithConf { 38 | /// Create a new vec curve with confidence intervals 39 | pub fn new( 40 | data: DataVecDeque, 41 | color: [Color; N], 42 | clear_cond: ClearCondition, 43 | v_line: Option, 44 | ) -> Self { 45 | CurveVecWithConf { 46 | data, 47 | style: CurveVecWithConfStyle { colors: color }, 48 | clear_cond, 49 | v_line, 50 | } 51 | } 52 | } 53 | 54 | impl CurveTrait<([f64; N], [f64; N]), CurveVecWithConfStyle> 55 | for CurveVecWithConf 56 | { 57 | fn mut_tuples(&mut self) -> &mut VecDeque<(f64, ([f64; N], [f64; N]))> { 58 | &mut self.data 59 | } 60 | 61 | fn update_vline(&mut self, v_line: Option) { 62 | self.v_line = v_line; 63 | } 64 | 65 | fn assign_style(&mut self, style: CurveVecWithConfStyle) { 66 | self.style = style; 67 | } 68 | } 69 | 70 | /// CurveVecWithConf with plot name and curve name 71 | #[derive(Clone, Debug)] 72 | pub struct NamedVecConfCurve { 73 | /// plot name 74 | pub plot_name: String, 75 | /// curve name 76 | pub curve_name: String, 77 | /// scalar curve 78 | pub scalar_curve: CurveVecWithConf, 79 | } 80 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/packets/plot_view_packet/scalar_curve.rs: -------------------------------------------------------------------------------- 1 | use sophus_renderer::renderables::Color; 2 | 3 | use crate::{ 4 | packets::{ 5 | VerticalLine, 6 | plot_view_packet::{ 7 | ClearCondition, 8 | CurveTrait, 9 | LineType, 10 | }, 11 | }, 12 | prelude::*, 13 | }; 14 | 15 | /// Scalar curve style 16 | #[derive(Copy, Clone, Debug)] 17 | pub struct ScalarCurveStyle { 18 | /// color 19 | pub color: Color, 20 | /// line type 21 | pub line_type: LineType, 22 | } 23 | 24 | /// Scalar curve 25 | #[derive(Clone, Debug)] 26 | pub struct ScalarCurve { 27 | /// data 28 | pub data: VecDeque<(f64, f64)>, 29 | /// style 30 | pub style: ScalarCurveStyle, 31 | 32 | /// clear condition 33 | pub clear_cond: ClearCondition, 34 | 35 | /// v-line 36 | pub v_line: Option, 37 | } 38 | 39 | impl ScalarCurve { 40 | /// Create a new scalar curve 41 | pub fn new( 42 | data: VecDeque<(f64, f64)>, 43 | color: Color, 44 | line_type: LineType, 45 | clear_cond: ClearCondition, 46 | v_line: Option, 47 | ) -> Self { 48 | ScalarCurve { 49 | data, 50 | style: ScalarCurveStyle { color, line_type }, 51 | clear_cond, 52 | v_line, 53 | } 54 | } 55 | } 56 | 57 | impl CurveTrait for ScalarCurve { 58 | fn mut_tuples(&mut self) -> &mut VecDeque<(f64, f64)> { 59 | &mut self.data 60 | } 61 | 62 | fn update_vline(&mut self, v_line: Option) { 63 | self.v_line = v_line; 64 | } 65 | 66 | fn assign_style(&mut self, style: ScalarCurveStyle) { 67 | self.style = style; 68 | } 69 | } 70 | 71 | /// Named scalar curve 72 | #[derive(Clone, Debug)] 73 | pub struct NamedScalarCurve { 74 | /// plot name 75 | pub plot_name: String, 76 | /// graph name 77 | pub graph_name: String, 78 | /// scalar curve 79 | pub scalar_curve: ScalarCurve, 80 | } 81 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/packets/plot_view_packet/vec_curve.rs: -------------------------------------------------------------------------------- 1 | use sophus_renderer::renderables::Color; 2 | 3 | use crate::{ 4 | packets::{ 5 | VerticalLine, 6 | plot_view_packet::{ 7 | ClearCondition, 8 | CurveTrait, 9 | LineType, 10 | }, 11 | }, 12 | prelude::*, 13 | }; 14 | 15 | /// Vector of curves 16 | #[derive(Clone, Debug)] 17 | pub struct CurveVec { 18 | /// data 19 | pub data: VecDeque<(f64, [f64; N])>, 20 | /// style 21 | pub style: CurveVecStyle, 22 | /// clear condition 23 | pub clear_cond: ClearCondition, 24 | /// vertical line 25 | pub v_line: Option, 26 | } 27 | 28 | /// style of CurveVec 29 | #[derive(Copy, Clone, Debug)] 30 | pub struct CurveVecStyle { 31 | /// colors, one for each curve 32 | pub colors: [Color; N], 33 | /// line type 34 | pub line_type: LineType, 35 | } 36 | 37 | impl CurveVec { 38 | /// Create a new curve vector 39 | pub fn new( 40 | data: VecDeque<(f64, [f64; N])>, 41 | color: [Color; N], 42 | line_type: LineType, 43 | clear_cond: ClearCondition, 44 | v_line: Option, 45 | ) -> Self { 46 | CurveVec { 47 | data, 48 | style: CurveVecStyle { 49 | colors: color, 50 | line_type, 51 | }, 52 | clear_cond, 53 | v_line, 54 | } 55 | } 56 | } 57 | 58 | impl CurveTrait<[f64; N], CurveVecStyle> for CurveVec { 59 | fn mut_tuples(&mut self) -> &mut VecDeque<(f64, [f64; N])> { 60 | &mut self.data 61 | } 62 | 63 | fn update_vline(&mut self, v_line: Option) { 64 | self.v_line = v_line; 65 | } 66 | 67 | fn assign_style(&mut self, style: CurveVecStyle) { 68 | self.style = style; 69 | } 70 | } 71 | 72 | /// Curve vector with curve name and plot name 73 | #[derive(Clone, Debug)] 74 | pub struct NamedCurveVec { 75 | /// plot name 76 | pub plot_name: String, 77 | /// curve name 78 | pub curve_name: String, 79 | /// scalar curve 80 | pub scalar_curve: CurveVec, 81 | } 82 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/packets/scene_view_packet.rs: -------------------------------------------------------------------------------- 1 | use sophus_lie::Isometry3F64; 2 | use sophus_renderer::{ 3 | camera::RenderCamera, 4 | renderables::SceneRenderable, 5 | }; 6 | 7 | use crate::prelude::*; 8 | 9 | /// Content of a scene view packet 10 | #[derive(Clone, Debug)] 11 | pub enum SceneViewPacketContent { 12 | /// List of 3d renderables 13 | Renderables(Vec), 14 | /// create a new view 15 | Creation(SceneViewCreation), 16 | /// world-from-scene pose update 17 | WorldFromSceneUpdate(Isometry3F64), 18 | } 19 | 20 | /// Creation of a scene view 21 | #[derive(Clone, Debug)] 22 | pub struct SceneViewCreation { 23 | /// Initial camera, ignored if not the first packet for this view 24 | pub initial_camera: RenderCamera, 25 | /// lock xy plane 26 | pub locked_to_birds_eye_orientation: bool, 27 | } 28 | 29 | /// Packet to populate a scene view 30 | #[derive(Clone, Debug)] 31 | pub struct SceneViewPacket { 32 | /// Name of the view 33 | pub view_label: String, 34 | /// Content of the packet 35 | pub content: SceneViewPacketContent, 36 | } 37 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/simple_viewer.rs: -------------------------------------------------------------------------------- 1 | use eframe::egui; 2 | use sophus_renderer::RenderContext; 3 | use thingbuf::mpsc::blocking::Receiver; 4 | 5 | use crate::{ 6 | packets::Packet, 7 | prelude::*, 8 | viewer_base::{ 9 | ViewerBase, 10 | ViewerBaseConfig, 11 | }, 12 | }; 13 | 14 | /// Simple viewer 15 | pub struct SimpleViewer { 16 | base: ViewerBase, 17 | } 18 | 19 | impl SimpleViewer { 20 | /// Create a new simple viewer 21 | pub fn new( 22 | render_state: RenderContext, 23 | message_recv: Receiver>, 24 | ) -> Box { 25 | Box::new(SimpleViewer { 26 | base: ViewerBase::new(render_state, ViewerBaseConfig { message_recv }), 27 | }) 28 | } 29 | } 30 | 31 | impl eframe::App for SimpleViewer { 32 | fn update(&mut self, ctx: &egui::Context, _frame: &mut eframe::Frame) { 33 | self.base.update_data(); 34 | 35 | egui::TopBottomPanel::top("top").show(ctx, |ui| { 36 | self.base.update_top_bar(ui, ctx); 37 | }); 38 | 39 | egui::SidePanel::left("left").show(ctx, |ui| { 40 | self.base.update_left_panel(ui, ctx); 41 | }); 42 | 43 | egui::TopBottomPanel::bottom("bottom") 44 | .max_height(32.0) 45 | .show(ctx, |ui| { 46 | self.base.update_bottom_status_bar(ui, ctx); 47 | }); 48 | 49 | // central pane must be always created last 50 | egui::CentralPanel::default().show(ctx, |ui| { 51 | self.base.update_central_panel(ui, ctx); 52 | }); 53 | 54 | self.base.process_events(); 55 | 56 | ctx.request_repaint(); 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/views/active_view_info.rs: -------------------------------------------------------------------------------- 1 | use sophus_image::ImageSize; 2 | use sophus_lie::Isometry3F64; 3 | use sophus_renderer::{ 4 | SceneFocusMarker, 5 | camera::RenderCameraProperties, 6 | }; 7 | 8 | use crate::prelude::*; 9 | 10 | /// active view info 11 | pub struct ActiveViewInfo { 12 | /// name of active view 13 | pub active_view: String, 14 | /// scene-from-camera pose 15 | pub scene_from_camera: Isometry3F64, 16 | /// camere properties 17 | pub camera_properties: Option, 18 | /// scene focus 19 | pub scene_focus: SceneFocusMarker, 20 | /// type 21 | pub view_type: String, 22 | /// view-port size 23 | pub view_port_size: ImageSize, 24 | /// xy-locked 25 | pub locked_to_birds_eye_orientation: bool, 26 | } 27 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/views/image_view.rs: -------------------------------------------------------------------------------- 1 | use linked_hash_map::LinkedHashMap; 2 | use sophus_renderer::{ 3 | HasAspectRatio, 4 | OffscreenRenderer, 5 | RenderContext, 6 | camera::RenderIntrinsics, 7 | }; 8 | 9 | use crate::{ 10 | interactions::{ 11 | InteractionEnum, 12 | inplane_interaction::InplaneInteraction, 13 | }, 14 | packets::ImageViewPacket, 15 | prelude::*, 16 | views::View, 17 | }; 18 | 19 | pub(crate) struct ImageView { 20 | pub(crate) renderer: OffscreenRenderer, 21 | pub(crate) interaction: InteractionEnum, 22 | pub(crate) enabled: bool, 23 | } 24 | 25 | impl ImageView { 26 | fn create_if_new( 27 | views: &mut LinkedHashMap, 28 | packet: &ImageViewPacket, 29 | state: &RenderContext, 30 | ) -> bool { 31 | if views.contains_key(&packet.view_label) { 32 | return false; 33 | } 34 | if let Some(frame) = &packet.frame { 35 | views.insert( 36 | packet.view_label.clone(), 37 | View::Image(ImageView { 38 | renderer: OffscreenRenderer::new(state, frame.camera_properties()), 39 | interaction: InteractionEnum::InPlane(InplaneInteraction::new( 40 | &packet.view_label, 41 | )), 42 | enabled: true, 43 | }), 44 | ); 45 | return true; 46 | } 47 | 48 | false 49 | } 50 | 51 | pub fn update( 52 | views: &mut LinkedHashMap, 53 | packet: ImageViewPacket, 54 | state: &RenderContext, 55 | ) { 56 | Self::create_if_new(views, &packet, state); 57 | let view = views.get_mut(&packet.view_label).unwrap(); 58 | 59 | let view = match view { 60 | View::Image(view) => view, 61 | _ => panic!("View type mismatch"), 62 | }; 63 | 64 | if let Some(frame) = packet.frame { 65 | let new_camera_properties = frame.camera_properties().clone(); 66 | 67 | // We got a new frame, hence we need to clear all renderables and then add the 68 | // intrinsics and background image if present. The easiest and most error-proof way to 69 | // do this is to create a new SceneRenderer and PixelRenderer and replace the old ones. 70 | view.renderer = OffscreenRenderer::new(state, &new_camera_properties); 71 | 72 | view.renderer 73 | .reset_2d_frame(&new_camera_properties.intrinsics, frame.maybe_image()); 74 | } 75 | 76 | let view = views.get_mut(&packet.view_label).unwrap(); 77 | let view = match view { 78 | View::Image(view) => view, 79 | _ => panic!("View type mismatch"), 80 | }; 81 | 82 | view.renderer.update_pixels(packet.pixel_renderables); 83 | view.renderer.update_scene(packet.scene_renderables); 84 | } 85 | 86 | pub fn intrinsics(&self) -> RenderIntrinsics { 87 | self.renderer.intrinsics() 88 | } 89 | } 90 | 91 | impl HasAspectRatio for ImageView { 92 | fn aspect_ratio(&self) -> f32 { 93 | self.renderer.aspect_ratio() 94 | } 95 | } 96 | -------------------------------------------------------------------------------- /crates/sophus_viewer/src/views/scene_view.rs: -------------------------------------------------------------------------------- 1 | use linked_hash_map::LinkedHashMap; 2 | use log::warn; 3 | use sophus_renderer::{ 4 | HasAspectRatio, 5 | OffscreenRenderer, 6 | RenderContext, 7 | camera::RenderIntrinsics, 8 | }; 9 | 10 | use crate::{ 11 | interactions::{ 12 | InteractionEnum, 13 | orbit_interaction::OrbitalInteraction, 14 | }, 15 | packets::{ 16 | SceneViewCreation, 17 | SceneViewPacket, 18 | SceneViewPacketContent, 19 | }, 20 | prelude::*, 21 | views::View, 22 | }; 23 | 24 | pub(crate) struct SceneView { 25 | pub(crate) renderer: OffscreenRenderer, 26 | pub(crate) interaction: InteractionEnum, 27 | pub(crate) enabled: bool, 28 | pub(crate) locked_to_birds_eye_orientation: bool, 29 | } 30 | 31 | impl SceneView { 32 | fn create( 33 | views: &mut LinkedHashMap, 34 | view_label: &str, 35 | creation: &SceneViewCreation, 36 | state: &RenderContext, 37 | ) { 38 | views.insert( 39 | view_label.to_string(), 40 | View::Scene(SceneView { 41 | renderer: OffscreenRenderer::new(state, &creation.initial_camera.properties), 42 | interaction: InteractionEnum::Orbital(OrbitalInteraction::new( 43 | view_label, 44 | creation.initial_camera.scene_from_camera, 45 | creation.initial_camera.properties.clipping_planes, 46 | )), 47 | enabled: true, 48 | locked_to_birds_eye_orientation: creation.locked_to_birds_eye_orientation, 49 | }), 50 | ); 51 | } 52 | 53 | pub fn update( 54 | views: &mut LinkedHashMap, 55 | packet: SceneViewPacket, 56 | state: &RenderContext, 57 | ) { 58 | match &packet.content { 59 | SceneViewPacketContent::Creation(creation) => { 60 | Self::create(views, &packet.view_label, creation, state); 61 | } 62 | SceneViewPacketContent::Renderables(r) => { 63 | if let Some(view) = views.get_mut(&packet.view_label) { 64 | if let View::Scene(scene_view) = view { 65 | scene_view.renderer.update_scene(r.clone()); 66 | } else { 67 | warn!("Is not a scene-view: {}", packet.view_label); 68 | } 69 | } else { 70 | warn!("View not found: {}", packet.view_label); 71 | } 72 | } 73 | SceneViewPacketContent::WorldFromSceneUpdate(world_from_scene) => { 74 | if let Some(view) = views.get_mut(&packet.view_label) { 75 | if let View::Scene(scene_view) = view { 76 | scene_view.renderer.scene.world_from_scene = *world_from_scene 77 | } else { 78 | warn!("Is not a scene-view: {}", packet.view_label); 79 | } 80 | } else { 81 | warn!("View not found: {}", packet.view_label); 82 | } 83 | } 84 | } 85 | } 86 | 87 | pub fn intrinsics(&self) -> RenderIntrinsics { 88 | self.renderer.intrinsics() 89 | } 90 | } 91 | 92 | impl HasAspectRatio for SceneView { 93 | fn aspect_ratio(&self) -> f32 { 94 | self.renderer.aspect_ratio() 95 | } 96 | } 97 | -------------------------------------------------------------------------------- /justfile: -------------------------------------------------------------------------------- 1 | clippy: 2 | cargo clippy 3 | 4 | build: 5 | cargo build --release --all-targets 6 | 7 | build-std: 8 | cargo build --release --all-targets --features std 9 | 10 | build-simd: 11 | cargo +nightly build --release --all-targets --features simd 12 | 13 | test-simd: 14 | cargo +nightly test --release --features simd 15 | 16 | test: 17 | cargo test --release --features std 18 | 19 | format: 20 | pre-commit run -a 21 | cargo +nightly fmt 22 | 23 | doc: 24 | cargo +nightly doc --no-deps --all-features 25 | cargo +nightly test --release --doc --all-features 26 | 27 | camera_sim: 28 | cargo run --example camera_sim --release --features std 29 | 30 | mini_optics_sim: 31 | cargo run --example mini_optics_sim --release --features std 32 | 33 | viewer_ex: 34 | cargo run --example viewer_ex --release --features std 35 | -------------------------------------------------------------------------------- /publish_all.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x # echo on 4 | set -e # exit on error 5 | 6 | cargo publish -p sophus_autodiff 7 | cargo publish -p sophus_tensor 8 | cargo publish -p sophus_image 9 | cargo publish -p sophus_lie 10 | cargo publish -p sophus_geo 11 | cargo publish -p sophus_spline 12 | cargo publish -p sophus_sensor 13 | cargo publish -p sophus_opt 14 | cargo publish -p sophus_timeseries 15 | cargo publish -p sophus_renderer 16 | cargo publish -p sophus_sim 17 | cargo publish -p sophus_viewer 18 | cargo publish -p sophus 19 | -------------------------------------------------------------------------------- /rustfmt.toml: -------------------------------------------------------------------------------- 1 | comment_width = 100 2 | format_code_in_doc_comments = true 3 | group_imports = "StdExternalCrate" 4 | imports_granularity = "Crate" 5 | imports_layout = "Vertical" 6 | wrap_comments = true 7 | -------------------------------------------------------------------------------- /sophus-rs.code-workspace: -------------------------------------------------------------------------------- 1 | { 2 | "folders": [ 3 | { 4 | "path": "." 5 | } 6 | ], 7 | "settings": { 8 | "rust-analyzer.linkedProjects": [ 9 | "./Cargo.toml", 10 | ], 11 | "rust-analyzer.cargo.target": null, 12 | "rust-analyzer.diagnostics.styleLints.enable": true, 13 | "rust-analyzer.imports.granularity.group": "item", 14 | "rust-analyzer.imports.prefix": "crate", 15 | "rust-analyzer.cargo.unsetTest": [], 16 | "rust-analyzer.cargo.features": [ 17 | ], 18 | } 19 | } 20 | --------------------------------------------------------------------------------