diff --git a/Cargo.toml b/Cargo.toml index 195dda62..779c75b1 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,12 +1,15 @@ [workspace] members = [ "crates/sophus", - "crates/sophus_core", - "crates/sophus_lie", + "crates/sophus_autodiff", + "crates/sophus_tensor", "crates/sophus_image", + "crates/sophus_lie", "crates/sophus_geo", + "crates/sophus_spline", "crates/sophus_sensor", "crates/sophus_opt", + "crates/sophus_timeseries", "crates/sophus_renderer", "crates/sophus_viewer", "crates/sophus_sim", @@ -23,12 +26,15 @@ version = "0.11.0" [workspace.dependencies] sophus = { path = "crates/sophus", version = "0.11.0" } -sophus_core = { path = "crates/sophus_core", version = "0.11.0" } +sophus_autodiff = { path = "crates/sophus_autodiff", version = "0.11.0" } +sophus_tensor = { path = "crates/sophus_tensor", version = "0.11.0" } sophus_image = { path = "crates/sophus_image", version = "0.11.0" } sophus_lie = { path = "crates/sophus_lie", version = "0.11.0" } sophus_geo = { path = "crates/sophus_geo", version = "0.11.0" } -sophus_opt = { path = "crates/sophus_opt", version = "0.11.0" } +sophus_spline = { path = "crates/sophus_spline", version = "0.11.0" } sophus_sensor = { path = "crates/sophus_sensor", version = "0.11.0" } +sophus_opt = { path = "crates/sophus_opt", version = "0.11.0" } +sophus_timeseries = { path = "crates/sophus_timeseries", version = "0.11.0" } sophus_renderer = { path = "crates/sophus_renderer", version = "0.11.0" } sophus_sim = { path = "crates/sophus_sim", version = "0.11.0" } sophus_viewer = { path = "crates/sophus_viewer", version = "0.11.0" } diff --git a/crates/sophus/Cargo.toml b/crates/sophus/Cargo.toml index cd6724b7..655e0e66 100644 --- a/crates/sophus/Cargo.toml +++ b/crates/sophus/Cargo.toml @@ -11,7 +11,7 @@ repository.workspace = true version.workspace = true [dependencies] -sophus_core.workspace = true +sophus_autodiff.workspace = true sophus_geo.workspace = true sophus_image.workspace = true sophus_lie.workspace = true @@ -37,14 +37,14 @@ wgpu.workspace = true [features] simd = [ - "sophus_core/simd", + "sophus_autodiff/simd", "sophus_image/simd", "sophus_lie/simd", "sophus_opt/simd", "sophus_sensor/simd", ] std = [ - "sophus_core/std", + "sophus_autodiff/std", "sophus_image/std", "sophus_lie/std", "sophus_opt/std", diff --git a/crates/sophus/src/examples/viewer_example.rs b/crates/sophus/src/examples/viewer_example.rs index 178b753e..0309ed45 100644 --- a/crates/sophus/src/examples/viewer_example.rs +++ b/crates/sophus/src/examples/viewer_example.rs @@ -1,6 +1,6 @@ -use sophus_core::linalg::SVec; -use sophus_core::linalg::VecF64; -use sophus_core::prelude::IsVector; +use sophus_autodiff::linalg::SVec; +use sophus_autodiff::linalg::VecF64; +use sophus_autodiff::prelude::IsVector; use sophus_image::arc_image::ArcImage4U8; use sophus_image::mut_image::MutImage4U8; use sophus_image::mut_image_view::IsMutImageView; diff --git a/crates/sophus/src/lib.rs b/crates/sophus/src/lib.rs index e5325644..074629a6 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_core as core; +pub use sophus_autodiff as core; #[doc(inline)] pub use sophus_geo as geo; #[doc(inline)] diff --git a/crates/sophus_core/Cargo.toml b/crates/sophus_autodiff/Cargo.toml similarity index 91% rename from crates/sophus_core/Cargo.toml rename to crates/sophus_autodiff/Cargo.toml index 226fcb26..9f6a2cc3 100644 --- a/crates/sophus_core/Cargo.toml +++ b/crates/sophus_autodiff/Cargo.toml @@ -1,6 +1,6 @@ [package] description = "sophus - geometry for robotics and computer vision" -name = "sophus_core" +name = "sophus_autodiff" readme = "../../README.md" edition.workspace = true @@ -12,7 +12,6 @@ version.workspace = true [dependencies] approx.workspace = true -concat-arrays.workspace = true log.workspace = true nalgebra.workspace = true ndarray.workspace = true diff --git a/crates/sophus_core/src/calculus/dual.rs b/crates/sophus_autodiff/src/dual.rs similarity index 59% rename from crates/sophus_core/src/calculus/dual.rs rename to crates/sophus_autodiff/src/dual.rs index 2675a5c0..d54746ca 100644 --- a/crates/sophus_core/src/calculus/dual.rs +++ b/crates/sophus_autodiff/src/dual.rs @@ -21,11 +21,11 @@ pub mod scalar; pub mod vector; #[cfg(feature = "simd")] -pub use crate::calculus::dual::dual_batch_matrix::DualBatchMatrix; +pub use crate::dual::dual_batch_matrix::DualBatchMatrix; #[cfg(feature = "simd")] -pub use crate::calculus::dual::dual_batch_scalar::DualBatchScalar; +pub use crate::dual::dual_batch_scalar::DualBatchScalar; #[cfg(feature = "simd")] -pub use crate::calculus::dual::dual_batch_vector::DualBatchVector; -pub use crate::calculus::dual::dual_matrix::DualMatrix; -pub use crate::calculus::dual::dual_scalar::DualScalar; -pub use crate::calculus::dual::dual_vector::DualVector; +pub use crate::dual::dual_batch_vector::DualBatchVector; +pub use crate::dual::dual_matrix::DualMatrix; +pub use crate::dual::dual_scalar::DualScalar; +pub use crate::dual::dual_vector::DualVector; diff --git a/crates/sophus_core/src/calculus/dual/dual_batch_matrix.rs b/crates/sophus_autodiff/src/dual/dual_batch_matrix.rs similarity index 99% rename from crates/sophus_core/src/calculus/dual/dual_batch_matrix.rs rename to crates/sophus_autodiff/src/dual/dual_batch_matrix.rs index aca17436..2abb61a3 100644 --- a/crates/sophus_core/src/calculus/dual/dual_batch_matrix.rs +++ b/crates/sophus_autodiff/src/dual/dual_batch_matrix.rs @@ -1,5 +1,5 @@ -use crate::calculus::dual::dual_batch_scalar::DualBatchScalar; -use crate::calculus::dual::DualBatchVector; +use crate::dual::dual_batch_scalar::DualBatchScalar; +use crate::dual::DualBatchVector; use crate::linalg::batch_mask::BatchMask; use crate::linalg::BatchMatF64; use crate::linalg::BatchScalarF64; diff --git a/crates/sophus_core/src/calculus/dual/dual_batch_scalar.rs b/crates/sophus_autodiff/src/dual/dual_batch_scalar.rs similarity index 99% rename from crates/sophus_core/src/calculus/dual/dual_batch_scalar.rs rename to crates/sophus_autodiff/src/dual/dual_batch_scalar.rs index 437f6093..3d1a9459 100644 --- a/crates/sophus_core/src/calculus/dual/dual_batch_scalar.rs +++ b/crates/sophus_autodiff/src/dual/dual_batch_scalar.rs @@ -1,6 +1,6 @@ use super::dual_scalar::DualScalar; -pub use crate::calculus::dual::dual_batch_matrix::DualBatchMatrix; -pub use crate::calculus::dual::dual_batch_vector::DualBatchVector; +pub use crate::dual::dual_batch_matrix::DualBatchMatrix; +pub use crate::dual::dual_batch_vector::DualBatchVector; use crate::linalg::batch_mask::BatchMask; use crate::linalg::scalar::NumberCategory; use crate::linalg::BatchMatF64; diff --git a/crates/sophus_core/src/calculus/dual/dual_batch_vector.rs b/crates/sophus_autodiff/src/dual/dual_batch_vector.rs similarity index 98% rename from crates/sophus_core/src/calculus/dual/dual_batch_vector.rs rename to crates/sophus_autodiff/src/dual/dual_batch_vector.rs index 76cd5eaf..1f6c713a 100644 --- a/crates/sophus_core/src/calculus/dual/dual_batch_vector.rs +++ b/crates/sophus_autodiff/src/dual/dual_batch_vector.rs @@ -1,7 +1,7 @@ use super::vector::HasJacobian; use super::vector::VectorValuedDerivative; -use crate::calculus::dual::dual_batch_scalar::DualBatchScalar; -use crate::calculus::dual::DualBatchMatrix; +use crate::dual::dual_batch_scalar::DualBatchScalar; +use crate::dual::DualBatchMatrix; use crate::linalg::batch_mask::BatchMask; use crate::linalg::BatchMatF64; use crate::linalg::BatchScalarF64; diff --git a/crates/sophus_core/src/calculus/dual/dual_matrix.rs b/crates/sophus_autodiff/src/dual/dual_matrix.rs similarity index 99% rename from crates/sophus_core/src/calculus/dual/dual_matrix.rs rename to crates/sophus_autodiff/src/dual/dual_matrix.rs index 09701476..b3599b02 100644 --- a/crates/sophus_core/src/calculus/dual/dual_matrix.rs +++ b/crates/sophus_autodiff/src/dual/dual_matrix.rs @@ -1,5 +1,5 @@ -use crate::calculus::dual::DualScalar; -use crate::calculus::dual::DualVector; +use crate::dual::DualScalar; +use crate::dual::DualVector; use crate::linalg::MatF64; use crate::linalg::SMat; use crate::prelude::*; diff --git a/crates/sophus_core/src/calculus/dual/dual_scalar.rs b/crates/sophus_autodiff/src/dual/dual_scalar.rs similarity index 100% rename from crates/sophus_core/src/calculus/dual/dual_scalar.rs rename to crates/sophus_autodiff/src/dual/dual_scalar.rs diff --git a/crates/sophus_core/src/calculus/dual/dual_vector.rs b/crates/sophus_autodiff/src/dual/dual_vector.rs similarity index 100% rename from crates/sophus_core/src/calculus/dual/dual_vector.rs rename to crates/sophus_autodiff/src/dual/dual_vector.rs diff --git a/crates/sophus_core/src/calculus/dual/matrix.rs b/crates/sophus_autodiff/src/dual/matrix.rs similarity index 97% rename from crates/sophus_core/src/calculus/dual/matrix.rs rename to crates/sophus_autodiff/src/dual/matrix.rs index ea540698..df169916 100644 --- a/crates/sophus_core/src/calculus/dual/matrix.rs +++ b/crates/sophus_autodiff/src/dual/matrix.rs @@ -59,12 +59,12 @@ pub trait IsDualMatrix< #[test] fn dual_matrix_tests() { #[cfg(feature = "simd")] - use crate::calculus::dual::DualBatchScalar; - use crate::calculus::dual::DualScalar; - use crate::calculus::maps::matrix_valued_maps::MatrixValuedMatrixMap; + use crate::dual::DualBatchScalar; + use crate::dual::DualScalar; #[cfg(feature = "simd")] use crate::linalg::BatchScalarF64; use crate::linalg::EPS_F64; + use crate::maps::matrix_valued_maps::MatrixValuedMatrixMap; use crate::prelude::IsScalar; #[cfg(test)] diff --git a/crates/sophus_core/src/calculus/dual/scalar.rs b/crates/sophus_autodiff/src/dual/scalar.rs similarity index 99% rename from crates/sophus_core/src/calculus/dual/scalar.rs rename to crates/sophus_autodiff/src/dual/scalar.rs index e2de7975..2fe867fe 100644 --- a/crates/sophus_core/src/calculus/dual/scalar.rs +++ b/crates/sophus_autodiff/src/dual/scalar.rs @@ -41,12 +41,12 @@ pub trait IsDualScalar: #[test] fn dual_scalar_tests() { #[cfg(feature = "simd")] - use crate::calculus::dual::DualBatchScalar; - use crate::calculus::dual::DualScalar; - use crate::calculus::maps::curves::ScalarValuedCurve; + use crate::dual::DualBatchScalar; + use crate::dual::DualScalar; #[cfg(feature = "simd")] use crate::linalg::BatchScalarF64; use crate::linalg::EPS_F64; + use crate::maps::curves::ScalarValuedCurve; trait DualScalarTest { fn run_dual_scalar_test(); diff --git a/crates/sophus_core/src/calculus/dual/vector.rs b/crates/sophus_autodiff/src/dual/vector.rs similarity index 96% rename from crates/sophus_core/src/calculus/dual/vector.rs rename to crates/sophus_autodiff/src/dual/vector.rs index 1902363f..6f670208 100644 --- a/crates/sophus_core/src/calculus/dual/vector.rs +++ b/crates/sophus_autodiff/src/dual/vector.rs @@ -68,19 +68,18 @@ pub trait HasJacobian< #[test] fn dual_vector_tests() { - use crate::calculus::dual::dual_scalar::DualScalar; - use crate::calculus::maps::scalar_valued_maps::ScalarValuedVectorMap; - use crate::calculus::maps::vector_valued_maps::VectorValuedVectorMap; + use crate::dual::dual_scalar::DualScalar; + #[cfg(feature = "simd")] + use crate::dual::DualBatchScalar; use crate::linalg::vector::IsVector; + #[cfg(feature = "simd")] + use crate::linalg::BatchScalarF64; use crate::linalg::EPS_F64; + use crate::maps::scalar_valued_maps::ScalarValuedVectorMap; + use crate::maps::vector_valued_maps::VectorValuedVectorMap; use crate::points::example_points; use crate::prelude::IsScalar; - #[cfg(feature = "simd")] - use crate::calculus::dual::DualBatchScalar; - #[cfg(feature = "simd")] - use crate::linalg::BatchScalarF64; - #[cfg(test)] trait Test { fn run(); diff --git a/crates/sophus_core/src/floating_point.rs b/crates/sophus_autodiff/src/floating_point.rs similarity index 100% rename from crates/sophus_core/src/floating_point.rs rename to crates/sophus_autodiff/src/floating_point.rs diff --git a/crates/sophus_core/src/lib.rs b/crates/sophus_autodiff/src/lib.rs similarity index 54% rename from crates/sophus_core/src/lib.rs rename to crates/sophus_autodiff/src/lib.rs index a9dc293f..1585a790 100644 --- a/crates/sophus_core/src/lib.rs +++ b/crates/sophus_autodiff/src/lib.rs @@ -2,65 +2,46 @@ #![deny(missing_docs)] #![no_std] #![allow(clippy::needless_range_loop)] -//! Core math functionality including +//! Automatic differentiation module //! - linear algebra types //! * such as [linalg::VecF64], and [linalg::MatF64] //! * batch types such as [linalg::BatchScalarF64], [linalg::BatchVecF64], //! [linalg::BatchMatF64] - require the `simd` feature -//! - tensors -//! * design: dynamic tensor (ndarray) of static tensors (nalgebra) -//! - differentiation tools -//! * dual numbers: [calculus::dual::DualScalar], [calculus::dual::DualVector], +//! - dual numbers: [calculus::dual::DualScalar], [calculus::dual::DualVector], //! [calculus::dual::DualMatrix] //! * [calculus::maps::curves] f: ℝ -> ℝ, f: ℝ -> ℝʳ, f: ℝ -> ℝʳ x ℝᶜ //! * [calculus::maps::scalar_valued_maps]: f: ℝᵐ -> ℝ, f: ℝᵐ x ℝⁿ -> ℝ //! * [calculus::maps::vector_valued_maps]: f: ℝᵐ -> ℝᵖ, f: ℝᵐ x ℝⁿ -> ℝᵖ //! * [calculus::maps::matrix_valued_maps]: f: ℝᵐ -> ℝʳ x ℝᶜ, f: ℝᵐ x ℝⁿ -> ℝʳ x ℝᶜ -//! - splines -//! * [calculus::spline::CubicBSpline] -//! - intervals, regions -//! * closed interval: [calculus::region::Interval] -//! * closed region: [calculus::region::Interval] -//! - manifolds: [manifold::traits] #[cfg(feature = "std")] extern crate std; -/// calculus - differentiation, splines, and more -pub mod calculus; +/// dual numbers - for automatic differentiation +pub mod dual; /// floating point pub mod floating_point; -/// linear algebra types +/// core linear algebra types pub mod linalg; /// manifolds pub mod manifold; +/// curves, scalar-valued, vector-valued, and matrix-valued maps +pub mod maps; /// params pub mod params; /// points pub mod points; -/// time series -pub mod time_series; - -pub use crate::manifold::*; -pub use crate::params::*; -pub use crate::points::*; pub use nalgebra; pub use ndarray; -/// tensors -pub mod tensor; -pub use crate::tensor::arc_tensor::*; -pub use crate::tensor::mut_tensor::*; -pub use crate::tensor::mut_tensor_view::*; -pub use crate::tensor::tensor_view::*; +pub use crate::points::*; -/// sophus_core prelude +/// sophus_autodiff prelude pub mod prelude { - pub use crate::calculus::dual::matrix::IsDualMatrix; - pub use crate::calculus::dual::scalar::IsDualScalar; - pub use crate::calculus::dual::vector::IsDualVector; - pub use crate::calculus::region::IsRegion; + pub use crate::dual::matrix::IsDualMatrix; + pub use crate::dual::scalar::IsDualScalar; + pub use crate::dual::vector::IsDualVector; pub use crate::linalg::bool_mask::IsBoolMask; pub use crate::linalg::matrix::IsMatrix; pub use crate::linalg::matrix::IsRealMatrix; @@ -72,10 +53,8 @@ pub mod prelude { pub use crate::linalg::vector::IsRealVector; pub use crate::linalg::vector::IsSingleVector; pub use crate::linalg::vector::IsVector; - pub use crate::manifold::traits::IsManifold; + pub use crate::manifold::IsManifold; + pub use crate::manifold::IsTangent; pub use crate::params::HasParams; - pub use crate::tensor::element::IsStaticTensor; - pub use crate::tensor::mut_tensor_view::IsMutTensorLike; - pub use crate::tensor::tensor_view::IsTensorLike; - pub use crate::tensor::tensor_view::IsTensorView; + pub use crate::params::IsParamsImpl; } diff --git a/crates/sophus_core/src/linalg.rs b/crates/sophus_autodiff/src/linalg.rs similarity index 100% rename from crates/sophus_core/src/linalg.rs rename to crates/sophus_autodiff/src/linalg.rs diff --git a/crates/sophus_core/src/linalg/batch_mask.rs b/crates/sophus_autodiff/src/linalg/batch_mask.rs similarity index 100% rename from crates/sophus_core/src/linalg/batch_mask.rs rename to crates/sophus_autodiff/src/linalg/batch_mask.rs diff --git a/crates/sophus_core/src/linalg/batch_matrix.rs b/crates/sophus_autodiff/src/linalg/batch_matrix.rs similarity index 99% rename from crates/sophus_core/src/linalg/batch_matrix.rs rename to crates/sophus_autodiff/src/linalg/batch_matrix.rs index ec5928bf..61c87964 100644 --- a/crates/sophus_core/src/linalg/batch_matrix.rs +++ b/crates/sophus_autodiff/src/linalg/batch_matrix.rs @@ -1,4 +1,4 @@ -use crate::calculus::dual::DualBatchMatrix; +use crate::dual::DualBatchMatrix; use crate::linalg::BatchMatF64; use crate::linalg::BatchScalarF64; use crate::linalg::BatchVecF64; diff --git a/crates/sophus_core/src/linalg/batch_scalar.rs b/crates/sophus_autodiff/src/linalg/batch_scalar.rs similarity index 98% rename from crates/sophus_core/src/linalg/batch_scalar.rs rename to crates/sophus_autodiff/src/linalg/batch_scalar.rs index 596a19a4..c4627064 100644 --- a/crates/sophus_core/src/linalg/batch_scalar.rs +++ b/crates/sophus_autodiff/src/linalg/batch_scalar.rs @@ -1,6 +1,6 @@ -use crate::calculus::dual::DualBatchMatrix; -use crate::calculus::dual::DualBatchScalar; -use crate::calculus::dual::DualBatchVector; +use crate::dual::DualBatchMatrix; +use crate::dual::DualBatchScalar; +use crate::dual::DualBatchVector; use crate::linalg::scalar::IsBatchScalar; use crate::linalg::scalar::NumberCategory; use crate::linalg::BatchMatF64; diff --git a/crates/sophus_core/src/linalg/batch_vector.rs b/crates/sophus_autodiff/src/linalg/batch_vector.rs similarity index 98% rename from crates/sophus_core/src/linalg/batch_vector.rs rename to crates/sophus_autodiff/src/linalg/batch_vector.rs index d1be2536..2d696684 100644 --- a/crates/sophus_core/src/linalg/batch_vector.rs +++ b/crates/sophus_autodiff/src/linalg/batch_vector.rs @@ -1,4 +1,4 @@ -use crate::calculus::dual::DualBatchVector; +use crate::dual::DualBatchVector; use crate::linalg::BatchMatF64; use crate::linalg::BatchScalarF64; use crate::linalg::BatchVecF64; diff --git a/crates/sophus_core/src/linalg/bool_mask.rs b/crates/sophus_autodiff/src/linalg/bool_mask.rs similarity index 100% rename from crates/sophus_core/src/linalg/bool_mask.rs rename to crates/sophus_autodiff/src/linalg/bool_mask.rs diff --git a/crates/sophus_core/src/linalg/matrix.rs b/crates/sophus_autodiff/src/linalg/matrix.rs similarity index 99% rename from crates/sophus_core/src/linalg/matrix.rs rename to crates/sophus_autodiff/src/linalg/matrix.rs index 4cc2f9d3..f919d73c 100644 --- a/crates/sophus_core/src/linalg/matrix.rs +++ b/crates/sophus_autodiff/src/linalg/matrix.rs @@ -1,4 +1,4 @@ -use crate::calculus::dual::DualMatrix; +use crate::dual::dual_matrix::DualMatrix; use crate::linalg::MatF64; use crate::linalg::VecF64; use crate::prelude::*; diff --git a/crates/sophus_core/src/linalg/scalar.rs b/crates/sophus_autodiff/src/linalg/scalar.rs similarity index 98% rename from crates/sophus_core/src/linalg/scalar.rs rename to crates/sophus_autodiff/src/linalg/scalar.rs index 615c6ecc..da49f7fc 100644 --- a/crates/sophus_core/src/linalg/scalar.rs +++ b/crates/sophus_autodiff/src/linalg/scalar.rs @@ -1,6 +1,6 @@ -use crate::calculus::dual::DualMatrix; -use crate::calculus::dual::DualScalar; -use crate::calculus::dual::DualVector; +use crate::dual::dual_matrix::DualMatrix; +use crate::dual::dual_scalar::DualScalar; +use crate::dual::dual_vector::DualVector; use crate::linalg::MatF64; use crate::linalg::VecF64; use crate::linalg::EPS_F64; diff --git a/crates/sophus_core/src/linalg/vector.rs b/crates/sophus_autodiff/src/linalg/vector.rs similarity index 99% rename from crates/sophus_core/src/linalg/vector.rs rename to crates/sophus_autodiff/src/linalg/vector.rs index 0e12013e..3ddb5ba0 100644 --- a/crates/sophus_core/src/linalg/vector.rs +++ b/crates/sophus_autodiff/src/linalg/vector.rs @@ -1,4 +1,4 @@ -use crate::calculus::dual::DualVector; +use crate::dual::dual_vector::DualVector; use crate::linalg::MatF64; use crate::linalg::VecF64; use crate::prelude::*; diff --git a/crates/sophus_core/src/manifold/traits.rs b/crates/sophus_autodiff/src/manifold.rs similarity index 61% rename from crates/sophus_core/src/manifold/traits.rs rename to crates/sophus_autodiff/src/manifold.rs index 48f1f7ec..1fd39e70 100644 --- a/crates/sophus_core/src/manifold/traits.rs +++ b/crates/sophus_autodiff/src/manifold.rs @@ -1,10 +1,10 @@ use crate::linalg::VecF64; -use crate::params::ParamsImpl; -use crate::prelude::*; +use crate::params::HasParams; +use crate::prelude::IsScalar; extern crate alloc; /// A tangent implementation. -pub trait TangentImpl< +pub trait IsTangent< S: IsScalar, const DOF: usize, const BATCH: usize, @@ -16,22 +16,6 @@ pub trait TangentImpl< fn tangent_examples() -> alloc::vec::Vec>; } -/// A manifold implementation. -pub trait ManifoldImpl< - S: IsScalar, - const DOF: usize, - const PARAMS: usize, - const BATCH: usize, - const DM: usize, - const DN: usize, ->: ParamsImpl + TangentImpl -{ - /// o-plus operation. - fn oplus(params: &S::Vector, tangent: &S::Vector) -> S::Vector; - /// o-minus operation. - fn ominus(params1: &S::Vector, params2: &S::Vector) -> S::Vector; -} - /// A manifold. pub trait IsManifold< S: IsScalar, diff --git a/crates/sophus_autodiff/src/maps.rs b/crates/sophus_autodiff/src/maps.rs new file mode 100644 index 00000000..bb858a67 --- /dev/null +++ b/crates/sophus_autodiff/src/maps.rs @@ -0,0 +1,18 @@ +/// curve - a function from ℝ to M, where M is a manifold +pub mod curves; +/// matrix-valued map - a function from M to ℝʳ x ℝᶜ, where M is a manifold +pub mod matrix_valued_maps; +/// scalar-valued map - a function from M to ℝ, where M is a manifold +pub mod scalar_valued_maps; +/// vector-valued map - a function from M to ℝⁿ, where M is a manifold +pub mod vector_valued_maps; + +pub use crate::maps::curves::MatrixValuedCurve; +pub use crate::maps::curves::ScalarValuedCurve; +pub use crate::maps::curves::VectorValuedCurve; +pub use crate::maps::matrix_valued_maps::MatrixValuedMatrixMap; +pub use crate::maps::matrix_valued_maps::MatrixValuedVectorMap; +pub use crate::maps::scalar_valued_maps::ScalarValuedMatrixMap; +pub use crate::maps::scalar_valued_maps::ScalarValuedVectorMap; +pub use crate::maps::vector_valued_maps::VectorValuedMatrixMap; +pub use crate::maps::vector_valued_maps::VectorValuedVectorMap; diff --git a/crates/sophus_core/src/calculus/maps/curves.rs b/crates/sophus_autodiff/src/maps/curves.rs similarity index 99% rename from crates/sophus_core/src/calculus/maps/curves.rs rename to crates/sophus_autodiff/src/maps/curves.rs index c8b339b6..16e140bd 100644 --- a/crates/sophus_core/src/calculus/maps/curves.rs +++ b/crates/sophus_autodiff/src/maps/curves.rs @@ -142,14 +142,13 @@ impl, const BATCH: usize> MatrixValuedCurve, const PARAMS: usize, const BATCH: usize, @@ -34,7 +33,7 @@ pub trait HasParams< const BATCH: usize, const DM: usize, const DN: usize, ->: ParamsImpl +>: IsParamsImpl { /// Create from parameters. fn from_params

(params: P) -> Self @@ -48,7 +47,7 @@ pub trait HasParams< fn params(&self) -> &S::Vector; } -impl ParamsImpl for VecF64 { +impl IsParamsImpl for VecF64 { fn are_params_valid

(_params: P) -> bool where P: Borrow>, diff --git a/crates/sophus_core/src/points.rs b/crates/sophus_autodiff/src/points.rs similarity index 100% rename from crates/sophus_core/src/points.rs rename to crates/sophus_autodiff/src/points.rs diff --git a/crates/sophus_core/src/calculus.rs b/crates/sophus_core/src/calculus.rs deleted file mode 100644 index 7a59f864..00000000 --- a/crates/sophus_core/src/calculus.rs +++ /dev/null @@ -1,18 +0,0 @@ -#![deny(missing_docs)] -//! # Calculus module - -/// dual numbers - for automatic differentiation -pub mod dual; -/// curves, scalar-valued, vector-valued, and matrix-valued maps -pub mod maps; -/// intervals and regions -pub mod region; -/// splines -pub mod spline; - -pub use crate::calculus::region::IInterval; -pub use crate::calculus::region::IRegion; -pub use crate::calculus::region::Interval; -pub use crate::calculus::region::Region; -pub use crate::calculus::spline::CubicBSpline; -pub use crate::calculus::spline::CubicBSplineParams; diff --git a/crates/sophus_core/src/calculus/maps.rs b/crates/sophus_core/src/calculus/maps.rs deleted file mode 100644 index f8528c0e..00000000 --- a/crates/sophus_core/src/calculus/maps.rs +++ /dev/null @@ -1,20 +0,0 @@ -/// curve - a function from ℝ to M, where M is a manifold -pub mod curves; -pub use crate::calculus::maps::curves::MatrixValuedCurve; -pub use crate::calculus::maps::curves::ScalarValuedCurve; -pub use crate::calculus::maps::curves::VectorValuedCurve; - -/// scalar-valued map - a function from M to ℝ, where M is a manifold -pub mod scalar_valued_maps; -pub use crate::calculus::maps::scalar_valued_maps::ScalarValuedMatrixMap; -pub use crate::calculus::maps::scalar_valued_maps::ScalarValuedVectorMap; - -/// vector-valued map - a function from M to ℝⁿ, where M is a manifold -pub mod vector_valued_maps; -pub use crate::calculus::maps::vector_valued_maps::VectorValuedMatrixMap; -pub use crate::calculus::maps::vector_valued_maps::VectorValuedVectorMap; - -/// matrix-valued map - a function from M to ℝʳ x ℝᶜ, where M is a manifold -pub mod matrix_valued_maps; -pub use crate::calculus::maps::matrix_valued_maps::MatrixValuedMatrixMap; -pub use crate::calculus::maps::matrix_valued_maps::MatrixValuedVectorMap; diff --git a/crates/sophus_core/src/manifold.rs b/crates/sophus_core/src/manifold.rs deleted file mode 100644 index 3df267de..00000000 --- a/crates/sophus_core/src/manifold.rs +++ /dev/null @@ -1,5 +0,0 @@ -/// manifolds -pub mod traits; - -/// unit vector -pub mod unit_vector; diff --git a/crates/sophus_core/src/tensor.rs b/crates/sophus_core/src/tensor.rs deleted file mode 100644 index efc4b352..00000000 --- a/crates/sophus_core/src/tensor.rs +++ /dev/null @@ -1,21 +0,0 @@ -#![deny(missing_docs)] -//! # Calculus module - -/// Arc tensor -pub mod arc_tensor; -pub use crate::tensor::arc_tensor::ArcTensor; - -/// Tensor element -pub mod element; - -/// Mutable tensor -pub mod mut_tensor; -pub use crate::tensor::mut_tensor::MutTensor; - -/// Mutable tensor view -pub mod mut_tensor_view; -pub use crate::tensor::mut_tensor_view::MutTensorView; - -/// Tensor view -pub mod tensor_view; -pub use crate::tensor::tensor_view::TensorView; diff --git a/crates/sophus_geo/Cargo.toml b/crates/sophus_geo/Cargo.toml index 8bf7a1bf..a19762ee 100644 --- a/crates/sophus_geo/Cargo.toml +++ b/crates/sophus_geo/Cargo.toml @@ -12,8 +12,8 @@ version.workspace = true [dependencies] approx.workspace = true -sophus_core.workspace = true +sophus_autodiff.workspace = true sophus_lie.workspace = true [features] -simd = ["sophus_core/simd"] +simd = ["sophus_lie/simd"] diff --git a/crates/sophus_geo/src/hyperplane.rs b/crates/sophus_geo/src/hyperplane.rs index de391c0b..f458b81b 100644 --- a/crates/sophus_geo/src/hyperplane.rs +++ b/crates/sophus_geo/src/hyperplane.rs @@ -1,13 +1,12 @@ -use std::borrow::Borrow; - -use sophus_core::linalg::MatF64; -use sophus_core::linalg::VecF64; -use sophus_core::unit_vector::UnitVector; +use crate::unit_vector::UnitVector; +use sophus_autodiff::linalg::MatF64; +use sophus_autodiff::linalg::VecF64; use sophus_lie::prelude::*; use sophus_lie::Isometry2; use sophus_lie::Isometry2F64; use sophus_lie::Isometry3; use sophus_lie::Isometry3F64; +use std::borrow::Borrow; /// N-dimensional Hyperplane. pub struct HyperPlane< @@ -181,10 +180,10 @@ impl PlaneF64 { #[test] fn plane_test() { - use sophus_core::calculus::dual::DualScalar; - use sophus_core::calculus::maps::VectorValuedVectorMap; - use sophus_core::linalg::VecF64; - use sophus_core::linalg::EPS_F64; + use sophus_autodiff::dual::DualScalar; + use sophus_autodiff::linalg::VecF64; + use sophus_autodiff::linalg::EPS_F64; + use sophus_autodiff::maps::VectorValuedVectorMap; { let plane = Plane::::from_isometry3(Isometry3::rot_y(0.2)); @@ -201,11 +200,11 @@ fn plane_test() { plane.proj_onto(v) } - let a: sophus_core::nalgebra::Matrix< + let a: sophus_autodiff::nalgebra::Matrix< f64, - sophus_core::nalgebra::Const<3>, - sophus_core::nalgebra::Const<1>, - sophus_core::nalgebra::ArrayStorage, + sophus_autodiff::nalgebra::Const<3>, + sophus_autodiff::nalgebra::Const<1>, + sophus_autodiff::nalgebra::ArrayStorage, > = VecF64::<3>::new(1.0, 2.0, 3.0); let finite_diff = VectorValuedVectorMap::::sym_diff_quotient_jacobian( proj_x_onto::, diff --git a/crates/sophus_geo/src/hypersphere.rs b/crates/sophus_geo/src/hypersphere.rs index a0c789e9..e851fb8b 100644 --- a/crates/sophus_geo/src/hypersphere.rs +++ b/crates/sophus_geo/src/hypersphere.rs @@ -1,4 +1,4 @@ -use sophus_core::linalg::EPS_F64; +use sophus_autodiff::linalg::EPS_F64; use sophus_lie::prelude::*; use crate::ray::Ray; diff --git a/crates/sophus_geo/src/lib.rs b/crates/sophus_geo/src/lib.rs index 5f101ab8..058eaad3 100644 --- a/crates/sophus_geo/src/lib.rs +++ b/crates/sophus_geo/src/lib.rs @@ -4,3 +4,13 @@ pub mod hyperplane; pub mod hypersphere; /// ray pub mod ray; +/// region +pub mod region; +/// unit vector +pub mod unit_vector; + +/// sophus_geo prelude +pub mod prelude { + pub use crate::region::IsRegion; + pub use sophus_autodiff::prelude::*; +} diff --git a/crates/sophus_core/src/calculus/manifold.rs b/crates/sophus_geo/src/manifold.rs similarity index 100% rename from crates/sophus_core/src/calculus/manifold.rs rename to crates/sophus_geo/src/manifold.rs diff --git a/crates/sophus_geo/src/ray.rs b/crates/sophus_geo/src/ray.rs index f273cb66..79b6d57e 100644 --- a/crates/sophus_geo/src/ray.rs +++ b/crates/sophus_geo/src/ray.rs @@ -1,4 +1,4 @@ -use sophus_core::unit_vector::UnitVector; +use crate::unit_vector::UnitVector; use sophus_lie::prelude::IsScalar; use sophus_lie::prelude::IsSingleScalar; use sophus_lie::prelude::IsVector; diff --git a/crates/sophus_core/src/calculus/region.rs b/crates/sophus_geo/src/region.rs similarity index 82% rename from crates/sophus_core/src/calculus/region.rs rename to crates/sophus_geo/src/region.rs index 98e52063..227d0e01 100644 --- a/crates/sophus_core/src/calculus/region.rs +++ b/crates/sophus_geo/src/region.rs @@ -1,6 +1,5 @@ -use nalgebra::SVector; - -use crate::IsPoint; +use sophus_autodiff::linalg::SVec; +use sophus_autodiff::IsPoint; /// Floating-point interval #[derive(Debug, Copy, Clone, PartialEq)] @@ -20,14 +19,14 @@ pub struct IInterval { #[derive(Debug, Copy, Clone, PartialEq)] pub struct Region { /// min and max of the region - pub min_max: Option<(SVector, SVector)>, + pub min_max: Option<(SVec, SVec)>, } /// Integer Region - n-dimensional interval #[derive(Debug, Copy, Clone, PartialEq)] pub struct IRegion { /// min and max of the region - pub min_max: Option<(SVector, SVector)>, + pub min_max: Option<(SVec, SVec)>, } impl IRegion { @@ -38,8 +37,8 @@ impl IRegion { } // example: [2, 5] -> [1.5, 5.5] Region::from_min_max( - self.min().cast() - SVector::repeat(0.5), - self.max().cast() + SVector::repeat(0.5), + self.min().cast() - SVec::repeat(0.5), + self.max().cast() + SVec::repeat(0.5), ) } } @@ -201,10 +200,10 @@ impl IsRegion<1, f64> for Interval { } } -impl IsRegion> for Region { +impl IsRegion> for Region { fn unbounded() -> Self { - let s: SVector = SVector::::smallest(); - let l: SVector = SVector::::largest(); + let s: SVec = SVec::::smallest(); + let l: SVec = SVec::::largest(); Self::from_min_max(s, l) } @@ -229,41 +228,41 @@ impl IsRegion> for Region { if self.is_empty() { return false; } - self.min() == SVector::::smallest() && self.max() == SVector::::largest() + self.min() == SVec::::smallest() && self.max() == SVec::::largest() } - fn from_min_max(min: SVector, max: SVector) -> Self { + fn from_min_max(min: SVec, max: SVec) -> Self { Self { min_max: Option::Some((min, max)), } } - fn try_min(&self) -> Option> { + fn try_min(&self) -> Option> { Some(self.min_max?.0) } - fn try_max(&self) -> Option> { + fn try_max(&self) -> Option> { Some(self.min_max?.1) } - fn clamp(&self, p: SVector) -> SVector { + fn clamp(&self, p: SVec) -> SVec { p.clamp(self.min(), self.max()) } - fn range(&self) -> SVector { - let p: SVector; + fn range(&self) -> SVec { + let p: SVec; if self.is_empty() { - p = SVector::::zeros(); + p = SVec::::zeros(); return p; } p = self.max() - self.min(); p } - fn mid(&self) -> SVector { + fn mid(&self) -> SVec { self.min() + 0.5 * self.range() } - fn extend(&mut self, point: &SVector) { + fn extend(&mut self, point: &SVec) { if self.is_empty() { *self = Self::from_point(*point); } @@ -340,10 +339,10 @@ impl IsRegion<1, i64> for IInterval { } } -impl IsRegion> for IRegion { +impl IsRegion> for IRegion { fn unbounded() -> Self { - let s: SVector = SVector::::smallest(); - let l: SVector = SVector::::largest(); + let s: SVec = SVec::::smallest(); + let l: SVec = SVec::::largest(); Self::from_min_max(s, l) } @@ -365,41 +364,41 @@ impl IsRegion> for IRegion { if self.is_empty() { return false; } - self.min() == SVector::::smallest() && self.max() == SVector::::largest() + self.min() == SVec::::smallest() && self.max() == SVec::::largest() } - fn from_min_max(min: SVector, max: SVector) -> Self { + fn from_min_max(min: SVec, max: SVec) -> Self { Self { min_max: Option::Some((min, max)), } } - fn try_min(&self) -> Option> { + fn try_min(&self) -> Option> { Some(self.min_max?.0) } - fn try_max(&self) -> Option> { + fn try_max(&self) -> Option> { Some(self.min_max?.1) } - fn clamp(&self, p: SVector) -> SVector { + fn clamp(&self, p: SVec) -> SVec { p.clamp(self.min(), self.max()) } - fn range(&self) -> SVector { - let p: SVector; + fn range(&self) -> SVec { + let p: SVec; if self.is_empty() { - p = SVector::::zeros(); + p = SVec::::zeros(); return p; } - p = self.max() - self.min() + SVector::::repeat(1); + p = self.max() - self.min() + SVec::::repeat(1); p } - fn mid(&self) -> SVector { + fn mid(&self) -> SVec { self.min() + self.range() / 2 } - fn extend(&mut self, point: &SVector) { + fn extend(&mut self, point: &SVec) { if self.is_empty() { *self = Self::from_point(*point); } @@ -428,13 +427,13 @@ mod tests { assert!(unbounded.is_proper()); assert!(unbounded.is_unbounded()); - let one_i64 = IRegion::<2>::from_point(SVector::::repeat(1)); + let one_i64 = IRegion::<2>::from_point(SVec::::repeat(1)); assert!(!one_i64.is_empty()); assert!(!one_i64.is_degenerated()); assert!(one_i64.is_proper()); assert!(!one_i64.is_unbounded()); - let two_f64 = Region::<2>::from_point(SVector::::repeat(2.0)); + let two_f64 = Region::<2>::from_point(SVec::::repeat(2.0)); assert!(!two_f64.is_empty()); assert!(two_f64.is_degenerated()); assert!(!two_f64.is_proper()); @@ -455,7 +454,7 @@ mod tests { assert!(unbounded.is_proper()); assert!(unbounded.is_unbounded()); - let one_i64 = IRegion::<1>::from_point(SVector::::repeat(1)); + let one_i64 = IRegion::<1>::from_point(SVec::::repeat(1)); assert!(!one_i64.is_empty()); assert!(!one_i64.is_degenerated()); assert!(one_i64.is_proper()); diff --git a/crates/sophus_core/src/manifold/unit_vector.rs b/crates/sophus_geo/src/unit_vector.rs similarity index 87% rename from crates/sophus_core/src/manifold/unit_vector.rs rename to crates/sophus_geo/src/unit_vector.rs index 2649ab0a..be55c0e8 100644 --- a/crates/sophus_core/src/manifold/unit_vector.rs +++ b/crates/sophus_geo/src/unit_vector.rs @@ -1,16 +1,7 @@ -use crate::linalg::matrix::IsMatrix; -use crate::linalg::vector::IsVector; -use crate::prelude::IsBoolMask; -use crate::prelude::IsScalar; -use crate::prelude::IsSingleScalar; -use crate::traits::IsManifold; -use crate::traits::ManifoldImpl; -use crate::traits::TangentImpl; -use crate::HasParams; -use crate::ParamsImpl; use core::borrow::Borrow; use core::marker::PhantomData; use core::ops::Neg; +use sophus_lie::prelude::*; extern crate alloc; #[derive(Clone, Debug, Copy)] @@ -100,7 +91,7 @@ impl< const BATCH: usize, const DM: usize, const DN: usize, - > TangentImpl for UnitVectorImpl + > IsTangent for UnitVectorImpl { fn tangent_examples() -> alloc::vec::Vec> { todo!() @@ -114,7 +105,7 @@ impl< const BATCH: usize, const DM: usize, const DN: usize, - > ParamsImpl for UnitVectorImpl + > IsParamsImpl for UnitVectorImpl { fn are_params_valid

(params: P) -> S::Mask where @@ -135,24 +126,6 @@ impl< } } -impl< - S: IsScalar, - const DOF: usize, - const DIM: usize, - const BATCH: usize, - const DM: usize, - const DN: usize, - > ManifoldImpl for UnitVectorImpl -{ - fn oplus(params: &S::Vector, tangent: &S::Vector) -> S::Vector { - Self::mat_rx(params) * Self::exp(tangent) - } - - fn ominus(lhs_params: &S::Vector, rhs_params: &S::Vector) -> S::Vector { - Self::log(&(Self::mat_rx(lhs_params).transposed() * *rhs_params)) - } -} - /// Unit vector #[derive(Clone, Debug)] pub struct UnitVector< @@ -309,7 +282,7 @@ impl< const BATCH: usize, const DM: usize, const DN: usize, - > ParamsImpl for UnitVector + > IsParamsImpl for UnitVector { fn are_params_valid

(params: P) -> S::Mask where @@ -365,11 +338,15 @@ impl< > IsManifold for UnitVector { fn oplus(&self, tangent: &S::Vector) -> Self { - let params = UnitVectorImpl::::oplus(&self.params, tangent); + let params = UnitVectorImpl::::mat_rx(&self.params) + * UnitVectorImpl::::exp(tangent); Self::from_params(params) } fn ominus(&self, rhs: &Self) -> S::Vector { - UnitVectorImpl::::ominus(&self.params, &rhs.params) + UnitVectorImpl::::log( + &(UnitVectorImpl::::mat_rx(rhs.params()).transposed() + * *rhs.params()), + ) } } diff --git a/crates/sophus_image/Cargo.toml b/crates/sophus_image/Cargo.toml index 8473f2a7..bf229531 100644 --- a/crates/sophus_image/Cargo.toml +++ b/crates/sophus_image/Cargo.toml @@ -11,7 +11,8 @@ repository.workspace = true version.workspace = true [dependencies] -sophus_core.workspace = true +sophus_autodiff.workspace = true +sophus_tensor.workspace = true approx.workspace = true bytemuck.workspace = true @@ -23,5 +24,5 @@ tiff = {version = "0.9.0", optional = true} png = {version ="0.17", optional = true} [features] -simd = ["sophus_core/simd"] -std = ["png", "tiff", "sophus_core/std"] +simd = ["sophus_tensor/simd"] +std = ["png", "tiff", "sophus_tensor/std"] diff --git a/crates/sophus_image/src/arc_image.rs b/crates/sophus_image/src/arc_image.rs index d75d054a..d7049e19 100644 --- a/crates/sophus_image/src/arc_image.rs +++ b/crates/sophus_image/src/arc_image.rs @@ -2,9 +2,9 @@ use crate::image_view::GenImageView; use crate::mut_image::GenMutImage; use crate::prelude::*; use crate::ImageSize; -use sophus_core::linalg::SVec; -use sophus_core::tensor::tensor_view::TensorView; -use sophus_core::tensor::ArcTensor; +use sophus_autodiff::linalg::SVec; +use sophus_tensor::tensor_view::TensorView; +use sophus_tensor::ArcTensor; /// Image of static tensors with shared ownership #[derive(Debug, Clone)] diff --git a/crates/sophus_image/src/color_map.rs b/crates/sophus_image/src/color_map.rs index 2bf50811..14f506e6 100644 --- a/crates/sophus_image/src/color_map.rs +++ b/crates/sophus_image/src/color_map.rs @@ -1,4 +1,4 @@ -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; /// blue to white to red to black pub struct BlueWhiteRedBlackColorMap; diff --git a/crates/sophus_image/src/image_view.rs b/crates/sophus_image/src/image_view.rs index aefc3608..324f2156 100644 --- a/crates/sophus_image/src/image_view.rs +++ b/crates/sophus_image/src/image_view.rs @@ -1,8 +1,8 @@ use crate::prelude::*; use crate::ImageSize; use ndarray::ShapeBuilder; -use sophus_core::linalg::SVec; -use sophus_core::tensor::tensor_view::TensorView; +use sophus_autodiff::linalg::SVec; +use sophus_tensor::tensor_view::TensorView; /// Image view of static tensors #[derive(Debug, Clone, PartialEq)] diff --git a/crates/sophus_image/src/intensity_image/intensity_arc_image.rs b/crates/sophus_image/src/intensity_image/intensity_arc_image.rs index e7501704..78596687 100644 --- a/crates/sophus_image/src/intensity_image/intensity_arc_image.rs +++ b/crates/sophus_image/src/intensity_image/intensity_arc_image.rs @@ -7,7 +7,7 @@ use crate::intensity_image::intensity_pixel::IntensityPixel; use crate::intensity_image::intensity_scalar::IsIntensityScalar; use crate::prelude::*; use crate::ArcImage; -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; /// Trait for "intensity" images (grayscale, grayscale+alpha, RGB, RGBA). /// diff --git a/crates/sophus_image/src/intensity_image/intensity_mut_image.rs b/crates/sophus_image/src/intensity_image/intensity_mut_image.rs index 36aa6e50..307bc2af 100644 --- a/crates/sophus_image/src/intensity_image/intensity_mut_image.rs +++ b/crates/sophus_image/src/intensity_image/intensity_mut_image.rs @@ -5,7 +5,7 @@ use crate::mut_image::MutImageR; use crate::prelude::DynIntensityMutImage; use crate::prelude::*; use crate::MutImage; -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; /// Trait for "intensity" images (grayscale, grayscale+alpha, RGB, RGBA). /// diff --git a/crates/sophus_image/src/intensity_image/intensity_pixel.rs b/crates/sophus_image/src/intensity_image/intensity_pixel.rs index 21671545..0013f0df 100644 --- a/crates/sophus_image/src/intensity_image/intensity_pixel.rs +++ b/crates/sophus_image/src/intensity_image/intensity_pixel.rs @@ -1,4 +1,4 @@ -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; use crate::intensity_image::intensity_scalar::IsIntensityScalar; diff --git a/crates/sophus_image/src/intensity_image/intensity_scalar.rs b/crates/sophus_image/src/intensity_image/intensity_scalar.rs index 00d97458..f18e80bf 100644 --- a/crates/sophus_image/src/intensity_image/intensity_scalar.rs +++ b/crates/sophus_image/src/intensity_image/intensity_scalar.rs @@ -1,5 +1,5 @@ use core::any::TypeId; -use sophus_core::prelude::*; +use sophus_autodiff::prelude::*; /// either u8, u16, or f32 pub trait IsIntensityScalar: IsCoreScalar + Sized + Copy { diff --git a/crates/sophus_image/src/interpolation.rs b/crates/sophus_image/src/interpolation.rs index 1a157374..d1aa9f03 100644 --- a/crates/sophus_image/src/interpolation.rs +++ b/crates/sophus_image/src/interpolation.rs @@ -1,5 +1,5 @@ use crate::prelude::*; -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; /// Bilinear interpolated image lookup pub fn interpolate_impl<'a, I: IsImageView<'a, 2, 0, f32, f32, 1, 1>>( diff --git a/crates/sophus_image/src/lib.rs b/crates/sophus_image/src/lib.rs index 368da8d4..6650e41a 100644 --- a/crates/sophus_image/src/lib.rs +++ b/crates/sophus_image/src/lib.rs @@ -88,5 +88,5 @@ 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_core::prelude::*; + pub use sophus_tensor::prelude::*; } diff --git a/crates/sophus_image/src/mut_image.rs b/crates/sophus_image/src/mut_image.rs index c4fefdfd..236b534b 100644 --- a/crates/sophus_image/src/mut_image.rs +++ b/crates/sophus_image/src/mut_image.rs @@ -2,9 +2,9 @@ use crate::arc_image::GenArcImage; use crate::image_view::GenImageView; use crate::prelude::*; use crate::ImageSize; -use sophus_core::linalg::SVec; -use sophus_core::tensor::MutTensor; -use sophus_core::tensor::TensorView; +use sophus_autodiff::linalg::SVec; +use sophus_tensor::MutTensor; +use sophus_tensor::TensorView; extern crate alloc; diff --git a/crates/sophus_image/src/mut_image_view.rs b/crates/sophus_image/src/mut_image_view.rs index f9fa7d5c..6008fb08 100644 --- a/crates/sophus_image/src/mut_image_view.rs +++ b/crates/sophus_image/src/mut_image_view.rs @@ -1,7 +1,7 @@ use crate::image_view::GenImageView; use crate::prelude::*; use crate::ImageSize; -use sophus_core::tensor::mut_tensor_view::MutTensorView; +use sophus_tensor::mut_tensor_view::MutTensorView; /// Mutable image view of a static tensors #[derive(Debug, PartialEq)] diff --git a/crates/sophus_lie/Cargo.toml b/crates/sophus_lie/Cargo.toml index 0f8d31b6..db0a1e4b 100644 --- a/crates/sophus_lie/Cargo.toml +++ b/crates/sophus_lie/Cargo.toml @@ -18,8 +18,8 @@ nalgebra.workspace = true num-traits.workspace = true snafu.workspace = true -sophus_core = {workspace = true} +sophus_autodiff = {workspace = true} [features] -simd = ["sophus_core/simd"] -std = ["sophus_core/std"] +simd = ["sophus_autodiff/simd"] +std = ["sophus_autodiff/std"] diff --git a/crates/sophus_lie/src/factor_lie_group.rs b/crates/sophus_lie/src/factor_lie_group.rs index cf779f95..b7c64049 100644 --- a/crates/sophus_lie/src/factor_lie_group.rs +++ b/crates/sophus_lie/src/factor_lie_group.rs @@ -1,20 +1,17 @@ -use core::borrow::Borrow; - use crate::lie_group::LieGroup; use crate::prelude::*; use crate::traits::IsRealLieFactorGroupImpl; use crate::Rotation2; use crate::Rotation3; use approx::assert_relative_eq; -use sophus_core::calculus::dual::DualScalar; -use sophus_core::calculus::maps::MatrixValuedVectorMap; -use sophus_core::manifold::traits::TangentImpl; - +use core::borrow::Borrow; #[cfg(feature = "simd")] -use sophus_core::calculus::dual::DualBatchScalar; - +use sophus_autodiff::dual::DualBatchScalar; +use sophus_autodiff::dual::DualScalar; #[cfg(feature = "simd")] -use sophus_core::linalg::BatchScalarF64; +use sophus_autodiff::linalg::BatchScalarF64; +use sophus_autodiff::manifold::IsTangent; +use sophus_autodiff::maps::MatrixValuedVectorMap; impl< S: IsRealScalar, @@ -88,7 +85,7 @@ macro_rules! def_real_group_test_template { impl RealFactorLieGroupTest for $group { fn mat_v_test() { use crate::traits::IsLieGroup; - use sophus_core::linalg::scalar::IsScalar; + use sophus_autodiff::linalg::scalar::IsScalar; const POINT: usize = <$group>::POINT; @@ -107,11 +104,11 @@ macro_rules! def_real_group_test_template { fn test_mat_v_jacobian() { use crate::traits::IsLieGroup; use log::info; - use sophus_core::calculus::maps::vector_valued_maps::VectorValuedVectorMap; - use sophus_core::linalg::scalar::IsScalar; - use sophus_core::linalg::vector::IsVector; - use sophus_core::params::HasParams; - use sophus_core::points::example_points; + use sophus_autodiff::maps::vector_valued_maps::VectorValuedVectorMap; + use sophus_autodiff::linalg::scalar::IsScalar; + use sophus_autodiff::linalg::vector::IsVector; + use sophus_autodiff::params::HasParams; + use sophus_autodiff::points::example_points; const DOF: usize = <$group>::DOF; const POINT: usize = <$group>::POINT; diff --git a/crates/sophus_lie/src/groups/isometry2.rs b/crates/sophus_lie/src/groups/isometry2.rs index 8a34c65f..3c2d6d66 100644 --- a/crates/sophus_lie/src/groups/isometry2.rs +++ b/crates/sophus_lie/src/groups/isometry2.rs @@ -1,5 +1,3 @@ -use core::borrow::Borrow; - use crate::groups::rotation2::Rotation2Impl; use crate::groups::translation_product_product::TranslationProductGroupImpl; use crate::lie_group::average::iterative_average; @@ -9,6 +7,7 @@ use crate::prelude::*; use crate::traits::EmptySliceError; use crate::traits::HasAverage; use crate::Rotation2; +use core::borrow::Borrow; use log::warn; @@ -142,12 +141,11 @@ impl + PartialOrd, const DM: usize, const DN: usize> #[test] fn isometry2_prop_tests() { use crate::lie_group::real_lie_group::RealLieGroupTest; - use sophus_core::calculus::dual::dual_scalar::DualScalar; - #[cfg(feature = "simd")] - use sophus_core::calculus::dual::dual_batch_scalar::DualBatchScalar; + use sophus_autodiff::dual::dual_batch_scalar::DualBatchScalar; + use sophus_autodiff::dual::dual_scalar::DualScalar; #[cfg(feature = "simd")] - use sophus_core::linalg::BatchScalarF64; + use sophus_autodiff::linalg::BatchScalarF64; Isometry2F64::test_suite(); #[cfg(feature = "simd")] diff --git a/crates/sophus_lie/src/groups/isometry3.rs b/crates/sophus_lie/src/groups/isometry3.rs index 4203bbc9..1f13556a 100644 --- a/crates/sophus_lie/src/groups/isometry3.rs +++ b/crates/sophus_lie/src/groups/isometry3.rs @@ -164,12 +164,11 @@ impl + PartialOrd, const DM: usize, const DN: usize> #[test] fn isometry3_prop_tests() { use crate::lie_group::real_lie_group::RealLieGroupTest; - use sophus_core::calculus::dual::dual_scalar::DualScalar; - + use sophus_autodiff::dual::dual_scalar::DualScalar; #[cfg(feature = "simd")] - use sophus_core::calculus::dual::DualBatchScalar; + use sophus_autodiff::dual::DualBatchScalar; #[cfg(feature = "simd")] - use sophus_core::linalg::BatchScalarF64; + use sophus_autodiff::linalg::BatchScalarF64; Isometry3F64::test_suite(); #[cfg(feature = "simd")] diff --git a/crates/sophus_lie/src/groups/rotation2.rs b/crates/sophus_lie/src/groups/rotation2.rs index 917ae0ed..b8875d15 100644 --- a/crates/sophus_lie/src/groups/rotation2.rs +++ b/crates/sophus_lie/src/groups/rotation2.rs @@ -9,9 +9,10 @@ use crate::traits::IsRealLieFactorGroupImpl; use crate::traits::IsRealLieGroupImpl; use core::borrow::Borrow; use core::marker::PhantomData; -use sophus_core::linalg::EPS_F64; -use sophus_core::manifold::traits::TangentImpl; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::linalg::EPS_F64; +use sophus_autodiff::manifold::IsTangent; +use sophus_autodiff::params::HasParams; +use sophus_autodiff::params::IsParamsImpl; extern crate alloc; @@ -37,7 +38,7 @@ impl, const BATCH: usize, const DM: usize, const DN: } impl, const BATCH: usize, const DM: usize, const DN: usize> - ParamsImpl for Rotation2Impl + IsParamsImpl for Rotation2Impl { fn params_examples() -> alloc::vec::Vec> { let mut params = alloc::vec![]; @@ -70,7 +71,7 @@ impl, const BATCH: usize, const DM: usize, const DN: } impl, const BATCH: usize, const DM: usize, const DN: usize> - TangentImpl for Rotation2Impl + IsTangent for Rotation2Impl { fn tangent_examples() -> alloc::vec::Vec> { alloc::vec![ @@ -366,13 +367,11 @@ impl + PartialOrd, const DM: usize, const DN: usize> fn rotation2_prop_tests() { use crate::factor_lie_group::RealFactorLieGroupTest; use crate::lie_group::real_lie_group::RealLieGroupTest; - use sophus_core::calculus::dual::dual_scalar::DualScalar; - + use sophus_autodiff::dual::dual_scalar::DualScalar; #[cfg(feature = "simd")] - use sophus_core::calculus::dual::DualBatchScalar; - + use sophus_autodiff::dual::DualBatchScalar; #[cfg(feature = "simd")] - use sophus_core::linalg::BatchScalarF64; + use sophus_autodiff::linalg::BatchScalarF64; Rotation2F64::test_suite(); #[cfg(feature = "simd")] diff --git a/crates/sophus_lie/src/groups/rotation3.rs b/crates/sophus_lie/src/groups/rotation3.rs index c962e105..240c906b 100644 --- a/crates/sophus_lie/src/groups/rotation3.rs +++ b/crates/sophus_lie/src/groups/rotation3.rs @@ -12,11 +12,12 @@ use core::borrow::Borrow; use core::f64; use core::marker::PhantomData; use log::warn; -use sophus_core::linalg::vector::cross; -use sophus_core::linalg::MatF64; -use sophus_core::linalg::EPS_F64; -use sophus_core::manifold::traits::TangentImpl; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::linalg::vector::cross; +use sophus_autodiff::linalg::MatF64; +use sophus_autodiff::linalg::EPS_F64; +use sophus_autodiff::manifold::IsTangent; +use sophus_autodiff::params::HasParams; +use sophus_autodiff::params::IsParamsImpl; extern crate alloc; @@ -43,7 +44,7 @@ impl, const BATCH: usize, const DM: usize, const DN: } impl, const BATCH: usize, const DM: usize, const DN: usize> - ParamsImpl for Rotation3Impl + IsParamsImpl for Rotation3Impl { fn params_examples() -> alloc::vec::Vec> { const NEAR_PI: f64 = f64::consts::PI - 1e-6; @@ -183,7 +184,7 @@ impl, const BATCH: usize, const DM: usize, const DN: } impl, const BATCH: usize, const DM: usize, const DN: usize> - TangentImpl for Rotation3Impl + IsTangent for Rotation3Impl { fn tangent_examples() -> alloc::vec::Vec> { alloc::vec![ @@ -1041,11 +1042,11 @@ impl + PartialOrd, const DM: usize, const DN: usize> fn rotation3_prop_tests() { use crate::factor_lie_group::RealFactorLieGroupTest; use crate::lie_group::real_lie_group::RealLieGroupTest; - use sophus_core::calculus::dual::dual_scalar::DualScalar; + use sophus_autodiff::dual::dual_scalar::DualScalar; #[cfg(feature = "simd")] - use sophus_core::calculus::dual::DualBatchScalar; + use sophus_autodiff::dual::DualBatchScalar; #[cfg(feature = "simd")] - use sophus_core::linalg::BatchScalarF64; + use sophus_autodiff::linalg::BatchScalarF64; Rotation3F64::test_suite(); #[cfg(feature = "simd")] diff --git a/crates/sophus_lie/src/groups/translation_product_product.rs b/crates/sophus_lie/src/groups/translation_product_product.rs index 65829771..7cb136b7 100644 --- a/crates/sophus_lie/src/groups/translation_product_product.rs +++ b/crates/sophus_lie/src/groups/translation_product_product.rs @@ -7,9 +7,9 @@ use crate::traits::IsLieFactorGroupImpl; use crate::traits::IsLieGroupImpl; use crate::traits::IsRealLieFactorGroupImpl; use crate::traits::IsRealLieGroupImpl; -use sophus_core::manifold::traits::TangentImpl; -use sophus_core::params::ParamsImpl; -use sophus_core::points::example_points; +use sophus_autodiff::manifold::IsTangent; +use sophus_autodiff::params::IsParamsImpl; +use sophus_autodiff::points::example_points; extern crate alloc; @@ -123,7 +123,7 @@ impl< const DM: usize, const DN: usize, F: IsLieFactorGroupImpl, - > ParamsImpl + > IsParamsImpl for TranslationProductGroupImpl { fn are_params_valid

(params: P) -> S::Mask @@ -172,7 +172,7 @@ impl< const DM: usize, const DN: usize, F: IsLieFactorGroupImpl, - > TangentImpl + > IsTangent for TranslationProductGroupImpl { fn tangent_examples() -> alloc::vec::Vec> { @@ -604,7 +604,7 @@ impl< fn set_translation

(&mut self, translation: P) where - P: Borrow<>::Vector>, + P: Borrow<>::Vector>, { self.set_params(Self::G::params_from( translation.borrow(), diff --git a/crates/sophus_lie/src/lib.rs b/crates/sophus_lie/src/lib.rs index 35e2a60e..c5fec822 100644 --- a/crates/sophus_lie/src/lib.rs +++ b/crates/sophus_lie/src/lib.rs @@ -29,5 +29,5 @@ pub mod traits; pub mod prelude { pub use crate::traits::IsLieGroup; pub use crate::traits::IsTranslationProductGroup; - pub use sophus_core::prelude::*; + pub use sophus_autodiff::prelude::*; } diff --git a/crates/sophus_lie/src/lie_group.rs b/crates/sophus_lie/src/lie_group.rs index ab508d1d..fa6ae63a 100644 --- a/crates/sophus_lie/src/lie_group.rs +++ b/crates/sophus_lie/src/lie_group.rs @@ -3,8 +3,9 @@ use crate::prelude::*; use approx::assert_relative_eq; use core::borrow::Borrow; use core::fmt::Debug; -use sophus_core::manifold::traits::TangentImpl; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::manifold::IsTangent; +use sophus_autodiff::params::HasParams; +use sophus_autodiff::params::IsParamsImpl; extern crate alloc; @@ -44,7 +45,7 @@ impl< const DM: usize, const DN: usize, G: IsLieGroupImpl, - > ParamsImpl + > IsParamsImpl for LieGroup { fn are_params_valid

(params: P) -> S::Mask @@ -116,7 +117,7 @@ impl< const DM: usize, const DN: usize, G: IsLieGroupImpl, - > TangentImpl + > IsTangent for LieGroup { fn tangent_examples() -> alloc::vec::Vec<>::Vector> { diff --git a/crates/sophus_lie/src/lie_group/average.rs b/crates/sophus_lie/src/lie_group/average.rs index f7ec4f7e..1e791b1d 100644 --- a/crates/sophus_lie/src/lie_group/average.rs +++ b/crates/sophus_lie/src/lie_group/average.rs @@ -5,9 +5,9 @@ use crate::Isometry2F64; use crate::Isometry3F64; use crate::LieGroup; use snafu::prelude::*; -use sophus_core::linalg::EPS_F64; -use sophus_core::prelude::IsSingleScalar; -use sophus_core::prelude::IsVector; +use sophus_autodiff::linalg::EPS_F64; +use sophus_autodiff::prelude::IsSingleScalar; +use sophus_autodiff::prelude::IsVector; /// error of iterative_average #[derive(Snafu, Debug)] @@ -88,7 +88,7 @@ macro_rules! def_average_test_template { fn run_average_tests() { use crate::traits::HasAverage; use approx::assert_relative_eq; - use sophus_core::linalg::VecF64; + use sophus_autodiff::linalg::VecF64; // test: empty slice assert!(Self::average(&[]).is_err()); diff --git a/crates/sophus_lie/src/lie_group/group_mul.rs b/crates/sophus_lie/src/lie_group/group_mul.rs index 2206bde6..29d39e36 100644 --- a/crates/sophus_lie/src/lie_group/group_mul.rs +++ b/crates/sophus_lie/src/lie_group/group_mul.rs @@ -1,7 +1,7 @@ use crate::lie_group::LieGroup; use crate::traits::IsLieGroupImpl; use core::ops::Mul; -use sophus_core::prelude::IsScalar; +use sophus_autodiff::prelude::IsScalar; // a * b impl< 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 93c1cce7..fd6e276b 100644 --- a/crates/sophus_lie/src/lie_group/lie_group_manifold.rs +++ b/crates/sophus_lie/src/lie_group/lie_group_manifold.rs @@ -3,7 +3,9 @@ use crate::prelude::*; use crate::traits::IsLieGroupImpl; use core::borrow::Borrow; use core::fmt::Debug; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::manifold::IsManifold; +use sophus_autodiff::params::HasParams; +use sophus_autodiff::params::IsParamsImpl; extern crate alloc; @@ -36,7 +38,7 @@ impl< const DM: usize, const DN: usize, G: IsLieGroupImpl + Clone + Debug, - > ParamsImpl + > IsParamsImpl for LeftGroupManifold { fn are_params_valid

(params: P) -> S::Mask diff --git a/crates/sophus_lie/src/lie_group/real_lie_group.rs b/crates/sophus_lie/src/lie_group/real_lie_group.rs index 24b838f5..b4221a0c 100644 --- a/crates/sophus_lie/src/lie_group/real_lie_group.rs +++ b/crates/sophus_lie/src/lie_group/real_lie_group.rs @@ -11,22 +11,20 @@ use crate::Rotation2F64; use crate::Rotation3; use crate::Rotation3F64; use approx::assert_relative_eq; +use core::borrow::Borrow; +use core::fmt::Display; +use core::fmt::Formatter; use nalgebra::SVector; -use sophus_core::calculus::dual::DualScalar; -use sophus_core::calculus::maps::MatrixValuedVectorMap; -use sophus_core::calculus::maps::VectorValuedMatrixMap; -use sophus_core::calculus::maps::VectorValuedVectorMap; - -extern crate alloc; - #[cfg(feature = "simd")] -use sophus_core::calculus::dual::dual_batch_scalar::DualBatchScalar; +use sophus_autodiff::dual::dual_batch_scalar::DualBatchScalar; +use sophus_autodiff::dual::DualScalar; #[cfg(feature = "simd")] -use sophus_core::linalg::BatchScalarF64; +use sophus_autodiff::linalg::BatchScalarF64; +use sophus_autodiff::maps::MatrixValuedVectorMap; +use sophus_autodiff::maps::VectorValuedMatrixMap; +use sophus_autodiff::maps::VectorValuedVectorMap; -use core::borrow::Borrow; -use core::fmt::Display; -use core::fmt::Formatter; +extern crate alloc; impl< S: IsRealScalar, @@ -192,7 +190,7 @@ macro_rules! def_real_group_test_template { fn adjoint_jacobian_tests() { use crate::traits::IsLieGroup; const DOF: usize = <$group>::DOF; - use sophus_core::manifold::traits::TangentImpl; + use sophus_autodiff::manifold::IsTangent; let tangent_examples: alloc::vec::Vec<<$scalar as IsScalar<$batch,0,0>>::Vector> = <$group>::tangent_examples(); @@ -262,8 +260,8 @@ macro_rules! def_real_group_test_template { const POINT: usize = <$group>::POINT; const PARAMS: usize = <$group>::PARAMS; - use sophus_core::manifold::traits::TangentImpl; - use sophus_core::points::example_points; + use sophus_autodiff::manifold::IsTangent; + use sophus_autodiff::points::example_points; for t in <$group>::tangent_examples() { // x == log(exp(x)) @@ -451,7 +449,7 @@ macro_rules! def_real_group_test_template { fn hat_jacobians_tests() { use crate::traits::IsLieGroup; - use sophus_core::manifold::traits::TangentImpl; + use sophus_autodiff::manifold::IsTangent; const DOF: usize = <$group>::DOF; const AMBIENT: usize = <$group>::AMBIENT; diff --git a/crates/sophus_lie/src/traits.rs b/crates/sophus_lie/src/traits.rs index 9d3e1baa..604610ce 100644 --- a/crates/sophus_lie/src/traits.rs +++ b/crates/sophus_lie/src/traits.rs @@ -1,8 +1,9 @@ use crate::prelude::*; use core::borrow::Borrow; use core::fmt::Debug; -use sophus_core::manifold::traits::TangentImpl; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::manifold::IsTangent; +use sophus_autodiff::params::HasParams; +use sophus_autodiff::params::IsParamsImpl; /// Disambiguate the parameters. pub trait HasDisambiguate< @@ -51,8 +52,8 @@ pub trait IsLieGroupImpl< const DM: usize, const DN: usize, >: - ParamsImpl - + TangentImpl + IsParamsImpl + + IsTangent + HasDisambiguate + Clone + Debug @@ -265,7 +266,7 @@ pub trait IsLieGroup< const BATCH: usize, const DM: usize, const DN: usize, ->: TangentImpl + HasParams +>: IsTangent + HasParams { /// This is the actual Lie Group implementation type G: IsLieGroupImpl; diff --git a/crates/sophus_opt/Cargo.toml b/crates/sophus_opt/Cargo.toml index 5c053b4a..837bbf41 100644 --- a/crates/sophus_opt/Cargo.toml +++ b/crates/sophus_opt/Cargo.toml @@ -11,7 +11,7 @@ repository.workspace = true version.workspace = true [dependencies] -sophus_core.workspace = true +sophus_autodiff.workspace = true sophus_image.workspace = true sophus_lie.workspace = true sophus_sensor.workspace = true @@ -28,13 +28,13 @@ rayon = "1.10" [features] simd = [ - "sophus_core/simd", + "sophus_autodiff/simd", "sophus_lie/simd", "sophus_sensor/simd", "sophus_image/simd", ] std = [ - "sophus_core/std", + "sophus_autodiff/std", "sophus_lie/std", "sophus_sensor/std", "sophus_image/std", diff --git a/crates/sophus_opt/src/block.rs b/crates/sophus_opt/src/block.rs index 270c9832..f8c70ca8 100644 --- a/crates/sophus_opt/src/block.rs +++ b/crates/sophus_opt/src/block.rs @@ -1,6 +1,6 @@ use nalgebra::Const; -use sophus_core::linalg::MatF64; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::MatF64; +use sophus_autodiff::linalg::VecF64; /// Range of a block #[derive(Clone, Debug, Copy, Default)] diff --git a/crates/sophus_opt/src/example_problems/cam_calib.rs b/crates/sophus_opt/src/example_problems/cam_calib.rs index 976ab1a1..f737c7be 100644 --- a/crates/sophus_opt/src/example_problems/cam_calib.rs +++ b/crates/sophus_opt/src/example_problems/cam_calib.rs @@ -11,8 +11,8 @@ use crate::robust_kernel::HuberKernel; use crate::variables::VarFamily; use crate::variables::VarKind; use crate::variables::VarPoolBuilder; -use sophus_core::linalg::MatF64; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::MatF64; +use sophus_autodiff::linalg::VecF64; use sophus_image::ImageSize; use sophus_lie::Isometry3; use sophus_lie::Isometry3F64; diff --git a/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs b/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs index 485d3403..c31ff97c 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/isometry2_prior.rs @@ -5,10 +5,10 @@ use crate::robust_kernel; use crate::term::MakeTerm; use crate::term::Term; use crate::variables::VarKind; -use sophus_core::calculus::dual::DualScalar; -use sophus_core::calculus::dual::DualVector; -use sophus_core::calculus::maps::VectorValuedVectorMap; -use sophus_core::linalg::VecF64; +use sophus_autodiff::dual::DualScalar; +use sophus_autodiff::dual::DualVector; +use sophus_autodiff::linalg::VecF64; +use sophus_autodiff::maps::VectorValuedVectorMap; use sophus_lie::Isometry2; use sophus_lie::Isometry2F64; diff --git a/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs b/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs index 4df88cfb..7b92945b 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/isometry3_prior.rs @@ -5,11 +5,11 @@ use crate::robust_kernel; use crate::term::MakeTerm; use crate::term::Term; use crate::variables::VarKind; -use sophus_core::calculus::dual::DualScalar; -use sophus_core::calculus::dual::DualVector; -use sophus_core::calculus::maps::VectorValuedVectorMap; -use sophus_core::linalg::MatF64; -use sophus_core::linalg::VecF64; +use sophus_autodiff::dual::DualScalar; +use sophus_autodiff::dual::DualVector; +use sophus_autodiff::linalg::MatF64; +use sophus_autodiff::linalg::VecF64; +use sophus_autodiff::maps::VectorValuedVectorMap; use sophus_lie::Isometry3; use sophus_lie::Isometry3F64; diff --git a/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs b/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs index 83e17ab3..f258b549 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs @@ -5,10 +5,10 @@ use crate::robust_kernel; use crate::term::MakeTerm; use crate::term::Term; use crate::variables::VarKind; -use sophus_core::calculus::dual::DualScalar; -use sophus_core::calculus::dual::DualVector; -use sophus_core::calculus::maps::VectorValuedVectorMap; -use sophus_core::linalg::VecF64; +use sophus_autodiff::dual::DualScalar; +use sophus_autodiff::dual::DualVector; +use sophus_autodiff::linalg::VecF64; +use sophus_autodiff::maps::VectorValuedVectorMap; use sophus_lie::Isometry3; use sophus_lie::Isometry3F64; use sophus_sensor::camera_enum::perspective_camera::PinholeCameraF64; diff --git a/crates/sophus_opt/src/example_problems/pose_circle.rs b/crates/sophus_opt/src/example_problems/pose_circle.rs index 0fa95aac..ac9a33df 100644 --- a/crates/sophus_opt/src/example_problems/pose_circle.rs +++ b/crates/sophus_opt/src/example_problems/pose_circle.rs @@ -9,7 +9,7 @@ use crate::variables::VarFamily; use crate::variables::VarKind; use crate::variables::VarPool; use crate::variables::VarPoolBuilder; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_lie::Isometry2; use sophus_lie::Isometry2F64; diff --git a/crates/sophus_opt/src/example_problems/simple_prior.rs b/crates/sophus_opt/src/example_problems/simple_prior.rs index e45e37f2..4dce4f7b 100644 --- a/crates/sophus_opt/src/example_problems/simple_prior.rs +++ b/crates/sophus_opt/src/example_problems/simple_prior.rs @@ -10,8 +10,8 @@ use crate::prelude::*; use crate::variables::VarFamily; use crate::variables::VarKind; use crate::variables::VarPoolBuilder; -use sophus_core::linalg::MatF64; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::MatF64; +use sophus_autodiff::linalg::VecF64; use sophus_lie::Isometry2F64; use sophus_lie::Isometry3F64; @@ -43,7 +43,7 @@ impl SimpleIso2PriorProblem { /// Test the simple 2D isometry prior problem pub fn test(&self) { - use sophus_core::linalg::EPS_F64; + use sophus_autodiff::linalg::EPS_F64; let cost_signature = alloc::vec![Isometry2PriorTermSignature { isometry_prior_mean: self.true_world_from_robot, entity_indices: [0], @@ -115,7 +115,7 @@ impl SimpleIso3PriorProblem { /// Test the simple 3D isometry prior problem pub fn test(&self) { - use sophus_core::linalg::EPS_F64; + use sophus_autodiff::linalg::EPS_F64; let cost_signature = alloc::vec![Isometry3PriorTermSignature { isometry_prior: (self.true_world_from_robot, MatF64::<6, 6>::identity()), diff --git a/crates/sophus_opt/src/term.rs b/crates/sophus_opt/src/term.rs index 633afb11..2972bd97 100644 --- a/crates/sophus_opt/src/term.rs +++ b/crates/sophus_opt/src/term.rs @@ -3,8 +3,8 @@ use crate::block::BlockVector; use crate::prelude::*; use crate::robust_kernel; use crate::variables::VarKind; -use sophus_core::linalg::MatF64; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::MatF64; +use sophus_autodiff::linalg::VecF64; extern crate alloc; diff --git a/crates/sophus_opt/src/variables.rs b/crates/sophus_opt/src/variables.rs index 879ba165..7d3952c8 100644 --- a/crates/sophus_opt/src/variables.rs +++ b/crates/sophus_opt/src/variables.rs @@ -1,7 +1,7 @@ use crate::prelude::*; use core::fmt::Debug; use dyn_clone::DynClone; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_lie::Isometry2F64; use sophus_lie::Isometry3F64; use sophus_sensor::camera_enum::perspective_camera::BrownConradyCameraF64; diff --git a/crates/sophus_renderer/Cargo.toml b/crates/sophus_renderer/Cargo.toml index c871b56e..5f71d91e 100644 --- a/crates/sophus_renderer/Cargo.toml +++ b/crates/sophus_renderer/Cargo.toml @@ -18,16 +18,17 @@ egui_extras.workspace = true env_logger.workspace = true linked-hash-map.workspace = true num-traits.workspace = true -sophus_core.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] std = [ - "sophus_core/std", + "sophus_autodiff/std", "sophus_image/std", "sophus_lie/std", "sophus_opt/std", diff --git a/crates/sophus_renderer/src/camera.rs b/crates/sophus_renderer/src/camera.rs index 376073be..2386b232 100644 --- a/crates/sophus_renderer/src/camera.rs +++ b/crates/sophus_renderer/src/camera.rs @@ -1,4 +1,4 @@ -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_image::ImageSize; use sophus_lie::Isometry3; use sophus_lie::Isometry3F64; diff --git a/crates/sophus_renderer/src/camera/clipping_planes.rs b/crates/sophus_renderer/src/camera/clipping_planes.rs index f2c5182b..b626d2ab 100644 --- a/crates/sophus_renderer/src/camera/clipping_planes.rs +++ b/crates/sophus_renderer/src/camera/clipping_planes.rs @@ -1,5 +1,5 @@ use num_traits::cast; -use sophus_core::floating_point::FloatingPointNumber; +use sophus_autodiff::floating_point::FloatingPointNumber; /// Clipping planes for the Wgpu renderer #[derive(Clone, Copy, Debug)] diff --git a/crates/sophus_renderer/src/camera/intrinsics.rs b/crates/sophus_renderer/src/camera/intrinsics.rs index 0e8a3657..f05e35d6 100644 --- a/crates/sophus_renderer/src/camera/intrinsics.rs +++ b/crates/sophus_renderer/src/camera/intrinsics.rs @@ -1,4 +1,4 @@ -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_image::ImageSize; use sophus_lie::prelude::IsVector; use sophus_sensor::camera_enum::perspective_camera::PinholeCameraF64; diff --git a/crates/sophus_renderer/src/lib.rs b/crates/sophus_renderer/src/lib.rs index 8b507ba2..7721bf83 100644 --- a/crates/sophus_renderer/src/lib.rs +++ b/crates/sophus_renderer/src/lib.rs @@ -29,8 +29,8 @@ pub mod uniform_buffers; pub use crate::render_context::RenderContext; -/// preludes -pub mod preludes { +/// prelude +pub mod prelude { pub(crate) use alloc::collections::btree_map::BTreeMap; pub(crate) use alloc::format; pub(crate) use alloc::string::String; @@ -38,6 +38,7 @@ pub mod preludes { pub(crate) use alloc::sync::Arc; pub(crate) use alloc::vec; pub(crate) use alloc::vec::Vec; + pub use sophus_image::prelude::*; extern crate alloc; } diff --git a/crates/sophus_renderer/src/offscreen_renderer.rs b/crates/sophus_renderer/src/offscreen_renderer.rs index 684e129e..761f2e3c 100644 --- a/crates/sophus_renderer/src/offscreen_renderer.rs +++ b/crates/sophus_renderer/src/offscreen_renderer.rs @@ -3,7 +3,7 @@ use crate::camera::properties::RenderCameraProperties; use crate::pixel_renderer::pixel_line::Line2dEntity; use crate::pixel_renderer::pixel_point::Point2dEntity; use crate::pixel_renderer::PixelRenderer; -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::pixel_renderable::PixelRenderable; use crate::renderables::scene_renderable::SceneRenderable; use crate::scene_renderer::distortion::DistortionRenderer; @@ -17,7 +17,6 @@ use crate::types::SceneFocusMarker; use crate::types::TranslationAndScaling; use crate::uniform_buffers::VertexShaderUniformBuffers; use crate::RenderContext; -use sophus_core::IsTensorLike; use sophus_image::arc_image::ArcImage4U8; use sophus_image::image_view::IsImageView; use sophus_image::ImageSize; diff --git a/crates/sophus_renderer/src/pipeline_builder.rs b/crates/sophus_renderer/src/pipeline_builder.rs index 4d796bea..9a8b7fc7 100644 --- a/crates/sophus_renderer/src/pipeline_builder.rs +++ b/crates/sophus_renderer/src/pipeline_builder.rs @@ -1,4 +1,4 @@ -use crate::preludes::*; +use crate::prelude::*; use crate::types::DOG_MULTISAMPLE_COUNT; use crate::uniform_buffers::VertexShaderUniformBuffers; use crate::RenderContext; diff --git a/crates/sophus_renderer/src/pixel_renderer.rs b/crates/sophus_renderer/src/pixel_renderer.rs index 49e11fae..d44d04f4 100644 --- a/crates/sophus_renderer/src/pixel_renderer.rs +++ b/crates/sophus_renderer/src/pixel_renderer.rs @@ -8,7 +8,7 @@ use crate::pipeline_builder::PointVertex2; use crate::pipeline_builder::TargetTexture; use crate::pixel_renderer::pixel_line::PixelLineRenderer; use crate::pixel_renderer::pixel_point::PixelPointRenderer; -use crate::preludes::*; +use crate::prelude::*; use crate::types::SceneFocusMarker; use crate::uniform_buffers::VertexShaderUniformBuffers; use crate::RenderContext; diff --git a/crates/sophus_renderer/src/pixel_renderer/pixel_line.rs b/crates/sophus_renderer/src/pixel_renderer/pixel_line.rs index 952d37b7..70951860 100644 --- a/crates/sophus_renderer/src/pixel_renderer/pixel_line.rs +++ b/crates/sophus_renderer/src/pixel_renderer/pixel_line.rs @@ -1,6 +1,6 @@ use crate::pipeline_builder::LineVertex2; use crate::pipeline_builder::PipelineBuilder; -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::pixel_renderable::LineSegments2; use crate::RenderContext; use wgpu::util::DeviceExt; diff --git a/crates/sophus_renderer/src/pixel_renderer/pixel_point.rs b/crates/sophus_renderer/src/pixel_renderer/pixel_point.rs index dddbb1bb..69245399 100644 --- a/crates/sophus_renderer/src/pixel_renderer/pixel_point.rs +++ b/crates/sophus_renderer/src/pixel_renderer/pixel_point.rs @@ -1,6 +1,6 @@ use crate::pipeline_builder::PipelineBuilder; use crate::pipeline_builder::PointVertex2; -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::pixel_renderable::PointCloud2; use crate::RenderContext; use eframe::egui::mutex::Mutex; diff --git a/crates/sophus_renderer/src/render_context.rs b/crates/sophus_renderer/src/render_context.rs index 97cfeb85..4ce36dbb 100644 --- a/crates/sophus_renderer/src/render_context.rs +++ b/crates/sophus_renderer/src/render_context.rs @@ -1,4 +1,4 @@ -use crate::preludes::*; +use crate::prelude::*; use crate::types::DOG_MULTISAMPLE_COUNT; use eframe::egui_wgpu; use eframe::epaint::mutex::RwLock; diff --git a/crates/sophus_renderer/src/renderables/pixel_renderable.rs b/crates/sophus_renderer/src/renderables/pixel_renderable.rs index 11f1f907..6c7fdcfa 100644 --- a/crates/sophus_renderer/src/renderables/pixel_renderable.rs +++ b/crates/sophus_renderer/src/renderables/pixel_renderable.rs @@ -1,6 +1,6 @@ -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::color::Color; -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; /// Pixel renderable #[derive(Clone, Debug)] diff --git a/crates/sophus_renderer/src/renderables/scene_renderable.rs b/crates/sophus_renderer/src/renderables/scene_renderable.rs index b6992c0b..6ca85981 100644 --- a/crates/sophus_renderer/src/renderables/scene_renderable.rs +++ b/crates/sophus_renderer/src/renderables/scene_renderable.rs @@ -1,7 +1,7 @@ -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::color::Color; use crate::renderables::pixel_renderable::HasToVec2F32; -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; use sophus_lie::Isometry3; use sophus_lie::Isometry3F64; diff --git a/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs b/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs index dcef2475..f1820a9f 100644 --- a/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs +++ b/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs @@ -1,7 +1,7 @@ -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::color::Color; use crate::renderables::scene_renderable::LineSegment3; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_lie::Isometry3F64; /// opaque axes builder type. diff --git a/crates/sophus_renderer/src/scene_renderer.rs b/crates/sophus_renderer/src/scene_renderer.rs index 814af5c4..2fd04c29 100644 --- a/crates/sophus_renderer/src/scene_renderer.rs +++ b/crates/sophus_renderer/src/scene_renderer.rs @@ -11,7 +11,7 @@ pub mod textured_mesh; use crate::pipeline_builder::PipelineBuilder; use crate::pipeline_builder::TargetTexture; -use crate::preludes::*; +use crate::prelude::*; use crate::scene_renderer::mesh::MeshRenderer; use crate::scene_renderer::point::ScenePointRenderer; use crate::textures::depth::DepthTextures; diff --git a/crates/sophus_renderer/src/scene_renderer/distortion.rs b/crates/sophus_renderer/src/scene_renderer/distortion.rs index 5d81d8d2..2333baee 100644 --- a/crates/sophus_renderer/src/scene_renderer/distortion.rs +++ b/crates/sophus_renderer/src/scene_renderer/distortion.rs @@ -1,4 +1,4 @@ -use crate::preludes::*; +use crate::prelude::*; use crate::textures::depth::DepthTextures; use crate::textures::rgba::RgbdTexture; use crate::uniform_buffers::VertexShaderUniformBuffers; diff --git a/crates/sophus_renderer/src/scene_renderer/line.rs b/crates/sophus_renderer/src/scene_renderer/line.rs index 13965297..a75efe9b 100644 --- a/crates/sophus_renderer/src/scene_renderer/line.rs +++ b/crates/sophus_renderer/src/scene_renderer/line.rs @@ -1,6 +1,6 @@ use crate::pipeline_builder::LineVertex3; use crate::pipeline_builder::PipelineBuilder; -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::scene_renderable::LineSegments3; use crate::uniform_buffers::VertexShaderUniformBuffers; use crate::RenderContext; diff --git a/crates/sophus_renderer/src/scene_renderer/mesh.rs b/crates/sophus_renderer/src/scene_renderer/mesh.rs index 02e4178e..818b7cbc 100644 --- a/crates/sophus_renderer/src/scene_renderer/mesh.rs +++ b/crates/sophus_renderer/src/scene_renderer/mesh.rs @@ -1,6 +1,6 @@ use crate::pipeline_builder::MeshVertex3; use crate::pipeline_builder::PipelineBuilder; -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::scene_renderable::TriangleMesh3; use crate::uniform_buffers::VertexShaderUniformBuffers; use crate::RenderContext; diff --git a/crates/sophus_renderer/src/scene_renderer/point.rs b/crates/sophus_renderer/src/scene_renderer/point.rs index b25f7818..f060a5e1 100644 --- a/crates/sophus_renderer/src/scene_renderer/point.rs +++ b/crates/sophus_renderer/src/scene_renderer/point.rs @@ -1,6 +1,6 @@ use crate::pipeline_builder::PipelineBuilder; use crate::pipeline_builder::PointVertex3; -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::scene_renderable::PointCloud3; use crate::uniform_buffers::VertexShaderUniformBuffers; use crate::RenderContext; diff --git a/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs b/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs index 4fc8fc82..6c7a1d54 100644 --- a/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs +++ b/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs @@ -1,10 +1,9 @@ use crate::pipeline_builder::PipelineBuilder; use crate::pipeline_builder::TexturedMeshVertex3; -use crate::preludes::*; +use crate::prelude::*; use crate::renderables::scene_renderable::TexturedTriangleMesh3; use crate::uniform_buffers::VertexShaderUniformBuffers; use crate::RenderContext; -use sophus_core::IsTensorLike; use sophus_image::arc_image::ArcImage4U8; use sophus_image::image_view::IsImageView; use sophus_lie::Isometry3F64; diff --git a/crates/sophus_renderer/src/textures/depth.rs b/crates/sophus_renderer/src/textures/depth.rs index 8e597cc7..ef0dbb33 100644 --- a/crates/sophus_renderer/src/textures/depth.rs +++ b/crates/sophus_renderer/src/textures/depth.rs @@ -1,9 +1,9 @@ use crate::camera::clipping_planes::ClippingPlanesF64; +use crate::prelude::*; use crate::textures::depth_image::DepthImage; use crate::textures::ndc_z_buffer::NdcZBuffer; use crate::textures::visual_depth::VisualDepthTexture; use crate::RenderContext; -use sophus_core::IsTensorLike; use sophus_image::arc_image::ArcImageF32; use sophus_image::image_view::ImageViewF32; use sophus_image::prelude::IsImageView; diff --git a/crates/sophus_renderer/src/textures/depth_image.rs b/crates/sophus_renderer/src/textures/depth_image.rs index 02e0ef74..a9609954 100644 --- a/crates/sophus_renderer/src/textures/depth_image.rs +++ b/crates/sophus_renderer/src/textures/depth_image.rs @@ -1,6 +1,6 @@ use crate::camera::clipping_planes::ClippingPlanesF32; use eframe::egui::mutex::Mutex; -use sophus_core::linalg::SVec; +use sophus_autodiff::linalg::SVec; use sophus_image::arc_image::ArcImage4U8; use sophus_image::arc_image::ArcImageF32; use sophus_image::color_map::BlueWhiteRedBlackColorMap; diff --git a/crates/sophus_renderer/src/types.rs b/crates/sophus_renderer/src/types.rs index c9fff0eb..cd84c12e 100644 --- a/crates/sophus_renderer/src/types.rs +++ b/crates/sophus_renderer/src/types.rs @@ -3,7 +3,7 @@ use crate::offscreen_renderer::OffscreenRenderer; use crate::renderables::color::Color; use crate::textures::depth_image::DepthImage; use eframe::egui; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_image::arc_image::ArcImage4U8; /// Render result diff --git a/crates/sophus_renderer/src/uniform_buffers.rs b/crates/sophus_renderer/src/uniform_buffers.rs index 2bf4b963..eb573aee 100644 --- a/crates/sophus_renderer/src/uniform_buffers.rs +++ b/crates/sophus_renderer/src/uniform_buffers.rs @@ -1,5 +1,5 @@ use crate::camera::properties::RenderCameraProperties; -use crate::preludes::*; +use crate::prelude::*; use crate::types::TranslationAndScaling; use crate::types::Zoom2dPod; use crate::RenderContext; diff --git a/crates/sophus_sensor/Cargo.toml b/crates/sophus_sensor/Cargo.toml index d0653c2e..3189ef6b 100644 --- a/crates/sophus_sensor/Cargo.toml +++ b/crates/sophus_sensor/Cargo.toml @@ -11,7 +11,8 @@ repository.workspace = true version.workspace = true [dependencies] -sophus_core.workspace = true +sophus_autodiff.workspace = true +sophus_geo.workspace = true sophus_image.workspace = true approx.workspace = true @@ -21,8 +22,8 @@ ndarray.workspace = true num-traits.workspace = true [features] -simd = ["sophus_core/simd", "sophus_image/simd"] +simd = ["sophus_autodiff/simd", "sophus_image/simd"] std = [ - "sophus_core/std", + "sophus_autodiff/std", "sophus_image/std", ] diff --git a/crates/sophus_sensor/src/distortion_table.rs b/crates/sophus_sensor/src/distortion_table.rs index 7abe86a6..21320cda 100644 --- a/crates/sophus_sensor/src/distortion_table.rs +++ b/crates/sophus_sensor/src/distortion_table.rs @@ -1,8 +1,8 @@ use crate::dyn_camera::DynCameraF64; use crate::prelude::*; use nalgebra::SVector; -use sophus_core::calculus::Region; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; +use sophus_geo::region::Region; use sophus_image::arc_image::ArcImage2F32; use sophus_image::image_view::IsImageView; use sophus_image::interpolation::interpolate; @@ -105,9 +105,9 @@ fn camera_distortion_table_tests() { use crate::distortion_table::DistortTable; use approx::assert_abs_diff_eq; use approx::assert_relative_eq; - use sophus_core::calculus::maps::vector_valued_maps::VectorValuedVectorMap; - use sophus_core::linalg::VecF64; - use sophus_core::linalg::EPS_F64; + use sophus_autodiff::linalg::VecF64; + use sophus_autodiff::linalg::EPS_F64; + use sophus_autodiff::maps::vector_valued_maps::VectorValuedVectorMap; use sophus_image::ImageSize; { diff --git a/crates/sophus_sensor/src/distortions/affine.rs b/crates/sophus_sensor/src/distortions/affine.rs index 90454585..e8c7f746 100644 --- a/crates/sophus_sensor/src/distortions/affine.rs +++ b/crates/sophus_sensor/src/distortions/affine.rs @@ -2,7 +2,7 @@ use crate::prelude::*; use crate::traits::IsCameraDistortionImpl; use core::borrow::Borrow; use core::marker::PhantomData; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::params::IsParamsImpl; extern crate alloc; @@ -20,7 +20,7 @@ pub struct AffineDistortionImpl< } impl, const BATCH: usize, const DM: usize, const DN: usize> - ParamsImpl for AffineDistortionImpl + IsParamsImpl for AffineDistortionImpl { fn are_params_valid

(_params: P) -> S::Mask where diff --git a/crates/sophus_sensor/src/distortions/brown_conrady.rs b/crates/sophus_sensor/src/distortions/brown_conrady.rs index aef246bc..5ca27076 100644 --- a/crates/sophus_sensor/src/distortions/brown_conrady.rs +++ b/crates/sophus_sensor/src/distortions/brown_conrady.rs @@ -4,8 +4,8 @@ use crate::traits::IsCameraDistortionImpl; use core::borrow::Borrow; use core::marker::PhantomData; use log::warn; -use sophus_core::linalg::EPS_F64; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::linalg::EPS_F64; +use sophus_autodiff::params::IsParamsImpl; extern crate alloc; @@ -21,7 +21,7 @@ pub struct BrownConradyDistortionImpl< } impl, const BATCH: usize, const DM: usize, const DN: usize> - ParamsImpl for BrownConradyDistortionImpl + IsParamsImpl for BrownConradyDistortionImpl { fn are_params_valid

(_params: P) -> S::Mask where diff --git a/crates/sophus_sensor/src/distortions/kannala_brandt.rs b/crates/sophus_sensor/src/distortions/kannala_brandt.rs index 36a88cac..c29a6f47 100644 --- a/crates/sophus_sensor/src/distortions/kannala_brandt.rs +++ b/crates/sophus_sensor/src/distortions/kannala_brandt.rs @@ -2,8 +2,8 @@ use crate::prelude::*; use crate::traits::IsCameraDistortionImpl; use core::borrow::Borrow; use core::marker::PhantomData; -use sophus_core::linalg::EPS_F64; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::linalg::EPS_F64; +use sophus_autodiff::params::IsParamsImpl; extern crate alloc; @@ -19,7 +19,7 @@ pub struct KannalaBrandtDistortionImpl< } impl, const BATCH: usize, const DM: usize, const DN: usize> - ParamsImpl for KannalaBrandtDistortionImpl + IsParamsImpl for KannalaBrandtDistortionImpl { fn are_params_valid

(_params: P) -> S::Mask where diff --git a/crates/sophus_sensor/src/distortions/unified.rs b/crates/sophus_sensor/src/distortions/unified.rs index 6c5a4c06..3be197b7 100644 --- a/crates/sophus_sensor/src/distortions/unified.rs +++ b/crates/sophus_sensor/src/distortions/unified.rs @@ -2,10 +2,10 @@ use crate::prelude::*; use crate::traits::IsCameraDistortionImpl; use core::borrow::Borrow; use core::marker::PhantomData; -use sophus_core::params::ParamsImpl; -use sophus_core::prelude::IsMatrix; -use sophus_core::prelude::IsScalar; -use sophus_core::prelude::IsVector; +use sophus_autodiff::params::IsParamsImpl; +use sophus_autodiff::prelude::IsMatrix; +use sophus_autodiff::prelude::IsScalar; +use sophus_autodiff::prelude::IsVector; extern crate alloc; @@ -21,7 +21,7 @@ pub struct UnifiedDistortionImpl< } impl, const BATCH: usize, const DM: usize, const DN: usize> - ParamsImpl for UnifiedDistortionImpl + IsParamsImpl for UnifiedDistortionImpl { fn are_params_valid

(_params: P) -> S::Mask where diff --git a/crates/sophus_sensor/src/dyn_camera.rs b/crates/sophus_sensor/src/dyn_camera.rs index 55f504d9..269f35cc 100644 --- a/crates/sophus_sensor/src/dyn_camera.rs +++ b/crates/sophus_sensor/src/dyn_camera.rs @@ -191,14 +191,13 @@ impl, const BATCH: usize, const DM: usize, const DN: #[test] fn dyn_camera_tests() { use crate::distortions::affine::AffineDistortionImpl; - use crate::distortions::kannala_brandt::KannalaBrandtDistortionImpl; use crate::distortions::unified::UnifiedDistortionImpl; use crate::traits::IsCameraDistortionImpl; use approx::assert_relative_eq; - use sophus_core::calculus::maps::vector_valued_maps::VectorValuedVectorMap; - use sophus_core::linalg::VecF64; - use sophus_core::linalg::EPS_F64; + use sophus_autodiff::linalg::VecF64; + use sophus_autodiff::linalg::EPS_F64; + use sophus_autodiff::maps::vector_valued_maps::VectorValuedVectorMap; use sophus_image::ImageSize; { diff --git a/crates/sophus_sensor/src/lib.rs b/crates/sophus_sensor/src/lib.rs index c0b1ccbd..91a7f6ec 100644 --- a/crates/sophus_sensor/src/lib.rs +++ b/crates/sophus_sensor/src/lib.rs @@ -35,5 +35,6 @@ pub mod prelude { pub use crate::traits::IsCamera; pub use crate::traits::IsPerspectiveCamera; pub use crate::traits::IsProjection; - pub use sophus_core::prelude::*; + pub use sophus_autodiff::prelude::*; + pub use sophus_geo::prelude::*; } diff --git a/crates/sophus_sensor/src/projections/orthographic.rs b/crates/sophus_sensor/src/projections/orthographic.rs index f321347e..fe2060aa 100644 --- a/crates/sophus_sensor/src/projections/orthographic.rs +++ b/crates/sophus_sensor/src/projections/orthographic.rs @@ -3,8 +3,8 @@ use crate::distortions::affine::AffineDistortionImpl; use crate::traits::IsProjection; use core::borrow::Borrow; use core::marker::PhantomData; -use sophus_core::linalg::scalar::IsScalar; -use sophus_core::linalg::vector::IsVector; +use sophus_autodiff::linalg::scalar::IsScalar; +use sophus_autodiff::linalg::vector::IsVector; /// Orthographic projection implementation #[derive(Debug, Clone)] diff --git a/crates/sophus_sensor/src/projections/perspective.rs b/crates/sophus_sensor/src/projections/perspective.rs index 1ed5d091..202bdb4b 100644 --- a/crates/sophus_sensor/src/projections/perspective.rs +++ b/crates/sophus_sensor/src/projections/perspective.rs @@ -1,10 +1,10 @@ use core::borrow::Borrow; use crate::traits::IsProjection; -use sophus_core::linalg::matrix::IsMatrix; -use sophus_core::linalg::scalar::IsScalar; -use sophus_core::linalg::vector::IsVector; -use sophus_core::prelude::IsSingleScalar; +use sophus_autodiff::linalg::matrix::IsMatrix; +use sophus_autodiff::linalg::scalar::IsScalar; +use sophus_autodiff::linalg::vector::IsVector; +use sophus_autodiff::prelude::IsSingleScalar; /// Perspective camera projection - using z=1 plane /// diff --git a/crates/sophus_sensor/src/traits.rs b/crates/sophus_sensor/src/traits.rs index 6e1b6560..5b8ca0e4 100644 --- a/crates/sophus_sensor/src/traits.rs +++ b/crates/sophus_sensor/src/traits.rs @@ -5,7 +5,7 @@ use crate::prelude::*; use crate::BrownConradyCamera; use crate::KannalaBrandtCamera; use crate::PinholeCamera; -use sophus_core::params::ParamsImpl; +use sophus_autodiff::params::IsParamsImpl; use sophus_image::ImageSize; /// Camera distortion implementation trait @@ -16,7 +16,7 @@ pub trait IsCameraDistortionImpl< const BATCH: usize, const DM: usize, const DN: usize, ->: ParamsImpl +>: IsParamsImpl { /// identity parameters fn identity_params() -> S::Vector { diff --git a/crates/sophus_sim/Cargo.toml b/crates/sophus_sim/Cargo.toml index 1648cc05..4c2b076c 100644 --- a/crates/sophus_sim/Cargo.toml +++ b/crates/sophus_sim/Cargo.toml @@ -20,7 +20,7 @@ egui_extras.workspace = true env_logger.workspace = true linked-hash-map.workspace = true num-traits.workspace = true -sophus_core.workspace = true +sophus_autodiff.workspace = true sophus_image.workspace = true sophus_lie.workspace = true sophus_opt.workspace = true @@ -29,7 +29,7 @@ wgpu.workspace = true [features] std = [ - "sophus_core/std", + "sophus_autodiff/std", "sophus_image/std", "sophus_lie/std", "sophus_opt/std", diff --git a/crates/sophus_spline/Cargo.toml b/crates/sophus_spline/Cargo.toml new file mode 100644 index 00000000..6725e2fa --- /dev/null +++ b/crates/sophus_spline/Cargo.toml @@ -0,0 +1,20 @@ +[package] +description = "sophus - geometry for robotics and computer vision" +name = "sophus_spline" +readme = "../../README.md" + +edition.workspace = true +include.workspace = true +keywords.workspace = true +license.workspace = true +repository.workspace = true +version.workspace = true + +[dependencies] +approx.workspace = true +log.workspace = true +num-traits.workspace = true +sophus_autodiff.workspace = true + +[features] +simd = ["sophus_autodiff/simd"] diff --git a/crates/sophus_core/src/calculus/spline.rs b/crates/sophus_spline/src/lib.rs similarity index 81% rename from crates/sophus_core/src/calculus/spline.rs rename to crates/sophus_spline/src/lib.rs index a45844b0..28106a27 100644 --- a/crates/sophus_core/src/calculus/spline.rs +++ b/crates/sophus_spline/src/lib.rs @@ -1,10 +1,10 @@ /// Cubic B-Spline details pub mod spline_segment; -use crate::calculus::spline::spline_segment::CubicBSplineSegment; -use crate::calculus::spline::spline_segment::SegmentCase; -use crate::prelude::*; +use crate::spline_segment::CubicBSplineSegment; +use crate::spline_segment::SegmentCase; use log::debug; +use sophus_autodiff::prelude::*; extern crate alloc; @@ -238,51 +238,46 @@ impl + 'static, const DIMS: usize, const DM: usize, co } } -mod test { +#[test] +fn test_pline() { + use log::info; + use sophus_autodiff::example_points; - #[test] - fn test() { - use super::super::spline::CubicBSpline; - use super::super::spline::CubicBSplineParams; - use crate::points::example_points; - use log::info; + let points = example_points::(); + for (t0, delta_t) in [(0.0, 1.0)] { + let params = CubicBSplineParams { delta_t, t0 }; - let points = example_points::(); - for (t0, delta_t) in [(0.0, 1.0)] { - let params = CubicBSplineParams { delta_t, t0 }; + let mut points = points.clone(); + let spline = CubicBSpline::new(points.clone(), params); + points.reverse(); + let rspline = CubicBSpline::new(points, params); - let mut points = points.clone(); - let spline = CubicBSpline::new(points.clone(), params); - points.reverse(); - let rspline = CubicBSpline::new(points, params); + info!("tmax: {}", spline.t_max()); - info!("tmax: {}", spline.t_max()); + let mut t = t0; - let mut t = t0; - - loop { - if t >= spline.t_max() { - break; - } + loop { + if t >= spline.t_max() { + break; + } - info!("t {}", t); - info!("{:?}", spline.idx_involved(t)); + info!("t {}", t); + info!("{:?}", spline.idx_involved(t)); - info!("i: {}", spline.interpolate(t)); - info!("i': {}", rspline.interpolate(spline.t_max() - t)); + info!("i: {}", spline.interpolate(t)); + info!("i': {}", rspline.interpolate(spline.t_max() - t)); - for i in 0..spline.num_segments() { - info!("dx: {} {}", i, spline.dxi_interpolate(t, i)); - info!( - "dx': {} {}", - i, - rspline.dxi_interpolate(spline.t_max() - t, spline.num_segments() - i) - ); - } - t += 0.1; + for i in 0..spline.num_segments() { + info!("dx: {} {}", i, spline.dxi_interpolate(t, i)); + info!( + "dx': {} {}", + i, + rspline.dxi_interpolate(spline.t_max() - t, spline.num_segments() - i) + ); } - - info!("{:?}", spline.idx_involved(1.01)); + t += 0.1; } + + info!("{:?}", spline.idx_involved(1.01)); } } diff --git a/crates/sophus_core/src/calculus/spline/spline_segment.rs b/crates/sophus_spline/src/spline_segment.rs similarity index 92% rename from crates/sophus_core/src/calculus/spline/spline_segment.rs rename to crates/sophus_spline/src/spline_segment.rs index 960aa3f2..56b8fec7 100644 --- a/crates/sophus_core/src/calculus/spline/spline_segment.rs +++ b/crates/sophus_spline/src/spline_segment.rs @@ -1,5 +1,5 @@ -use crate::prelude::*; use core::marker::PhantomData; +use sophus_autodiff::prelude::*; /// cubic basis function pub struct CubicBasisFunction { @@ -187,15 +187,15 @@ mod test { #[test] fn test_spline_basis_fn() { - use crate::calculus::dual::dual_scalar::DualScalar; - use crate::calculus::dual::dual_vector::DualVector; - use crate::calculus::maps::vector_valued_maps::VectorValuedVectorMap; - use crate::calculus::spline::spline_segment::CubicBSplineFn; - use crate::linalg::scalar::IsScalar; - use crate::linalg::vector::IsVector; - use crate::linalg::VecF64; - use crate::points::example_points; + use crate::spline_segment::CubicBSplineFn; use num_traits::Zero; + use sophus_autodiff::dual::dual_scalar::DualScalar; + use sophus_autodiff::dual::dual_vector::DualVector; + use sophus_autodiff::linalg::scalar::IsScalar; + use sophus_autodiff::linalg::vector::IsVector; + use sophus_autodiff::linalg::VecF64; + use sophus_autodiff::maps::vector_valued_maps::VectorValuedVectorMap; + use sophus_autodiff::points::example_points; let points = &example_points::(); assert!(points.len() >= 8); @@ -261,16 +261,16 @@ mod test { #[test] fn test_spline_segment() { - use crate::calculus::dual::dual_scalar::DualScalar; - use crate::calculus::dual::dual_vector::DualVector; - use crate::calculus::maps::vector_valued_maps::VectorValuedVectorMap; - use crate::calculus::spline::spline_segment::CubicBSplineSegment; - use crate::calculus::spline::spline_segment::SegmentCase; - use crate::linalg::scalar::IsScalar; - use crate::linalg::vector::IsVector; - use crate::linalg::VecF64; - use crate::points::example_points; + use crate::CubicBSplineSegment; + use crate::SegmentCase; use num_traits::Zero; + use sophus_autodiff::dual::dual_scalar::DualScalar; + use sophus_autodiff::dual::dual_vector::DualVector; + use sophus_autodiff::linalg::scalar::IsScalar; + use sophus_autodiff::linalg::vector::IsVector; + use sophus_autodiff::linalg::VecF64; + use sophus_autodiff::maps::vector_valued_maps::VectorValuedVectorMap; + use sophus_autodiff::points::example_points; let points = &example_points::(); assert!(points.len() >= 8); diff --git a/crates/sophus_tensor/Cargo.toml b/crates/sophus_tensor/Cargo.toml new file mode 100644 index 00000000..1e2fdb1d --- /dev/null +++ b/crates/sophus_tensor/Cargo.toml @@ -0,0 +1,27 @@ +[package] +description = "sophus - geometry for robotics and computer vision" +name = "sophus_tensor" +readme = "../../README.md" + +edition.workspace = true +include.workspace = true +keywords.workspace = true +license.workspace = true +repository.workspace = true +version.workspace = true + +[dependencies] +sophus_autodiff.workspace = true + +approx.workspace = true +bytemuck.workspace = true +nalgebra.workspace = true +log.workspace = true +ndarray.workspace = true +num-traits.workspace = true +concat-arrays.workspace = true + + +[features] +simd = ["sophus_autodiff/simd"] +std = ["sophus_autodiff/std"] diff --git a/crates/sophus_core/src/tensor/arc_tensor.rs b/crates/sophus_tensor/src/arc_tensor.rs similarity index 97% rename from crates/sophus_core/src/tensor/arc_tensor.rs rename to crates/sophus_tensor/src/arc_tensor.rs index 5dbc5993..5e8e2cd9 100644 --- a/crates/sophus_core/src/tensor/arc_tensor.rs +++ b/crates/sophus_tensor/src/arc_tensor.rs @@ -1,12 +1,10 @@ -use crate::linalg::SMat; -use crate::linalg::SVec; use crate::prelude::*; -use crate::tensor::mut_tensor::InnerScalarToVec; -use crate::tensor::mut_tensor::InnerVecToMat; -use crate::tensor::MutTensor; -use crate::tensor::TensorView; +use crate::MutTensor; +use crate::TensorView; use core::marker::PhantomData; use ndarray::Dimension; +use sophus_autodiff::linalg::SMat; +use sophus_autodiff::linalg::SVec; /// Arc tensor - a tensor with shared ownership /// @@ -337,9 +335,9 @@ arc_tensor_is_tensor_view!(5, 2, 3); fn arc_tensor_tests() { //from_mut_tensor - use crate::tensor::mut_tensor::MutTensorDDDR; - use crate::tensor::mut_tensor::MutTensorDDR; - use crate::tensor::mut_tensor::MutTensorDR; + use crate::mut_tensor::MutTensorDDDR; + use crate::mut_tensor::MutTensorDDR; + use crate::mut_tensor::MutTensorDR; { let shape = [4]; @@ -427,8 +425,8 @@ fn arc_tensor_tests() { #[cfg(feature = "std")] fn arc_tensor_std_tests() { // multi_threading - use crate::tensor::arc_tensor::ArcTensorDDRC; - use crate::tensor::mut_tensor::MutTensorDDRC; + use crate::arc_tensor::ArcTensorDDRC; + use crate::mut_tensor::MutTensorDDRC; use log::info; use std::thread; diff --git a/crates/sophus_core/src/tensor/element.rs b/crates/sophus_tensor/src/element.rs similarity index 91% rename from crates/sophus_core/src/tensor/element.rs rename to crates/sophus_tensor/src/element.rs index 63681681..05d88d25 100644 --- a/crates/sophus_core/src/tensor/element.rs +++ b/crates/sophus_tensor/src/element.rs @@ -1,9 +1,8 @@ -use crate::linalg::scalar::NumberCategory; -use crate::linalg::SMat; -use crate::linalg::SVec; -use crate::prelude::*; use core::fmt::Debug; -pub use typenum::generic_const_mappings::Const; +use sophus_autodiff::linalg::scalar::NumberCategory; +use sophus_autodiff::linalg::SMat; +use sophus_autodiff::linalg::SVec; +use sophus_autodiff::prelude::*; /// Trait for static tensors pub trait IsStaticTensor< @@ -149,19 +148,18 @@ impl STensorFormat { #[test] fn test_elements() { + use approx::assert_abs_diff_eq; #[cfg(feature = "simd")] - use crate::linalg::scalar::IsScalar; - use crate::linalg::scalar::NumberCategory; - + use sophus_autodiff::linalg::scalar::IsScalar; + use sophus_autodiff::linalg::scalar::NumberCategory; #[cfg(feature = "simd")] - use crate::linalg::BatchScalar; + use sophus_autodiff::linalg::BatchScalar; #[cfg(feature = "simd")] - use crate::linalg::BatchScalarF64; + use sophus_autodiff::linalg::BatchScalarF64; #[cfg(feature = "simd")] - use crate::linalg::BatchVecF64; + use sophus_autodiff::linalg::BatchVecF64; + use sophus_autodiff::linalg::VecF32; - use crate::linalg::VecF32; - use approx::assert_abs_diff_eq; assert_eq!(f32::number_category(), NumberCategory::Real); assert_eq!(u32::number_category(), NumberCategory::Unsigned); assert_eq!(i32::number_category(), NumberCategory::Signed); diff --git a/crates/sophus_tensor/src/lib.rs b/crates/sophus_tensor/src/lib.rs new file mode 100644 index 00000000..19ce58b7 --- /dev/null +++ b/crates/sophus_tensor/src/lib.rs @@ -0,0 +1,29 @@ +#![deny(missing_docs)] +//! # Tensor module + +/// Arc tensor +pub mod arc_tensor; +/// Tensor element +pub mod element; +/// Mutable tensor +pub mod mut_tensor; +/// Mutable tensor view +pub mod mut_tensor_view; +/// Tensor view +pub mod tensor_view; + +pub use crate::arc_tensor::ArcTensor; +pub use crate::mut_tensor::MutTensor; +pub use crate::mut_tensor_view::MutTensorView; +pub use crate::tensor_view::TensorView; + +/// sophus_tensor prelude +pub mod prelude { + pub use crate::element::IsStaticTensor; + pub use crate::mut_tensor::InnerScalarToVec; + pub use crate::mut_tensor::InnerVecToMat; + pub use crate::mut_tensor_view::IsMutTensorLike; + pub use crate::tensor_view::IsTensorLike; + pub use crate::tensor_view::IsTensorView; + pub use sophus_autodiff::prelude::*; +} diff --git a/crates/sophus_core/src/tensor/mut_tensor.rs b/crates/sophus_tensor/src/mut_tensor.rs similarity index 99% rename from crates/sophus_core/src/tensor/mut_tensor.rs rename to crates/sophus_tensor/src/mut_tensor.rs index 6cd113d9..1622733e 100644 --- a/crates/sophus_core/src/tensor/mut_tensor.rs +++ b/crates/sophus_tensor/src/mut_tensor.rs @@ -1,13 +1,13 @@ -use crate::linalg::SMat; -use crate::linalg::SVec; use crate::prelude::*; -use crate::tensor::ArcTensor; -use crate::tensor::MutTensorView; -use crate::tensor::TensorView; +use crate::ArcTensor; +use crate::MutTensorView; +use crate::TensorView; use core::fmt::Debug; use core::marker::PhantomData; use ndarray::Dim; use ndarray::Ix; +use sophus_autodiff::linalg::SMat; +use sophus_autodiff::linalg::SVec; /// mutable tensor /// @@ -507,7 +507,7 @@ mut_tensor_is_view_drank_2_plus!(5, 2, 3); #[test] fn mut_tensor_tests() { #[cfg(feature = "simd")] - use crate::linalg::BatchMatF64; + use sophus_autodiff::linalg::BatchMatF64; use log::info; { diff --git a/crates/sophus_core/src/tensor/mut_tensor_view.rs b/crates/sophus_tensor/src/mut_tensor_view.rs similarity index 99% rename from crates/sophus_core/src/tensor/mut_tensor_view.rs rename to crates/sophus_tensor/src/mut_tensor_view.rs index 5d05b632..da9a30e5 100644 --- a/crates/sophus_core/src/tensor/mut_tensor_view.rs +++ b/crates/sophus_tensor/src/mut_tensor_view.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -use crate::tensor::MutTensor; -use crate::tensor::TensorView; +use crate::MutTensor; +use crate::TensorView; use concat_arrays::concat_arrays; use core::marker::PhantomData; diff --git a/crates/sophus_core/src/tensor/tensor_view.rs b/crates/sophus_tensor/src/tensor_view.rs similarity index 99% rename from crates/sophus_core/src/tensor/tensor_view.rs rename to crates/sophus_tensor/src/tensor_view.rs index 630b2129..2f7632cc 100644 --- a/crates/sophus_core/src/tensor/tensor_view.rs +++ b/crates/sophus_tensor/src/tensor_view.rs @@ -1,9 +1,9 @@ -use crate::linalg::SMat; -use crate::linalg::SVec; use crate::prelude::*; -use crate::tensor::MutTensor; +use crate::MutTensor; use concat_arrays::concat_arrays; use core::marker::PhantomData; +use sophus_autodiff::linalg::SMat; +use sophus_autodiff::linalg::SVec; /// Tensor view /// diff --git a/crates/sophus_timeseries/Cargo.toml b/crates/sophus_timeseries/Cargo.toml new file mode 100644 index 00000000..2ff89bac --- /dev/null +++ b/crates/sophus_timeseries/Cargo.toml @@ -0,0 +1,20 @@ +[package] +description = "sophus - geometry for robotics and computer vision" +name = "sophus_timeseries" +readme = "../../README.md" + +edition.workspace = true +include.workspace = true +keywords.workspace = true +license.workspace = true +repository.workspace = true +version.workspace = true + +[dependencies] +approx.workspace = true +log.workspace = true +num-traits.workspace = true +sophus_geo.workspace = true + +[features] +simd = ["sophus_geo/simd"] diff --git a/crates/sophus_core/src/time_series.rs b/crates/sophus_timeseries/src/lib.rs similarity index 99% rename from crates/sophus_core/src/time_series.rs rename to crates/sophus_timeseries/src/lib.rs index d0cd86fb..ef0d238a 100644 --- a/crates/sophus_core/src/time_series.rs +++ b/crates/sophus_timeseries/src/lib.rs @@ -1,8 +1,8 @@ -use crate::calculus::Interval; -use crate::prelude::IsRegion; use core::ops::Index; use core::time::Duration; use log::warn; +use sophus_geo::region::Interval; +use sophus_geo::region::IsRegion; extern crate alloc; /// has time_stamp method diff --git a/crates/sophus_viewer/Cargo.toml b/crates/sophus_viewer/Cargo.toml index d15d67a6..03857845 100644 --- a/crates/sophus_viewer/Cargo.toml +++ b/crates/sophus_viewer/Cargo.toml @@ -23,7 +23,7 @@ linked-hash-map.workspace = true log.workspace = true num-traits.workspace = true thingbuf.workspace = true -sophus_core.workspace = true +sophus_autodiff.workspace = true sophus_image.workspace = true sophus_lie.workspace = true sophus_opt.workspace = true @@ -32,7 +32,7 @@ wgpu.workspace = true [features] std = [ - "sophus_core/std", + "sophus_autodiff/std", "sophus_image/std", "sophus_lie/std", "sophus_opt/std", diff --git a/crates/sophus_viewer/src/interactions.rs b/crates/sophus_viewer/src/interactions.rs index 88c4416f..f6973b84 100644 --- a/crates/sophus_viewer/src/interactions.rs +++ b/crates/sophus_viewer/src/interactions.rs @@ -5,10 +5,10 @@ pub mod orbit_interaction; use crate::interactions::inplane_interaction::InplaneInteraction; use crate::interactions::orbit_interaction::OrbitalInteraction; -use crate::preludes::*; +use crate::prelude::*; use crate::views::ViewportSize; use eframe::egui; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_image::arc_image::ArcImageF32; use sophus_image::ImageSize; use sophus_lie::Isometry3F64; diff --git a/crates/sophus_viewer/src/interactions/inplane_interaction.rs b/crates/sophus_viewer/src/interactions/inplane_interaction.rs index 26e00b7a..d6b63206 100644 --- a/crates/sophus_viewer/src/interactions/inplane_interaction.rs +++ b/crates/sophus_viewer/src/interactions/inplane_interaction.rs @@ -1,8 +1,8 @@ use crate::interactions::SceneFocus; use crate::interactions::ViewportScale; -use crate::preludes::*; +use crate::prelude::*; use eframe::egui; -use sophus_core::linalg::VecF64; +use sophus_autodiff::linalg::VecF64; use sophus_image::ImageSize; use sophus_lie::Isometry3; use sophus_lie::Isometry3F64; diff --git a/crates/sophus_viewer/src/interactions/orbit_interaction.rs b/crates/sophus_viewer/src/interactions/orbit_interaction.rs index 2826cd48..515ffa7e 100644 --- a/crates/sophus_viewer/src/interactions/orbit_interaction.rs +++ b/crates/sophus_viewer/src/interactions/orbit_interaction.rs @@ -1,9 +1,8 @@ use crate::interactions::SceneFocus; use crate::interactions::ViewportScale; -use crate::preludes::*; +use crate::prelude::*; use eframe::egui; -use sophus_core::linalg::VecF64; -use sophus_core::IsTensorLike; +use sophus_autodiff::linalg::VecF64; use sophus_image::arc_image::ArcImageF32; use sophus_image::image_view::IsImageView; use sophus_image::ImageSize; diff --git a/crates/sophus_viewer/src/lib.rs b/crates/sophus_viewer/src/lib.rs index 44b72b6c..6edd7207 100644 --- a/crates/sophus_viewer/src/lib.rs +++ b/crates/sophus_viewer/src/lib.rs @@ -25,14 +25,15 @@ pub fn recommened_eframe_native_options() -> eframe::NativeOptions { } } -/// preludes -pub mod preludes { +/// prelude +pub mod prelude { pub(crate) use alloc::boxed::Box; pub(crate) use alloc::collections::btree_map::BTreeMap; pub(crate) use alloc::collections::vec_deque::VecDeque; pub(crate) use alloc::string::String; pub(crate) use alloc::string::ToString; pub(crate) use alloc::vec::Vec; + pub use sophus_renderer::prelude::*; extern crate alloc; } diff --git a/crates/sophus_viewer/src/packets.rs b/crates/sophus_viewer/src/packets.rs index 4cf5d14c..dd5c8824 100644 --- a/crates/sophus_viewer/src/packets.rs +++ b/crates/sophus_viewer/src/packets.rs @@ -3,7 +3,7 @@ use crate::packets::plot_view_packet::PlotViewPacket; use crate::packets::scene_view_packet::SceneViewCreation; use crate::packets::scene_view_packet::SceneViewPacket; use crate::packets::scene_view_packet::SceneViewPacketContent; -use crate::preludes::*; +use crate::prelude::*; use sophus_lie::Isometry3F64; use sophus_renderer::camera::RenderCamera; use sophus_renderer::renderables::frame::ImageFrame; diff --git a/crates/sophus_viewer/src/packets/image_view_packet.rs b/crates/sophus_viewer/src/packets/image_view_packet.rs index e9be6dd0..988bb345 100644 --- a/crates/sophus_viewer/src/packets/image_view_packet.rs +++ b/crates/sophus_viewer/src/packets/image_view_packet.rs @@ -1,4 +1,4 @@ -use crate::preludes::*; +use crate::prelude::*; use sophus_renderer::renderables::frame::ImageFrame; use sophus_renderer::renderables::pixel_renderable::PixelRenderable; use sophus_renderer::renderables::scene_renderable::SceneRenderable; diff --git a/crates/sophus_viewer/src/packets/plot_view_packet.rs b/crates/sophus_viewer/src/packets/plot_view_packet.rs index f8d6ba12..bcf0a491 100644 --- a/crates/sophus_viewer/src/packets/plot_view_packet.rs +++ b/crates/sophus_viewer/src/packets/plot_view_packet.rs @@ -10,7 +10,7 @@ use crate::packets::plot_view_packet::scalar_curve::ScalarCurveStyle; use crate::packets::plot_view_packet::vec_curve::CurveVec; use crate::packets::plot_view_packet::vec_curve::CurveVecStyle; use crate::packets::plot_view_packet::vec_curve::NamedCurveVec; -use crate::preludes::*; +use crate::prelude::*; /// vec curve with confidence interval pub mod curve_vec_with_conf; diff --git a/crates/sophus_viewer/src/packets/plot_view_packet/curve_vec_with_conf.rs b/crates/sophus_viewer/src/packets/plot_view_packet/curve_vec_with_conf.rs index b35c9b2d..7f98ee7c 100644 --- a/crates/sophus_viewer/src/packets/plot_view_packet/curve_vec_with_conf.rs +++ b/crates/sophus_viewer/src/packets/plot_view_packet/curve_vec_with_conf.rs @@ -1,6 +1,6 @@ use crate::packets::plot_view_packet::ClearCondition; use crate::packets::plot_view_packet::CurveTrait; -use crate::preludes::*; +use crate::prelude::*; use sophus_renderer::renderables::color::Color; /// Vector of curves with confidence intervals diff --git a/crates/sophus_viewer/src/packets/plot_view_packet/scalar_curve.rs b/crates/sophus_viewer/src/packets/plot_view_packet/scalar_curve.rs index e8c2c39f..cafa76e2 100644 --- a/crates/sophus_viewer/src/packets/plot_view_packet/scalar_curve.rs +++ b/crates/sophus_viewer/src/packets/plot_view_packet/scalar_curve.rs @@ -1,7 +1,7 @@ use crate::packets::plot_view_packet::ClearCondition; use crate::packets::plot_view_packet::CurveTrait; use crate::packets::plot_view_packet::LineType; -use crate::preludes::*; +use crate::prelude::*; use sophus_renderer::renderables::color::Color; /// Scalar curve style diff --git a/crates/sophus_viewer/src/packets/plot_view_packet/vec_curve.rs b/crates/sophus_viewer/src/packets/plot_view_packet/vec_curve.rs index d48d2ba9..7a75afbd 100644 --- a/crates/sophus_viewer/src/packets/plot_view_packet/vec_curve.rs +++ b/crates/sophus_viewer/src/packets/plot_view_packet/vec_curve.rs @@ -1,7 +1,7 @@ use crate::packets::plot_view_packet::ClearCondition; use crate::packets::plot_view_packet::CurveTrait; use crate::packets::plot_view_packet::LineType; -use crate::preludes::*; +use crate::prelude::*; use sophus_renderer::renderables::color::Color; /// Vector of curves diff --git a/crates/sophus_viewer/src/packets/scene_view_packet.rs b/crates/sophus_viewer/src/packets/scene_view_packet.rs index 3e0bab2e..e7812765 100644 --- a/crates/sophus_viewer/src/packets/scene_view_packet.rs +++ b/crates/sophus_viewer/src/packets/scene_view_packet.rs @@ -1,4 +1,4 @@ -use crate::preludes::*; +use crate::prelude::*; use sophus_lie::Isometry3F64; use sophus_renderer::camera::RenderCamera; use sophus_renderer::renderables::scene_renderable::SceneRenderable; diff --git a/crates/sophus_viewer/src/simple_viewer.rs b/crates/sophus_viewer/src/simple_viewer.rs index 7fbb3527..733c0fb7 100644 --- a/crates/sophus_viewer/src/simple_viewer.rs +++ b/crates/sophus_viewer/src/simple_viewer.rs @@ -1,5 +1,5 @@ use crate::packets::Packet; -use crate::preludes::*; +use crate::prelude::*; use crate::viewer_base::ViewerBase; use crate::viewer_base::ViewerBaseConfig; use eframe::egui; diff --git a/crates/sophus_viewer/src/viewer_base.rs b/crates/sophus_viewer/src/viewer_base.rs index a66b5848..178323a8 100644 --- a/crates/sophus_viewer/src/viewer_base.rs +++ b/crates/sophus_viewer/src/viewer_base.rs @@ -23,7 +23,7 @@ use egui_plot::LineStyle; use egui_plot::PlotUi; use egui_plot::VLine; use linked_hash_map::LinkedHashMap; -use sophus_core::prelude::HasParams; +use sophus_autodiff::prelude::HasParams; use sophus_image::arc_image::ArcImageF32; use sophus_image::ImageSize; use sophus_lie::prelude::IsTranslationProductGroup; diff --git a/crates/sophus_viewer/src/views/active_view_info.rs b/crates/sophus_viewer/src/views/active_view_info.rs index 47f0e4da..0a1baba3 100644 --- a/crates/sophus_viewer/src/views/active_view_info.rs +++ b/crates/sophus_viewer/src/views/active_view_info.rs @@ -1,4 +1,4 @@ -use crate::preludes::*; +use crate::prelude::*; use sophus_image::ImageSize; use sophus_lie::Isometry3F64; use sophus_renderer::camera::properties::RenderCameraProperties; diff --git a/crates/sophus_viewer/src/views/image_view.rs b/crates/sophus_viewer/src/views/image_view.rs index d6a1cb4b..f41a87e4 100644 --- a/crates/sophus_viewer/src/views/image_view.rs +++ b/crates/sophus_viewer/src/views/image_view.rs @@ -1,7 +1,7 @@ use crate::interactions::inplane_interaction::InplaneInteraction; use crate::interactions::InteractionEnum; use crate::packets::image_view_packet::ImageViewPacket; -use crate::preludes::*; +use crate::prelude::*; use crate::views::View; use linked_hash_map::LinkedHashMap; use sophus_renderer::aspect_ratio::HasAspectRatio; diff --git a/crates/sophus_viewer/src/views/plot_view.rs b/crates/sophus_viewer/src/views/plot_view.rs index 3cd05661..67865a46 100644 --- a/crates/sophus_viewer/src/views/plot_view.rs +++ b/crates/sophus_viewer/src/views/plot_view.rs @@ -6,7 +6,7 @@ use crate::packets::plot_view_packet::scalar_curve::ScalarCurve; use crate::packets::plot_view_packet::vec_curve::CurveVec; use crate::packets::plot_view_packet::CurveTrait; use crate::packets::plot_view_packet::PlotViewPacket; -use crate::preludes::*; +use crate::prelude::*; use crate::views::View; use linked_hash_map::LinkedHashMap; use sophus_renderer::aspect_ratio::HasAspectRatio; diff --git a/crates/sophus_viewer/src/views/scene_view.rs b/crates/sophus_viewer/src/views/scene_view.rs index 48cc6341..d9de2be8 100644 --- a/crates/sophus_viewer/src/views/scene_view.rs +++ b/crates/sophus_viewer/src/views/scene_view.rs @@ -3,7 +3,7 @@ use crate::interactions::InteractionEnum; use crate::packets::scene_view_packet::SceneViewCreation; use crate::packets::scene_view_packet::SceneViewPacket; use crate::packets::scene_view_packet::SceneViewPacketContent; -use crate::preludes::*; +use crate::prelude::*; use crate::views::View; use linked_hash_map::LinkedHashMap; use log::warn; diff --git a/justfile b/justfile index 08c4b787..33a40de2 100644 --- a/justfile +++ b/justfile @@ -17,7 +17,7 @@ test-simd: cargo +nightly test --release --features simd test: - cargo test --release + cargo test --release --features std format: pre-commit run -a diff --git a/publish_all.sh b/publish_all.sh index 73fc4fbb..e0d58ba2 100644 --- a/publish_all.sh +++ b/publish_all.sh @@ -3,12 +3,15 @@ set -x # echo on set -e # exit on error -cargo publish -p sophus_core +cargo publish -p sophus_autodiff +cargo publish -p sophus_tensor +cargo publish -p sophus_image cargo publish -p sophus_lie cargo publish -p sophus_geo -cargo publish -p sophus_image +cargo publish -p sophus_spline cargo publish -p sophus_sensor cargo publish -p sophus_opt +cargo publish -p sophus_timeseries cargo publish -p sophus_renderer cargo publish -p sophus_sim cargo publish -p sophus_viewer