From ead874623fd063a75ee341716861ac79653f73b9 Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Sat, 21 Dec 2024 13:54:57 -0800 Subject: [PATCH] sophus-geo (#48) --- Cargo.toml | 2 +- crates/sophus/Cargo.toml | 1 + crates/sophus_core/src/lib.rs | 2 - crates/sophus_geo/Cargo.toml | 19 +++ crates/sophus_geo/src/hyperplane.rs | 117 ++++++++++++++++++ .../src}/hypersphere.rs | 9 +- .../src/geometry.rs => sophus_geo/src/lib.rs} | 2 + .../src/geometry => sophus_geo/src}/ray.rs | 8 +- .../src/renderables/scene_renderable/axes.rs | 1 + publish_all.sh | 1 + 10 files changed, 150 insertions(+), 12 deletions(-) create mode 100644 crates/sophus_geo/Cargo.toml create mode 100644 crates/sophus_geo/src/hyperplane.rs rename crates/{sophus_core/src/geometry => sophus_geo/src}/hypersphere.rs (97%) rename crates/{sophus_core/src/geometry.rs => sophus_geo/src/lib.rs} (53%) rename crates/{sophus_core/src/geometry => sophus_geo/src}/ray.rs (81%) diff --git a/Cargo.toml b/Cargo.toml index fc6a761..2f6ae5c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -30,9 +30,9 @@ sophus = {path = "crates/sophus", version = "0.11.0"} sophus_core = {path = "crates/sophus_core", 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_sensor = {path = "crates/sophus_sensor", 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 fb9866c..cd6724b 100644 --- a/crates/sophus/Cargo.toml +++ b/crates/sophus/Cargo.toml @@ -12,6 +12,7 @@ version.workspace = true [dependencies] sophus_core.workspace = true +sophus_geo.workspace = true sophus_image.workspace = true sophus_lie.workspace = true sophus_opt.workspace = true diff --git a/crates/sophus_core/src/lib.rs b/crates/sophus_core/src/lib.rs index 5f19b1b..8f6af47 100644 --- a/crates/sophus_core/src/lib.rs +++ b/crates/sophus_core/src/lib.rs @@ -30,8 +30,6 @@ extern crate std; pub mod calculus; /// floating point pub mod floating_point; -/// geometry -pub mod geometry; /// linear algebra types pub mod linalg; /// manifolds diff --git a/crates/sophus_geo/Cargo.toml b/crates/sophus_geo/Cargo.toml new file mode 100644 index 0000000..8bf7a1b --- /dev/null +++ b/crates/sophus_geo/Cargo.toml @@ -0,0 +1,19 @@ +[package] +description = "sophus - geometry for robotics and computer vision" +name = "sophus_geo" +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 +sophus_core.workspace = true +sophus_lie.workspace = true + +[features] +simd = ["sophus_core/simd"] diff --git a/crates/sophus_geo/src/hyperplane.rs b/crates/sophus_geo/src/hyperplane.rs new file mode 100644 index 0000000..94a333a --- /dev/null +++ b/crates/sophus_geo/src/hyperplane.rs @@ -0,0 +1,117 @@ +use std::borrow::Borrow; + +use sophus_core::unit_vector::UnitVector; +use sophus_lie::prelude::*; +use sophus_lie::Isometry2; +use sophus_lie::Isometry3; + +/// N-dimensional Hyperplane. +pub struct HyperPlane, const DOF: usize, const DIM: usize, const BATCH: usize> { + /// Hyperplane origin, i.e. the origin of the (N-1)d subspace. + pub origin: S::Vector, + /// Normal vector. + pub normal: UnitVector, +} + +impl, const DOF: usize, const DIM: usize, const BATCH: usize> + HyperPlane +{ + /// Projects a point onto the hyperplane. + /// + /// Given a N-d point, this function is projecting the point onto the hyperplane (along the planar + /// normal) and returning the result. + pub fn proj_onto>>(&self, point: B) -> S::Vector { + let point = point.borrow(); + let diff = point.clone() - self.origin.clone(); + point.clone() - self.normal.vector().scaled(diff.dot(self.normal.vector())) + } + + /// Returns the Jacobian of `proj_onto(point)` w.r.t. `point` itself. + pub fn dx_proj_x_onto(&self) -> S::Matrix { + // The normal is constant wrt. the input point. + let n = self.normal.vector(); + // dx (x - n * dot(x-o, n)) + // = dx x - dx n * dot(x, n) + // = I - n * dot(d x, n) [since n is constant] + // = I - n * n^T + S::Matrix::::identity() - n.outer(&n) + } + + /// Distance of a point to the hyperplane. + pub fn distance>>(&self, point: B) -> S { + let point = point.borrow(); + (self.proj_onto(point) - point.clone()).norm() + } +} + +/// A line in 2D - represented as a 2d hyperplane. +pub type Line = HyperPlane; +/// A line in 2D - for f64. +pub type LineF64 = Line; +// A plane in 3D - represented as a 3d hyperplane. +pub type Plane = HyperPlane; +/// A plane in 3D - for f64. +pub type PlaneF64 = Plane; + +impl, const BATCH: usize> Line { + /// Converting a 2d isometry to a line. + /// + /// Given an isometry "parent_from_line_origin", this function creates a line spanned by the + /// X axis of the "line_origin" frame. The line is specified relative to the "parent" + /// frame. + pub fn from_isometry2>>(parent_from_line_origin: B) -> Self { + let parent_from_line_origin = parent_from_line_origin.borrow(); + Line { + origin: parent_from_line_origin.translation(), + normal: UnitVector::from_vector_and_normalize( + &parent_from_line_origin.rotation().matrix().get_col_vec(2), + ), + } + } +} + +impl, const BATCH: usize> Plane { + /// Converting a 3d isometry to a plane. + /// + /// Given an isometry "parent_from_plane_origin", this function creates a plane spanned by the + /// X-Y axis of the "plane_origin" frame. The plane is specified relative to the "parent" + /// frame. + pub fn from_isometry3>>(parent_from_plane_origin: B) -> Self { + let parent_from_plane_origin = parent_from_plane_origin.borrow(); + Plane { + origin: parent_from_plane_origin.translation(), + normal: UnitVector::from_vector_and_normalize( + &parent_from_plane_origin.rotation().matrix().get_col_vec(2), + ), + } + } +} + +#[test] +fn plane_test() { + use sophus_core::calculus::dual::DualScalar; + use sophus_core::calculus::maps::VectorValuedMapFromVector; + use sophus_core::linalg::VecF64; + use sophus_core::linalg::EPS_F64; + + let plane = Plane::::from_isometry3(Isometry3::rot_y(0.2)); + + fn proj_x_onto, const BATCH: usize>(v: S::Vector<3>) -> S::Vector<3> { + let plane = Plane::::from_isometry3(Isometry3::rot_y(S::from_f64(0.2))); + plane.proj_onto(v) + } + + let a = VecF64::<3>::new(1.0, 2.0, 3.0); + let finite_diff = VectorValuedMapFromVector::::static_sym_diff_quotient( + proj_x_onto::, + a, + EPS_F64, + ); + let auto_grad = VectorValuedMapFromVector::::static_fw_autodiff( + proj_x_onto::, + a, + ); + + approx::assert_abs_diff_eq!(finite_diff, auto_grad, epsilon = 0.00001); + approx::assert_abs_diff_eq!(plane.dx_proj_x_onto(), auto_grad, epsilon = 0.00001); +} diff --git a/crates/sophus_core/src/geometry/hypersphere.rs b/crates/sophus_geo/src/hypersphere.rs similarity index 97% rename from crates/sophus_core/src/geometry/hypersphere.rs rename to crates/sophus_geo/src/hypersphere.rs index cb46f07..5710e98 100644 --- a/crates/sophus_core/src/geometry/hypersphere.rs +++ b/crates/sophus_geo/src/hypersphere.rs @@ -1,8 +1,7 @@ -use crate::geometry::ray::Ray; -use crate::linalg::EPS_F64; -use crate::prelude::IsScalar; -use crate::prelude::IsSingleScalar; -use crate::prelude::IsVector; +use sophus_core::linalg::EPS_F64; +use sophus_lie::prelude::*; + +use crate::ray::Ray; /// N-Sphere pub struct HyperSphere, const DIM: usize, const BATCH_SIZE: usize> { diff --git a/crates/sophus_core/src/geometry.rs b/crates/sophus_geo/src/lib.rs similarity index 53% rename from crates/sophus_core/src/geometry.rs rename to crates/sophus_geo/src/lib.rs index 64390a9..5f101ab 100644 --- a/crates/sophus_core/src/geometry.rs +++ b/crates/sophus_geo/src/lib.rs @@ -1,3 +1,5 @@ +/// hyper-plane: line in 2d, plane in 3d, ... +pub mod hyperplane; /// n-Sphere: circle, sphere, ... pub mod hypersphere; /// ray diff --git a/crates/sophus_core/src/geometry/ray.rs b/crates/sophus_geo/src/ray.rs similarity index 81% rename from crates/sophus_core/src/geometry/ray.rs rename to crates/sophus_geo/src/ray.rs index 127c899..bad1fc4 100644 --- a/crates/sophus_core/src/geometry/ray.rs +++ b/crates/sophus_geo/src/ray.rs @@ -1,7 +1,7 @@ -use crate::prelude::IsScalar; -use crate::prelude::IsSingleScalar; -use crate::prelude::IsVector; -use crate::unit_vector::UnitVector; +use sophus_core::unit_vector::UnitVector; +use sophus_lie::prelude::IsScalar; +use sophus_lie::prelude::IsSingleScalar; +use sophus_lie::prelude::IsVector; /// Ray #[derive(Clone, Debug)] diff --git a/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs b/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs index ac1f56d..dcef247 100644 --- a/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs +++ b/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs @@ -19,6 +19,7 @@ pub struct Axes3Builder { } impl Axes3Builder { + // use axes3(..) or axis3(..) for public API fn new(world_from_local: &[Isometry3F64]) -> Self { Self { world_from_local_axes: world_from_local.to_vec(), diff --git a/publish_all.sh b/publish_all.sh index 02dfaa1..73fc4fb 100644 --- a/publish_all.sh +++ b/publish_all.sh @@ -5,6 +5,7 @@ set -e # exit on error cargo publish -p sophus_core cargo publish -p sophus_lie +cargo publish -p sophus_geo cargo publish -p sophus_image cargo publish -p sophus_sensor cargo publish -p sophus_opt