From 2e4c57328624fcba50c544ef8c208e4f5108f359 Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Sun, 5 Jan 2025 17:32:58 -0800 Subject: [PATCH] tweaks - for 0.11.0 --- crates/sophus/Cargo.toml | 4 + crates/sophus/examples/viewer_ex.rs | 8 +- crates/sophus/src/lib.rs | 10 +- crates/sophus_autodiff/Cargo.toml | 1 + crates/sophus_autodiff/src/linalg/scalar.rs | 3 + crates/sophus_autodiff/src/manifold.rs | 20 ++++ crates/sophus_geo/Cargo.toml | 6 +- crates/sophus_geo/src/lib.rs | 1 + crates/sophus_geo/src/manifold.rs | 2 - crates/sophus_image/Cargo.toml | 4 +- crates/sophus_image/src/io/tiff.rs | 12 +- crates/sophus_image/src/lib.rs | 1 + crates/sophus_lie/Cargo.toml | 4 +- .../src/lie_group/lie_group_manifold.rs | 112 +++++------------- crates/sophus_opt/Cargo.toml | 5 +- crates/sophus_opt/src/lib.rs | 1 + crates/sophus_opt/src/variables.rs | 88 +------------- crates/sophus_renderer/Cargo.toml | 15 ++- crates/sophus_renderer/src/lib.rs | 4 + crates/sophus_sensor/Cargo.toml | 2 + .../src/camera_enum/perspective_camera.rs | 17 +++ crates/sophus_sensor/src/dyn_camera.rs | 10 +- crates/sophus_sensor/src/lib.rs | 2 + .../src/projections/perspective.rs | 6 +- crates/sophus_sensor/src/traits.rs | 12 +- crates/sophus_sim/Cargo.toml | 12 +- crates/sophus_sim/src/lib.rs | 8 ++ crates/sophus_spline/Cargo.toml | 7 +- crates/sophus_spline/src/lib.rs | 5 + crates/sophus_tensor/Cargo.toml | 1 + crates/sophus_tensor/src/lib.rs | 1 + crates/sophus_timeseries/Cargo.toml | 5 +- crates/sophus_viewer/Cargo.toml | 13 +- crates/sophus_viewer/src/lib.rs | 6 +- 34 files changed, 189 insertions(+), 219 deletions(-) delete mode 100644 crates/sophus_geo/src/manifold.rs diff --git a/crates/sophus/Cargo.toml b/crates/sophus/Cargo.toml index 655e0e6..84b15dd 100644 --- a/crates/sophus/Cargo.toml +++ b/crates/sophus/Cargo.toml @@ -16,9 +16,12 @@ sophus_geo.workspace = true sophus_image.workspace = true sophus_lie.workspace = true sophus_opt.workspace = true +sophus_tensor.workspace = true sophus_sensor.workspace = true sophus_renderer.workspace = true sophus_sim.workspace = true +sophus_spline.workspace = true +sophus_timeseries.workspace = true sophus_viewer.workspace = true nalgebra.workspace = true @@ -50,6 +53,7 @@ std = [ "sophus_opt/std", "sophus_sensor/std", ] +default = ["std"] [[example]] name = "camera_sim" diff --git a/crates/sophus/examples/viewer_ex.rs b/crates/sophus/examples/viewer_ex.rs index e16e6f7..9599db3 100644 --- a/crates/sophus/examples/viewer_ex.rs +++ b/crates/sophus/examples/viewer_ex.rs @@ -1,10 +1,10 @@ #![cfg(feature = "std")] use core::f64::consts::TAU; -use sophus::core::linalg::VecF64; +use sophus::autodiff::linalg::VecF64; use sophus::examples::viewer_example::make_distorted_frame; -use sophus::lie::prelude::IsVector; use sophus::lie::Isometry3; +use sophus::prelude::*; use sophus::sensor::dyn_camera::DynCameraF64; use sophus_image::intensity_image::intensity_arc_image::IsIntensityArcImage; use sophus_image::mut_image::MutImageF32; @@ -101,11 +101,11 @@ fn create_tiny_image_view_packet() -> Packet { fn create_scene(pinhole: bool) -> Vec { let unified_cam = DynCameraF64::new_unified( - &VecF64::from_array([500.0, 500.0, 320.0, 240.0, 0.629, 1.02]), + VecF64::from_array([500.0, 500.0, 320.0, 240.0, 0.629, 1.02]), ImageSize::new(639, 479), ); let pinhole_cam = DynCameraF64::new_pinhole( - &VecF64::from_array([500.0, 500.0, 320.0, 240.0]), + VecF64::from_array([500.0, 500.0, 320.0, 240.0]), ImageSize::new(639, 479), ); diff --git a/crates/sophus/src/lib.rs b/crates/sophus/src/lib.rs index 074629a..c52cba1 100644 --- a/crates/sophus/src/lib.rs +++ b/crates/sophus/src/lib.rs @@ -3,7 +3,7 @@ #![doc = include_str!(concat!("../", std::env!("CARGO_PKG_README")))] #[doc(inline)] -pub use sophus_autodiff as core; +pub use sophus_autodiff as autodiff; #[doc(inline)] pub use sophus_geo as geo; #[doc(inline)] @@ -19,6 +19,12 @@ pub use sophus_sensor as sensor; #[doc(inline)] pub use sophus_sim as sim; #[doc(inline)] +pub use sophus_spline as spline; +#[doc(inline)] +pub use sophus_tensor as tensor; +#[doc(inline)] +pub use sophus_timeseries as timeseries; +#[doc(inline)] pub use sophus_viewer as viewer; pub mod examples; @@ -29,7 +35,7 @@ pub use nalgebra; pub use ndarray; pub mod prelude { - pub use crate::core::prelude::*; + pub use crate::autodiff::prelude::*; pub use crate::image::prelude::*; pub use crate::lie::prelude::*; pub use crate::opt::prelude::*; diff --git a/crates/sophus_autodiff/Cargo.toml b/crates/sophus_autodiff/Cargo.toml index 9f6a2cc..fd13503 100644 --- a/crates/sophus_autodiff/Cargo.toml +++ b/crates/sophus_autodiff/Cargo.toml @@ -24,3 +24,4 @@ sleef = {version = "0.3", optional = true} [features] simd = ["sleef"] std = [] +default = ["std"] diff --git a/crates/sophus_autodiff/src/linalg/scalar.rs b/crates/sophus_autodiff/src/linalg/scalar.rs index da49f7f..3cfdbbd 100644 --- a/crates/sophus_autodiff/src/linalg/scalar.rs +++ b/crates/sophus_autodiff/src/linalg/scalar.rs @@ -84,6 +84,9 @@ pub trait IsScalar: + AbsDiffEq + RelativeEq + IsCoreScalar + + 'static + + Send + + Sync { /// Scalar type type Scalar: IsScalar; diff --git a/crates/sophus_autodiff/src/manifold.rs b/crates/sophus_autodiff/src/manifold.rs index 1fd39e7..d9d5bf3 100644 --- a/crates/sophus_autodiff/src/manifold.rs +++ b/crates/sophus_autodiff/src/manifold.rs @@ -2,6 +2,7 @@ use crate::linalg::VecF64; use crate::params::HasParams; use crate::prelude::IsScalar; extern crate alloc; +use core::fmt::Debug; /// A tangent implementation. pub trait IsTangent< @@ -16,6 +17,15 @@ pub trait IsTangent< fn tangent_examples() -> alloc::vec::Vec>; } +/// A decision variables +pub trait IsVariable: Clone + Debug + Send + Sync + 'static { + /// number of degrees of freedom + const DOF: usize; + + /// update the variable in-place. + fn update(&mut self, delta: nalgebra::DVectorView); +} + /// A manifold. pub trait IsManifold< S: IsScalar, @@ -41,3 +51,13 @@ impl IsManifold for VecF64 { self - rhs } } +impl IsVariable for VecF64 { + const DOF: usize = N; + + fn update(&mut self, delta: nalgebra::DVectorView) { + assert_eq!(delta.len(), Self::DOF); + for d in 0..Self::DOF { + self[d] += delta[d]; + } + } +} diff --git a/crates/sophus_geo/Cargo.toml b/crates/sophus_geo/Cargo.toml index a19762e..5d42c30 100644 --- a/crates/sophus_geo/Cargo.toml +++ b/crates/sophus_geo/Cargo.toml @@ -11,9 +11,13 @@ repository.workspace = true version.workspace = true [dependencies] -approx.workspace = true sophus_autodiff.workspace = true sophus_lie.workspace = true +approx.workspace = true + [features] simd = ["sophus_lie/simd"] +std = ["sophus_autodiff/std", "sophus_lie/std"] +default = ["std"] + diff --git a/crates/sophus_geo/src/lib.rs b/crates/sophus_geo/src/lib.rs index 058eaad..95c6eea 100644 --- a/crates/sophus_geo/src/lib.rs +++ b/crates/sophus_geo/src/lib.rs @@ -13,4 +13,5 @@ pub mod unit_vector; pub mod prelude { pub use crate::region::IsRegion; pub use sophus_autodiff::prelude::*; + pub use sophus_lie::prelude::*; } diff --git a/crates/sophus_geo/src/manifold.rs b/crates/sophus_geo/src/manifold.rs deleted file mode 100644 index e70d06f..0000000 --- a/crates/sophus_geo/src/manifold.rs +++ /dev/null @@ -1,2 +0,0 @@ -/// manifold trait -pub mod traits; diff --git a/crates/sophus_image/Cargo.toml b/crates/sophus_image/Cargo.toml index bf22953..d3edb1e 100644 --- a/crates/sophus_image/Cargo.toml +++ b/crates/sophus_image/Cargo.toml @@ -19,10 +19,10 @@ bytemuck.workspace = true nalgebra.workspace = true ndarray.workspace = true num-traits.workspace = true - tiff = {version = "0.9.0", optional = true} png = {version ="0.17", optional = true} [features] simd = ["sophus_tensor/simd"] -std = ["png", "tiff", "sophus_tensor/std"] +std = ["png", "tiff", "sophus_autodiff/std", "sophus_tensor/std"] +default = ["std"] diff --git a/crates/sophus_image/src/io/tiff.rs b/crates/sophus_image/src/io/tiff.rs index 32103ee..9d4189a 100644 --- a/crates/sophus_image/src/io/tiff.rs +++ b/crates/sophus_image/src/io/tiff.rs @@ -109,12 +109,12 @@ pub fn load_tiff(path: impl ToString) -> std::io::Result { )); } // num_channels == 4 - return Ok(DynIntensityMutImage::RgbaU8( + Ok(DynIntensityMutImage::RgbaU8( MutImage4U8::make_copy_from_size_and_slice( image_size, bytemuck::cast_slice(&u8_img), ), - )); + )) } DecodingResult::U16(u16_img) => { if num_channels == 1 { @@ -139,12 +139,12 @@ pub fn load_tiff(path: impl ToString) -> std::io::Result { )); } // num_channels == 4 - return Ok(DynIntensityMutImage::RgbaU16( + Ok(DynIntensityMutImage::RgbaU16( MutImage4U16::make_copy_from_size_and_slice( image_size, bytemuck::cast_slice(&u16_img), ), - )); + )) } DecodingResult::F32(f32_img) => { if num_channels == 1 { @@ -169,12 +169,12 @@ pub fn load_tiff(path: impl ToString) -> std::io::Result { )); } // num_channels == 4 - return Ok(DynIntensityMutImage::RgbaF32( + Ok(DynIntensityMutImage::RgbaF32( MutImage4F32::make_copy_from_size_and_slice( image_size, bytemuck::cast_slice(&f32_img), ), - )); + )) } _ => Err(std::io::Error::other(format!( "unsupported decoding: {:?}", diff --git a/crates/sophus_image/src/lib.rs b/crates/sophus_image/src/lib.rs index 6650e41..fb205cd 100644 --- a/crates/sophus_image/src/lib.rs +++ b/crates/sophus_image/src/lib.rs @@ -88,5 +88,6 @@ pub mod prelude { pub use crate::image_view::IsImageView; pub use crate::intensity_image::dyn_intensity_image::DynIntensityMutImage; pub use crate::mut_image_view::IsMutImageView; + pub use sophus_autodiff::prelude::*; pub use sophus_tensor::prelude::*; } diff --git a/crates/sophus_lie/Cargo.toml b/crates/sophus_lie/Cargo.toml index db0a1e4..30ac67b 100644 --- a/crates/sophus_lie/Cargo.toml +++ b/crates/sophus_lie/Cargo.toml @@ -11,6 +11,7 @@ repository.workspace = true version.workspace = true [dependencies] +sophus_autodiff = {workspace = true} approx.workspace = true log.workspace = true @@ -18,8 +19,7 @@ nalgebra.workspace = true num-traits.workspace = true snafu.workspace = true -sophus_autodiff = {workspace = true} - [features] simd = ["sophus_autodiff/simd"] std = ["sophus_autodiff/std"] +default = ["std"] diff --git a/crates/sophus_lie/src/lie_group/lie_group_manifold.rs b/crates/sophus_lie/src/lie_group/lie_group_manifold.rs index fd6e276..033c48a 100644 --- a/crates/sophus_lie/src/lie_group/lie_group_manifold.rs +++ b/crates/sophus_lie/src/lie_group/lie_group_manifold.rs @@ -1,33 +1,17 @@ +use sophus_autodiff::linalg::VecF64; +use sophus_autodiff::manifold::IsVariable; + use crate::lie_group::LieGroup; use crate::prelude::*; use crate::traits::IsLieGroupImpl; -use core::borrow::Borrow; use core::fmt::Debug; -use sophus_autodiff::manifold::IsManifold; -use sophus_autodiff::params::HasParams; -use sophus_autodiff::params::IsParamsImpl; extern crate alloc; -/// Left group manifold -/// -/// A ⊕ t := exp(t) * A -/// A ⊖ B := log(inv(A) * B) -#[derive(Debug, Clone)] -pub struct LeftGroupManifold< - S: IsScalar, - const DOF: usize, - const PARAMS: usize, - const POINT: usize, - const AMBIENT: usize, - const BATCH: usize, - const DM: usize, - const DN: usize, - G: IsLieGroupImpl, -> { - group: LieGroup, -} - +// Lie group as Left group manifold +// +// A ⊕ t := exp(t) * A +// A ⊖ B := log(inv(A) * B) impl< S: IsScalar, const DOF: usize, @@ -38,81 +22,39 @@ impl< const DM: usize, const DN: usize, G: IsLieGroupImpl + Clone + Debug, - > IsParamsImpl - for LeftGroupManifold + > IsManifold + for LieGroup { - fn are_params_valid

(params: P) -> S::Mask - where - P: for<'a> Borrow<>::Vector>, - { - G::are_params_valid(params) - } - - fn params_examples() -> alloc::vec::Vec<>::Vector> { - G::params_examples() + fn oplus(&self, tangent: &>::Vector) -> Self { + LieGroup::::exp(tangent) * self } - fn invalid_params_examples() -> alloc::vec::Vec<>::Vector> - { - G::invalid_params_examples() + fn ominus(&self, rhs: &Self) -> >::Vector { + (self.inverse() * rhs).log() } } impl< - S: IsScalar, const DOF: usize, const PARAMS: usize, const POINT: usize, const AMBIENT: usize, - const BATCH: usize, - const DM: usize, - const DN: usize, - G: IsLieGroupImpl + Clone + Debug, - > HasParams - for LeftGroupManifold + G: IsLieGroupImpl + + Clone + + Debug + + Send + + Sync + + 'static, + > IsVariable for LieGroup { - fn from_params

(params: P) -> Self - where - P: for<'a> Borrow<>::Vector>, - { - Self { - group: LieGroup::from_params(params), - } - } - - fn set_params

(&mut self, params: P) - where - P: for<'a> Borrow<>::Vector>, - { - self.group.set_params(params) - } - - fn params(&self) -> &>::Vector { - self.group.params() - } -} + const DOF: usize = DOF; -impl< - S: IsScalar, - const DOF: usize, - const PARAMS: usize, - const POINT: usize, - const AMBIENT: usize, - const BATCH: usize, - const DM: usize, - const DN: usize, - G: IsLieGroupImpl + Clone + Debug, - > IsManifold - for LeftGroupManifold -{ - fn oplus(&self, tangent: &>::Vector) -> Self { - Self { - group: &LieGroup::::exp(tangent) - * &self.group, + fn update(&mut self, delta: nalgebra::DVectorView) { + assert_eq!(delta.len(), DOF); + let mut tangent = VecF64::::zeros(); + for d in 0..DOF { + tangent[d] = delta[d]; } - } - - fn ominus(&self, rhs: &Self) -> >::Vector { - (&self.group.inverse() * &rhs.group).log() + *self = self.oplus(&tangent); } } diff --git a/crates/sophus_opt/Cargo.toml b/crates/sophus_opt/Cargo.toml index 837bbf4..17b53a8 100644 --- a/crates/sophus_opt/Cargo.toml +++ b/crates/sophus_opt/Cargo.toml @@ -29,13 +29,14 @@ rayon = "1.10" [features] simd = [ "sophus_autodiff/simd", + "sophus_image/simd", "sophus_lie/simd", "sophus_sensor/simd", - "sophus_image/simd", ] std = [ "sophus_autodiff/std", + "sophus_image/std", "sophus_lie/std", "sophus_sensor/std", - "sophus_image/std", ] +default = ["std"] diff --git a/crates/sophus_opt/src/lib.rs b/crates/sophus_opt/src/lib.rs index 3f94732..68fdad4 100644 --- a/crates/sophus_opt/src/lib.rs +++ b/crates/sophus_opt/src/lib.rs @@ -31,6 +31,7 @@ pub mod variables; /// Sophus optimization prelude pub mod prelude { pub use crate::robust_kernel::IsRobustKernel; + pub use sophus_autodiff::prelude::*; pub use sophus_image::prelude::*; pub use sophus_lie::prelude::*; pub use sophus_sensor::prelude::*; diff --git a/crates/sophus_opt/src/variables.rs b/crates/sophus_opt/src/variables.rs index 7d3952c..1b5f4cf 100644 --- a/crates/sophus_opt/src/variables.rs +++ b/crates/sophus_opt/src/variables.rs @@ -1,13 +1,6 @@ -use crate::prelude::*; use core::fmt::Debug; use dyn_clone::DynClone; -use sophus_autodiff::linalg::VecF64; -use sophus_lie::Isometry2F64; -use sophus_lie::Isometry3F64; -use sophus_sensor::camera_enum::perspective_camera::BrownConradyCameraF64; -use sophus_sensor::camera_enum::perspective_camera::KannalaBrandtCameraF64; -use sophus_sensor::camera_enum::perspective_camera::PinholeCameraF64; -use sophus_sensor::camera_enum::perspective_camera::UnifiedCameraF64; +use sophus_autodiff::manifold::IsVariable; extern crate alloc; @@ -22,51 +15,6 @@ pub enum VarKind { Marginalized, } -///A decision variables -pub trait IsVariable: Clone + Debug + Send + Sync + 'static { - /// number of degrees of freedom - const DOF: usize; - - /// update the variable in-place (called during optimization) - fn update(&mut self, delta: nalgebra::DVectorView); -} - -impl IsVariable for BrownConradyCameraF64 { - const DOF: usize = 8; - - fn update(&mut self, delta: nalgebra::DVectorView) { - let new_params = *self.params() + delta; - self.set_params(new_params); - } -} - -impl IsVariable for PinholeCameraF64 { - const DOF: usize = 4; - - fn update(&mut self, delta: nalgebra::DVectorView) { - let new_params = *self.params() + delta; - self.set_params(new_params); - } -} - -impl IsVariable for KannalaBrandtCameraF64 { - const DOF: usize = 12; - - fn update(&mut self, delta: nalgebra::DVectorView) { - let new_params = *self.params() + delta; - self.set_params(new_params); - } -} - -impl IsVariable for UnifiedCameraF64 { - const DOF: usize = 6; - - fn update(&mut self, delta: nalgebra::DVectorView) { - let new_params = *self.params() + delta; - self.set_params(new_params); - } -} - /// Tuple of variables (one for each argument of the cost function) pub trait IsVarTuple: Send + Sync + 'static { /// number of degrees of freedom for each variable @@ -401,40 +349,6 @@ impl< } } -impl IsVariable for VecF64 { - const DOF: usize = N; - - fn update(&mut self, delta: nalgebra::DVectorView) { - for d in 0..Self::DOF { - self[d] += delta[d]; - } - } -} - -impl IsVariable for Isometry2F64 { - const DOF: usize = 3; - - fn update(&mut self, delta: nalgebra::DVectorView) { - let mut delta_vec = VecF64::<3>::zeros(); - for d in 0..::DOF { - delta_vec[d] = delta[d]; - } - self.set_params((Isometry2F64::exp(delta_vec) * *self).params()); - } -} - -impl IsVariable for Isometry3F64 { - const DOF: usize = 6; - - fn update(&mut self, delta: nalgebra::DVectorView) { - let mut delta_vec = VecF64::<6>::zeros(); - for d in 0..::DOF { - delta_vec[d] = delta[d]; - } - self.set_params((Isometry3F64::exp(delta_vec) * *self).params()); - } -} - /// A generic family of variables /// /// A list of variables of the same nature (e.g., a list of 3D points, a list of 2D isometries, etc.) diff --git a/crates/sophus_renderer/Cargo.toml b/crates/sophus_renderer/Cargo.toml index 5f71d91..e7aded9 100644 --- a/crates/sophus_renderer/Cargo.toml +++ b/crates/sophus_renderer/Cargo.toml @@ -11,6 +11,13 @@ repository.workspace = true version.workspace = true [dependencies] +sophus_autodiff.workspace = true +sophus_image.workspace = true +sophus_lie.workspace = true +sophus_opt.workspace = true +sophus_sensor.workspace = true +sophus_tensor.workspace = true + approx.workspace = true bytemuck.workspace = true eframe.workspace = true @@ -18,12 +25,6 @@ egui_extras.workspace = true env_logger.workspace = true linked-hash-map.workspace = true num-traits.workspace = true -sophus_autodiff.workspace = true -sophus_image.workspace = true -sophus_lie.workspace = true -sophus_opt.workspace = true -sophus_sensor.workspace = true -sophus_tensor.workspace = true wgpu.workspace = true [features] @@ -33,4 +34,6 @@ std = [ "sophus_lie/std", "sophus_opt/std", "sophus_sensor/std", + "sophus_tensor/std", ] +default = ["std"] diff --git a/crates/sophus_renderer/src/lib.rs b/crates/sophus_renderer/src/lib.rs index 7721bf8..23b1dd6 100644 --- a/crates/sophus_renderer/src/lib.rs +++ b/crates/sophus_renderer/src/lib.rs @@ -38,7 +38,11 @@ pub mod prelude { pub(crate) use alloc::sync::Arc; pub(crate) use alloc::vec; pub(crate) use alloc::vec::Vec; + + pub use sophus_autodiff::prelude::*; pub use sophus_image::prelude::*; + pub use sophus_lie::prelude::*; + pub use sophus_opt::prelude::*; extern crate alloc; } diff --git a/crates/sophus_sensor/Cargo.toml b/crates/sophus_sensor/Cargo.toml index 3189ef6..5e64951 100644 --- a/crates/sophus_sensor/Cargo.toml +++ b/crates/sophus_sensor/Cargo.toml @@ -25,5 +25,7 @@ num-traits.workspace = true simd = ["sophus_autodiff/simd", "sophus_image/simd"] std = [ "sophus_autodiff/std", + "sophus_geo/std", "sophus_image/std", ] +default = ["std"] diff --git a/crates/sophus_sensor/src/camera_enum/perspective_camera.rs b/crates/sophus_sensor/src/camera_enum/perspective_camera.rs index 77ba3e7..ed2cb9d 100644 --- a/crates/sophus_sensor/src/camera_enum/perspective_camera.rs +++ b/crates/sophus_sensor/src/camera_enum/perspective_camera.rs @@ -6,7 +6,9 @@ use crate::distortions::kannala_brandt::KannalaBrandtDistortionImpl; use crate::distortions::unified::UnifiedDistortionImpl; use crate::prelude::*; use crate::projections::perspective::PerspectiveProjectionImpl; +use crate::traits::IsCameraDistortionImpl; use crate::Camera; +use sophus_autodiff::manifold::IsVariable; use sophus_image::ImageSize; /// Pinhole camera @@ -63,6 +65,21 @@ pub type BrownConradyCameraF64 = BrownConradyCamera; /// Unified camera with f64 scalar type pub type UnifiedCameraF64 = UnifiedCamera; +impl< + const DISTORT: usize, + const PARAMS: usize, + Distort: IsCameraDistortionImpl, + Proj: IsProjection, + > IsVariable for Camera +{ + const DOF: usize = PARAMS; + + fn update(&mut self, delta: nalgebra::DVectorView) { + let new_params = *self.params() + delta; + self.set_params(new_params); + } +} + /// Perspective camera enum #[derive(Debug, Clone)] pub enum PerspectiveCameraEnum< diff --git a/crates/sophus_sensor/src/dyn_camera.rs b/crates/sophus_sensor/src/dyn_camera.rs index 269f35c..eaecb6f 100644 --- a/crates/sophus_sensor/src/dyn_camera.rs +++ b/crates/sophus_sensor/src/dyn_camera.rs @@ -35,7 +35,7 @@ pub type DynCamera = pub type DynCameraF64 = DynCamera; impl< - S: IsScalar, + S: IsScalar + 'static + Send + Sync, const BATCH: usize, const DM: usize, const DN: usize, @@ -179,8 +179,12 @@ impl< } } -impl, const BATCH: usize, const DM: usize, const DN: usize> - DynCamera +impl< + S: IsScalar + 'static + Send + Sync, + const BATCH: usize, + const DM: usize, + const DN: usize, + > DynCamera { /// Returns the pinhole parameters pub fn pinhole_params(&self) -> S::Vector<4> { diff --git a/crates/sophus_sensor/src/lib.rs b/crates/sophus_sensor/src/lib.rs index 91a7f6e..71e9c22 100644 --- a/crates/sophus_sensor/src/lib.rs +++ b/crates/sophus_sensor/src/lib.rs @@ -35,6 +35,8 @@ pub mod prelude { pub use crate::traits::IsCamera; pub use crate::traits::IsPerspectiveCamera; pub use crate::traits::IsProjection; + pub use sophus_autodiff::prelude::*; pub use sophus_geo::prelude::*; + pub use sophus_image::prelude::*; } diff --git a/crates/sophus_sensor/src/projections/perspective.rs b/crates/sophus_sensor/src/projections/perspective.rs index 202bdb4..a4780af 100644 --- a/crates/sophus_sensor/src/projections/perspective.rs +++ b/crates/sophus_sensor/src/projections/perspective.rs @@ -70,7 +70,11 @@ impl, const BATCH: usize, const DM: usize, const DN: } /// Perspective projection for single scalar -pub fn perspect_proj, const DM: usize, const DN: usize>( +pub fn perspect_proj< + S: IsSingleScalar + 'static + Send + Sync, + const DM: usize, + const DN: usize, +>( point_in_camera: &S::Vector<3>, ) -> S::Vector<2> { PerspectiveProjectionImpl::::proj(point_in_camera) diff --git a/crates/sophus_sensor/src/traits.rs b/crates/sophus_sensor/src/traits.rs index 5b8ca0e..d84cec1 100644 --- a/crates/sophus_sensor/src/traits.rs +++ b/crates/sophus_sensor/src/traits.rs @@ -1,4 +1,5 @@ use core::borrow::Borrow; +use core::fmt::Debug; use crate::camera_enum::perspective_camera::UnifiedCamera; use crate::prelude::*; @@ -16,7 +17,7 @@ pub trait IsCameraDistortionImpl< const BATCH: usize, const DM: usize, const DN: usize, ->: IsParamsImpl +>: IsParamsImpl + Debug + Clone + Send + Sync + 'static { /// identity parameters fn identity_params() -> S::Vector { @@ -60,7 +61,7 @@ pub trait IsProjection< const BATCH: usize, const DM: usize, const DN: usize, -> +>: Debug + Clone + Send + Sync + 'static { /// Projects a 3D point in the camera frame to a 2D point in the z=1 plane fn proj

(point_in_camera: P) -> S::Vector<2> @@ -79,7 +80,12 @@ pub trait IsProjection< } /// Camera trait -pub trait IsCamera, const BATCH: usize, const DM: usize, const DN: usize> +pub trait IsCamera< + S: IsScalar + 'static + Send + Sync, + const BATCH: usize, + const DM: usize, + const DN: usize, +> { /// Creates a new pinhole camera fn new_pinhole

(params: P, image_size: ImageSize) -> Self diff --git a/crates/sophus_sim/Cargo.toml b/crates/sophus_sim/Cargo.toml index 4c2b076..2599f29 100644 --- a/crates/sophus_sim/Cargo.toml +++ b/crates/sophus_sim/Cargo.toml @@ -11,7 +11,12 @@ repository.workspace = true version.workspace = true [dependencies] +sophus_autodiff.workspace = true +sophus_image.workspace = true +sophus_lie.workspace = true +sophus_opt.workspace = true sophus_renderer.workspace = true +sophus_sensor.workspace = true approx.workspace = true bytemuck.workspace = true @@ -20,11 +25,6 @@ egui_extras.workspace = true env_logger.workspace = true linked-hash-map.workspace = true num-traits.workspace = true -sophus_autodiff.workspace = true -sophus_image.workspace = true -sophus_lie.workspace = true -sophus_opt.workspace = true -sophus_sensor.workspace = true wgpu.workspace = true [features] @@ -33,5 +33,7 @@ std = [ "sophus_image/std", "sophus_lie/std", "sophus_opt/std", + "sophus_renderer/std", "sophus_sensor/std", ] +default = ["std"] diff --git a/crates/sophus_sim/src/lib.rs b/crates/sophus_sim/src/lib.rs index 45b7a29..bda17ad 100644 --- a/crates/sophus_sim/src/lib.rs +++ b/crates/sophus_sim/src/lib.rs @@ -6,3 +6,11 @@ /// camera simulator - camera image renderer pub mod camera_simulator; + +/// sophus sim prelude +pub mod prelude { + pub use sophus_autodiff::prelude::*; + pub use sophus_image::prelude::*; + pub use sophus_lie::prelude::*; + pub use sophus_opt::prelude::*; +} diff --git a/crates/sophus_spline/Cargo.toml b/crates/sophus_spline/Cargo.toml index 6725e2f..1b32678 100644 --- a/crates/sophus_spline/Cargo.toml +++ b/crates/sophus_spline/Cargo.toml @@ -11,10 +11,15 @@ repository.workspace = true version.workspace = true [dependencies] +sophus_autodiff.workspace = true + approx.workspace = true log.workspace = true num-traits.workspace = true -sophus_autodiff.workspace = true [features] simd = ["sophus_autodiff/simd"] +std = [ + "sophus_autodiff/std", +] +default = ["std"] \ No newline at end of file diff --git a/crates/sophus_spline/src/lib.rs b/crates/sophus_spline/src/lib.rs index 28106a2..f4502ce 100644 --- a/crates/sophus_spline/src/lib.rs +++ b/crates/sophus_spline/src/lib.rs @@ -1,3 +1,8 @@ +#![deny(missing_docs)] +#![allow(clippy::needless_range_loop)] +#![no_std] +//! # Spline module + /// Cubic B-Spline details pub mod spline_segment; diff --git a/crates/sophus_tensor/Cargo.toml b/crates/sophus_tensor/Cargo.toml index 1e2fdb1..59ffca8 100644 --- a/crates/sophus_tensor/Cargo.toml +++ b/crates/sophus_tensor/Cargo.toml @@ -25,3 +25,4 @@ concat-arrays.workspace = true [features] simd = ["sophus_autodiff/simd"] std = ["sophus_autodiff/std"] +default = ["std"] diff --git a/crates/sophus_tensor/src/lib.rs b/crates/sophus_tensor/src/lib.rs index 19ce58b..13504aa 100644 --- a/crates/sophus_tensor/src/lib.rs +++ b/crates/sophus_tensor/src/lib.rs @@ -1,4 +1,5 @@ #![deny(missing_docs)] +#![allow(clippy::needless_range_loop)] //! # Tensor module /// Arc tensor diff --git a/crates/sophus_timeseries/Cargo.toml b/crates/sophus_timeseries/Cargo.toml index 2ff89ba..8ae7ec8 100644 --- a/crates/sophus_timeseries/Cargo.toml +++ b/crates/sophus_timeseries/Cargo.toml @@ -11,10 +11,13 @@ repository.workspace = true version.workspace = true [dependencies] +sophus_geo.workspace = true + approx.workspace = true log.workspace = true num-traits.workspace = true -sophus_geo.workspace = true [features] simd = ["sophus_geo/simd"] +std = ["sophus_geo/std"] +default = ["std"] diff --git a/crates/sophus_viewer/Cargo.toml b/crates/sophus_viewer/Cargo.toml index 0385784..30458ae 100644 --- a/crates/sophus_viewer/Cargo.toml +++ b/crates/sophus_viewer/Cargo.toml @@ -11,7 +11,12 @@ repository.workspace = true version.workspace = true [dependencies] +sophus_autodiff.workspace = true +sophus_image.workspace = true +sophus_lie.workspace = true +sophus_opt.workspace = true sophus_renderer.workspace = true +sophus_sensor.workspace = true approx.workspace = true bytemuck.workspace = true @@ -23,11 +28,7 @@ linked-hash-map.workspace = true log.workspace = true num-traits.workspace = true thingbuf.workspace = true -sophus_autodiff.workspace = true -sophus_image.workspace = true -sophus_lie.workspace = true -sophus_opt.workspace = true -sophus_sensor.workspace = true + wgpu.workspace = true [features] @@ -36,5 +37,7 @@ std = [ "sophus_image/std", "sophus_lie/std", "sophus_opt/std", + "sophus_renderer/std", "sophus_sensor/std", ] +default = ["std"] diff --git a/crates/sophus_viewer/src/lib.rs b/crates/sophus_viewer/src/lib.rs index 6edd720..ba13622 100644 --- a/crates/sophus_viewer/src/lib.rs +++ b/crates/sophus_viewer/src/lib.rs @@ -33,7 +33,11 @@ pub mod prelude { pub(crate) use alloc::string::String; pub(crate) use alloc::string::ToString; pub(crate) use alloc::vec::Vec; - pub use sophus_renderer::prelude::*; + + pub use sophus_autodiff::prelude::*; + pub use sophus_image::prelude::*; + pub use sophus_lie::prelude::*; + pub use sophus_opt::prelude::*; extern crate alloc; }