diff --git a/crates/sophus/src/examples/viewer_example.rs b/crates/sophus/src/examples/viewer_example.rs index 8e387c4f..178b753e 100644 --- a/crates/sophus/src/examples/viewer_example.rs +++ b/crates/sophus/src/examples/viewer_example.rs @@ -37,7 +37,7 @@ pub fn make_distorted_frame() -> ImageFrame { let cy = 240.0; let unified_cam = DynCameraF64::new_unified( - &VecF64::from_array([focal_length, focal_length, cx, cy, 0.629, 1.22]), + VecF64::from_array([focal_length, focal_length, cx, cy, 0.629, 1.22]), image_size, ); @@ -47,7 +47,7 @@ pub fn make_distorted_frame() -> ImageFrame { for v in 0..image_size.height { for u in 0..image_size.width { let uv = VecF64::<2>::new(u as f64, v as f64); - let p_on_z1 = unified_cam.cam_unproj(&uv); + let p_on_z1 = unified_cam.cam_unproj(uv); if p_on_z1[0].abs() < 0.5 { *img.mut_pixel(u, v) = SVec::::new(255, 0, 0, 255); diff --git a/crates/sophus_core/src/calculus/dual/dual_batch_matrix.rs b/crates/sophus_core/src/calculus/dual/dual_batch_matrix.rs index 1a06a71d..c889e8a8 100644 --- a/crates/sophus_core/src/calculus/dual/dual_batch_matrix.rs +++ b/crates/sophus_core/src/calculus/dual/dual_batch_matrix.rs @@ -1,4 +1,5 @@ use crate::calculus::dual::dual_batch_scalar::DualBatchScalar; +use crate::calculus::dual::dual_matrix::DijPairM; use crate::calculus::dual::dual_matrix::DijPairMV; use crate::calculus::dual::DualBatchVector; use crate::linalg::BatchMatF64; @@ -10,6 +11,7 @@ use crate::tensor::mut_tensor::MutTensorDDR; use crate::tensor::mut_tensor::MutTensorDDRC; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::Mul; @@ -20,8 +22,6 @@ use core::simd::Mask; use core::simd::SupportedLaneCount; use num_traits::Zero; -use crate::calculus::dual::dual_matrix::DijPairM; - /// DualScalarLike matrix #[derive(Clone)] pub struct DualBatchMatrix @@ -307,10 +307,15 @@ where } } - fn from_scalar(val: DualBatchScalar) -> Self { + fn from_scalar(val: S) -> Self + where + S: Borrow>, + { + let val = val.borrow(); + DualBatchMatrix { real_part: BatchMatF64::::from_scalar(val.real_part), - dij_part: val.dij_part.map(|dij_val| { + dij_part: val.dij_part.clone().map(|dij_val| { MutTensorDDRC::from_map(&dij_val.view(), |v| { BatchMatF64::::from_scalar(*v) }) @@ -318,29 +323,37 @@ where } } - fn mat_mul( - &self, - rhs: DualBatchMatrix, - ) -> DualBatchMatrix { + fn mat_mul(&self, rhs: M) -> DualBatchMatrix + where + M: Borrow>, + { DualBatchMatrix { - real_part: self.real_part * rhs.real_part, + real_part: self.real_part.mat_mul(rhs.borrow().real_part), dij_part: DualBatchMatrix::::binary_mm_dij( &self.dij_part, - &rhs.dij_part, - |l_dij| l_dij * rhs.real_part, - |r_dij| self.real_part * r_dij, + &rhs.borrow().dij_part, + |l_dij| l_dij.mat_mul(rhs.borrow().real_part), + |r_dij| self.real_part.mat_mul(r_dij), ), } } - fn from_real_matrix(val: BatchMatF64) -> Self { + fn from_real_matrix(val: A) -> Self + where + A: Borrow>, + { + let val = val.borrow(); Self { - real_part: val, + real_part: *val, dij_part: None, } } - fn scaled(&self, s: DualBatchScalar) -> Self { + fn scaled(&self, s: Q) -> Self + where + Q: Borrow>, + { + let s = s.borrow(); DualBatchMatrix { real_part: self.real_part * s.real_part, dij_part: DualBatchMatrix::::binary_ms_dij( @@ -366,7 +379,11 @@ where } } - fn from_array2(duals: [[DualBatchScalar; COLS]; ROWS]) -> Self { + fn from_array2(duals: A) -> Self + where + A: Borrow<[[DualBatchScalar; COLS]; ROWS]>, + { + let duals = duals.borrow(); let mut shape = None; let mut val_mat = BatchMatF64::::zeros(); for i in 0..duals.len() { @@ -532,14 +549,20 @@ where } } - fn from_real_scalar_array2(vals: [[BatchScalarF64; COLS]; ROWS]) -> Self { + fn from_real_scalar_array2(vals: A) -> Self + where + A: Borrow<[[BatchScalarF64; COLS]; ROWS]>, + { DualBatchMatrix { real_part: BatchMatF64::from_real_scalar_array2(vals), dij_part: None, } } - fn from_f64_array2(vals: [[f64; COLS]; ROWS]) -> Self { + fn from_f64_array2(vals: A) -> Self + where + A: Borrow<[[f64; COLS]; ROWS]>, + { DualBatchMatrix { real_part: BatchMatF64::from_f64_array2(vals), dij_part: None, @@ -555,12 +578,16 @@ where todo!(); } - fn to_dual(self) -> as IsScalar>::DualMatrix { - self + fn to_dual(&self) -> as IsScalar>::DualMatrix { + self.clone() } - fn select(self, mask: &Mask, other: Self) -> Self { - let maybe_dij = Self::two_dx(self.dij_part, other.dij_part); + fn select(&self, mask: &Mask, other: Q) -> Self + where + Q: Borrow, + { + let other = other.borrow(); + let maybe_dij = Self::two_dx(self.dij_part.clone(), other.dij_part.clone()); DualBatchMatrix { real_part: self.real_part.select(mask, other.real_part), @@ -581,7 +608,7 @@ where } } - fn transposed(self) -> DualBatchMatrix { + fn transposed(&self) -> DualBatchMatrix { DualBatchMatrix { real_part: self.real_part.transpose(), dij_part: self.dij_part.clone().map(|_dij_val| todo!()), diff --git a/crates/sophus_core/src/calculus/dual/dual_batch_scalar.rs b/crates/sophus_core/src/calculus/dual/dual_batch_scalar.rs index 8b1c4b45..41aa0f0a 100644 --- a/crates/sophus_core/src/calculus/dual/dual_batch_scalar.rs +++ b/crates/sophus_core/src/calculus/dual/dual_batch_scalar.rs @@ -13,6 +13,7 @@ use alloc::vec::Vec; use approx::assert_abs_diff_eq; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::AddAssign; @@ -340,7 +341,7 @@ where } } - fn cos(self) -> DualBatchScalar + fn cos(&self) -> DualBatchScalar where BatchScalarF64: IsCoreScalar, LaneCount: SupportedLaneCount, @@ -367,7 +368,7 @@ where } } - fn sin(self) -> DualBatchScalar + fn sin(&self) -> DualBatchScalar where BatchScalarF64: IsCoreScalar, LaneCount: SupportedLaneCount, @@ -387,7 +388,7 @@ where } } - fn abs(self) -> Self { + fn abs(&self) -> Self { Self { real_part: self.real_part.abs(), dij_part: match self.dij_part.clone() { @@ -403,7 +404,11 @@ where } } - fn atan2(self, rhs: Self) -> Self { + fn atan2(&self, rhs: S) -> Self + where + S: Borrow, + { + let rhs = rhs.borrow(); let inv_sq_nrm: BatchScalarF64 = BatchScalarF64::ones() / (self.real_part * self.real_part + rhs.real_part * rhs.real_part); Self { @@ -421,11 +426,11 @@ where self.real_part } - fn sqrt(self) -> Self { + fn sqrt(&self) -> Self { let sqrt = self.real_part.sqrt(); Self { real_part: sqrt, - dij_part: match self.dij_part { + dij_part: match &self.dij_part { Some(dij) => { let out_dij = MutTensorDD::from_map(&dij.view(), |dij: &BatchScalarF64| { @@ -439,12 +444,12 @@ where } } - fn to_vec(self) -> DualBatchVector<1, BATCH> { + fn to_vec(&self) -> DualBatchVector<1, BATCH> { DualBatchVector::<1, BATCH> { real_part: self.real_part.real_part().to_vec(), - dij_part: match self.dij_part { + dij_part: match &self.dij_part { Some(dij) => { - let tmp = dij.inner_scalar_to_vec(); + let tmp = dij.clone().inner_scalar_to_vec(); Some(tmp) } None => None, @@ -452,7 +457,7 @@ where } } - fn tan(self) -> Self { + fn tan(&self) -> Self { Self { real_part: self.real_part.tan(), dij_part: match self.dij_part.clone() { @@ -470,7 +475,7 @@ where } } - fn acos(self) -> Self { + fn acos(&self) -> Self { Self { real_part: self.real_part.acos(), dij_part: match self.dij_part.clone() { @@ -489,7 +494,7 @@ where } } - fn asin(self) -> Self { + fn asin(&self) -> Self { Self { real_part: self.real_part.asin(), dij_part: match self.dij_part.clone() { @@ -508,7 +513,7 @@ where } } - fn atan(self) -> Self { + fn atan(&self) -> Self { Self { real_part: self.real_part.atan(), dij_part: match self.dij_part.clone() { @@ -526,7 +531,7 @@ where } } - fn fract(self) -> Self { + fn fract(&self) -> Self { Self { real_part: self.real_part.fract(), dij_part: match self.dij_part.clone() { @@ -576,14 +581,14 @@ where self.real_part.less_equal(&rhs.real_part) } - fn to_dual(self) -> Self::DualScalar { - self + fn to_dual(&self) -> Self::DualScalar { + self.clone() } - fn select(self, mask: &Self::Mask, other: Self) -> Self { + fn select(&self, mask: &Self::Mask, other: Self) -> Self { Self { real_part: self.real_part.select(mask, other.real_part), - dij_part: match (self.dij_part, other.dij_part) { + dij_part: match (self.dij_part.clone(), other.dij_part) { (Some(lhs), Some(rhs)) => { let dyn_mat = MutTensorDD::from_map2( &lhs.view(), diff --git a/crates/sophus_core/src/calculus/dual/dual_batch_vector.rs b/crates/sophus_core/src/calculus/dual/dual_batch_vector.rs index 712dab1b..9fd038c5 100644 --- a/crates/sophus_core/src/calculus/dual/dual_batch_vector.rs +++ b/crates/sophus_core/src/calculus/dual/dual_batch_vector.rs @@ -9,6 +9,7 @@ use crate::tensor::mut_tensor::MutTensorDD; use crate::tensor::mut_tensor::MutTensorDDR; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::Neg; @@ -280,10 +281,11 @@ where } } - fn outer( - self, - rhs: DualBatchVector, - ) -> DualBatchMatrix { + fn outer(&self, rhs: V) -> DualBatchMatrix + where + V: Borrow>, + { + let rhs = rhs.borrow(); let mut result = DualBatchMatrix::zeros(); for i in 0..ROWS { for j in 0..R2 { @@ -311,11 +313,15 @@ where } } - fn from_array(duals: [DualBatchScalar; ROWS]) -> Self { + fn from_array(duals: A) -> Self + where + A: Borrow<[DualBatchScalar; ROWS]>, + { + let duals = duals.borrow().clone(); let mut shape = None; let mut val_v = BatchVecF64::::zeros(); for i in 0..duals.len() { - let d = duals.clone()[i].clone(); + let d = duals[i].clone(); val_v[i] = d.real_part; if d.dij_part.is_some() { @@ -350,16 +356,23 @@ where } } - fn from_real_array(vals: [BatchScalarF64; ROWS]) -> Self { + fn from_real_array(vals: A) -> Self + where + A: Borrow<[BatchScalarF64; ROWS]>, + { + let vals = vals.borrow(); DualBatchVector { real_part: BatchVecF64::from_real_array(vals), dij_part: None, } } - fn from_real_vector(val: BatchVecF64) -> Self { + fn from_real_vector(val: A) -> Self + where + A: Borrow>, + { Self { - real_part: val, + real_part: val.borrow().clone(), dij_part: None, } } @@ -368,10 +381,10 @@ where &self.real_part } - fn to_mat(self) -> DualBatchMatrix { + fn to_mat(&self) -> DualBatchMatrix { DualBatchMatrix { real_part: self.real_part, - dij_part: self.dij_part.map(|dij| dij.inner_vec_to_mat()), + dij_part: self.dij_part.clone().map(|dij| dij.inner_vec_to_mat()), } } @@ -404,7 +417,11 @@ where } } - fn scaled(&self, s: DualBatchScalar) -> Self { + fn scaled(&self, s: Q) -> Self + where + Q: Borrow>, + { + let s = s.borrow(); DualBatchVector { real_part: self.real_part * s.real_part, dij_part: Self::binary_vs_dij( @@ -416,7 +433,11 @@ where } } - fn dot(self, rhs: Self) -> DualBatchScalar { + fn dot(&self, rhs: Q) -> DualBatchScalar + where + Q: Borrow, + { + let rhs = rhs.borrow(); let mut sum = DualBatchScalar::from_f64(0.0); for i in 0..ROWS { @@ -431,14 +452,22 @@ where .scaled(DualBatchScalar::::from_f64(1.0) / self.norm()) } - fn from_f64_array(vals: [f64; ROWS]) -> Self { + fn from_f64_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>, + { + let vals = vals.borrow(); DualBatchVector { real_part: BatchVecF64::from_f64_array(vals), dij_part: None, } } - fn from_scalar_array(vals: [DualBatchScalar; ROWS]) -> Self { + fn from_scalar_array(vals: A) -> Self + where + A: Borrow<[DualBatchScalar; ROWS]>, + { + let vals = vals.borrow(); let mut shape = None; let mut val_v = BatchVecF64::::zeros(); for i in 0..vals.len() { @@ -489,15 +518,19 @@ where } } - fn to_dual(self) -> as IsScalar>::DualVector { - self + fn to_dual(&self) -> as IsScalar>::DualVector { + self.clone() } - fn select(self, mask: &Mask, other: Self) -> Self { - let maybe_dij = Self::two_dx(self.dij_part, other.dij_part); + fn select(&self, mask: &Mask, other: Q) -> Self + where + Q: Borrow, + { + let other = other.borrow(); + let maybe_dij = Self::two_dx(self.dij_part.clone(), other.dij_part.clone()); Self { - real_part: IsVector::select(self.real_part, mask, other.real_part), + real_part: IsVector::select(&self.real_part, mask, other.real_part), dij_part: match maybe_dij { Some(dij) => { let mut r = @@ -505,7 +538,7 @@ where for i in 0..dij.shape()[0] { for j in 0..dij.shape()[1] { *r.get_mut([i, j]) = - IsVector::select(dij.lhs.get([i, j]), mask, dij.rhs.get([i, j])); + IsVector::select(&dij.lhs.get([i, j]), mask, dij.rhs.get([i, j])); } } Some(r) diff --git a/crates/sophus_core/src/calculus/dual/dual_matrix.rs b/crates/sophus_core/src/calculus/dual/dual_matrix.rs index 892c5f28..b5d54c72 100644 --- a/crates/sophus_core/src/calculus/dual/dual_matrix.rs +++ b/crates/sophus_core/src/calculus/dual/dual_matrix.rs @@ -8,6 +8,7 @@ use crate::tensor::mut_tensor::MutTensorDDR; use crate::tensor::mut_tensor::MutTensorDDRC; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::Mul; @@ -326,7 +327,11 @@ impl RelativeEq for DualMatrix impl IsMatrix for DualMatrix { - fn mat_mul(&self, rhs: DualMatrix) -> DualMatrix { + fn mat_mul(&self, other: M) -> DualMatrix + where + M: Borrow>, + { + let rhs = other.borrow(); DualMatrix { real_part: self.real_part * rhs.real_part, dij_part: DualMatrix::binary_mm_dij( @@ -338,23 +343,34 @@ impl IsMatrix } } - fn from_scalar(val: DualScalar) -> Self { + fn from_scalar(val: S) -> Self + where + S: Borrow, + { + let val = val.borrow(); DualMatrix { real_part: MatF64::::from_scalar(val.real_part), - dij_part: val.dij_part.map(|dij_val| { + dij_part: val.dij_part.clone().map(|dij_val| { MutTensorDDRC::from_map(&dij_val.view(), |v| MatF64::::from_scalar(*v)) }), } } - fn from_real_matrix(val: MatF64) -> Self { + fn from_real_matrix(val: A) -> Self + where + A: Borrow>, + { Self { - real_part: val, + real_part: *val.borrow(), dij_part: None, } } - fn scaled(&self, s: DualScalar) -> Self { + fn scaled(&self, s: Q) -> Self + where + Q: Borrow, + { + let s = s.borrow(); DualMatrix { real_part: self.real_part * s.real_part, dij_part: DualMatrix::binary_ms_dij( @@ -380,7 +396,11 @@ impl IsMatrix } } - fn from_array2(duals: [[DualScalar; COLS]; ROWS]) -> Self { + fn from_array2(duals: A) -> Self + where + A: Borrow<[[DualScalar; COLS]; ROWS]>, + { + let duals = duals.borrow(); let mut shape = None; let mut val_mat = MatF64::::zeros(); for i in 0..duals.len() { @@ -529,14 +549,20 @@ impl IsMatrix } } - fn from_real_scalar_array2(vals: [[f64; COLS]; ROWS]) -> Self { + fn from_real_scalar_array2(vals: A) -> Self + where + A: Borrow<[[f64; COLS]; ROWS]>, + { DualMatrix { real_part: MatF64::from_real_scalar_array2(vals), dij_part: None, } } - fn from_f64_array2(vals: [[f64; COLS]; ROWS]) -> Self { + fn from_f64_array2(vals: A) -> Self + where + A: Borrow<[[f64; COLS]; ROWS]>, + { DualMatrix { real_part: MatF64::from_real_scalar_array2(vals), dij_part: None, @@ -555,15 +581,18 @@ impl IsMatrix todo!(); } - fn to_dual(self) -> >::DualMatrix { - self + fn to_dual(&self) -> >::DualMatrix { + self.clone() } - fn select(self, mask: &bool, other: Self) -> Self { + fn select(&self, mask: &bool, other: Q) -> Self + where + Q: Borrow, + { if *mask { - self + self.clone() } else { - other + other.borrow().clone() } } @@ -580,7 +609,7 @@ impl IsMatrix } } - fn transposed(self) -> >::Matrix { + fn transposed(&self) -> >::Matrix { todo!(); } } @@ -750,7 +779,7 @@ fn dual_matrix_tests() { fn mat_mul2_fn, const BATCH: usize>( x: S::Matrix<4, 4>, ) -> S::Matrix<4, 4> { - x.mat_mul(x.clone()) + x.mat_mul(&x) } let m_4x4 = <$scalar as IsScalar<$batch>>::Matrix::<4, 4>::from_f64_array2([ diff --git a/crates/sophus_core/src/calculus/dual/dual_scalar.rs b/crates/sophus_core/src/calculus/dual/dual_scalar.rs index e0295081..b7d202b9 100644 --- a/crates/sophus_core/src/calculus/dual/dual_scalar.rs +++ b/crates/sophus_core/src/calculus/dual/dual_scalar.rs @@ -9,6 +9,7 @@ use crate::tensor::mut_tensor::InnerScalarToVec; use crate::tensor::mut_tensor::MutTensorDD; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::AddAssign; @@ -286,7 +287,7 @@ impl IsScalar<1> for DualScalar { [self.real_part] } - fn cos(self) -> DualScalar { + fn cos(&self) -> DualScalar { Self { real_part: self.real_part.cos(), dij_part: match self.dij_part.clone() { @@ -301,7 +302,7 @@ impl IsScalar<1> for DualScalar { } } - fn sin(self) -> DualScalar { + fn sin(&self) -> DualScalar { Self { real_part: self.real_part.sin(), dij_part: match self.dij_part.clone() { @@ -316,7 +317,7 @@ impl IsScalar<1> for DualScalar { } } - fn abs(self) -> Self { + fn abs(&self) -> Self { Self { real_part: self.real_part.abs(), dij_part: match self.dij_part.clone() { @@ -332,7 +333,11 @@ impl IsScalar<1> for DualScalar { } } - fn atan2(self, rhs: Self) -> Self { + fn atan2(&self, rhs: S) -> Self + where + S: Borrow, + { + let rhs = rhs.borrow(); let inv_sq_nrm: f64 = 1.0 / (self.real_part * self.real_part + rhs.real_part * rhs.real_part); Self { @@ -350,11 +355,11 @@ impl IsScalar<1> for DualScalar { self.real_part } - fn sqrt(self) -> Self { + fn sqrt(&self) -> Self { let sqrt = self.real_part.sqrt(); Self { real_part: sqrt, - dij_part: match self.dij_part { + dij_part: match self.dij_part.clone() { Some(dij) => { let out_dij = >::from_map(&dij.view(), |dij: &f64| { (*dij) * 1.0 / (2.0 * sqrt) @@ -366,10 +371,10 @@ impl IsScalar<1> for DualScalar { } } - fn to_vec(self) -> DualVector<1> { + fn to_vec(&self) -> DualVector<1> { DualVector::<1> { real_part: self.real_part.real_part().to_vec(), - dij_part: match self.dij_part { + dij_part: match self.dij_part.clone() { Some(dij) => { let tmp = dij.inner_scalar_to_vec(); Some(tmp) @@ -379,7 +384,7 @@ impl IsScalar<1> for DualScalar { } } - fn tan(self) -> Self { + fn tan(&self) -> Self { Self { real_part: self.real_part.tan(), dij_part: match self.dij_part.clone() { @@ -396,7 +401,7 @@ impl IsScalar<1> for DualScalar { } } - fn acos(self) -> Self { + fn acos(&self) -> Self { Self { real_part: self.real_part.acos(), dij_part: match self.dij_part.clone() { @@ -411,7 +416,7 @@ impl IsScalar<1> for DualScalar { } } - fn asin(self) -> Self { + fn asin(&self) -> Self { Self { real_part: self.real_part.asin(), dij_part: match self.dij_part.clone() { @@ -426,7 +431,7 @@ impl IsScalar<1> for DualScalar { } } - fn atan(self) -> Self { + fn atan(&self) -> Self { Self { real_part: self.real_part.atan(), dij_part: match self.dij_part.clone() { @@ -441,7 +446,7 @@ impl IsScalar<1> for DualScalar { } } - fn fract(self) -> Self { + fn fract(&self) -> Self { Self { real_part: self.real_part.fract(), dij_part: match self.dij_part.clone() { @@ -490,13 +495,13 @@ impl IsScalar<1> for DualScalar { self.real_part.less_equal(&rhs.real_part) } - fn to_dual(self) -> Self::DualScalar { - self + fn to_dual(&self) -> Self::DualScalar { + self.clone() } - fn select(self, mask: &Self::Mask, other: Self) -> Self { + fn select(&self, mask: &Self::Mask, other: Self) -> Self { if *mask { - self + self.clone() } else { other } diff --git a/crates/sophus_core/src/calculus/dual/dual_vector.rs b/crates/sophus_core/src/calculus/dual/dual_vector.rs index 005b35c8..e999a5c7 100644 --- a/crates/sophus_core/src/calculus/dual/dual_vector.rs +++ b/crates/sophus_core/src/calculus/dual/dual_vector.rs @@ -7,6 +7,7 @@ use crate::tensor::mut_tensor::MutTensorDD; use crate::tensor::mut_tensor::MutTensorDDR; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::Neg; @@ -302,7 +303,11 @@ impl IsVector for DualVector { } } - fn from_array(duals: [DualScalar; ROWS]) -> Self { + fn from_array(duals: A) -> Self + where + A: Borrow<[DualScalar; ROWS]>, + { + let duals = duals.borrow(); let mut shape = None; let mut val_v = VecF64::::zeros(); for i in 0..duals.len() { @@ -341,16 +346,22 @@ impl IsVector for DualVector { } } - fn from_real_array(vals: [f64; ROWS]) -> Self { + fn from_real_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>, + { DualVector { real_part: VecF64::from_real_array(vals), dij_part: None, } } - fn from_real_vector(val: VecF64) -> Self { + fn from_real_vector(val: A) -> Self + where + A: Borrow>, + { Self { - real_part: val, + real_part: *val.borrow(), dij_part: None, } } @@ -359,10 +370,10 @@ impl IsVector for DualVector { &self.real_part } - fn to_mat(self) -> DualMatrix { + fn to_mat(&self) -> DualMatrix { DualMatrix { real_part: self.real_part, - dij_part: self.dij_part.map(|dij| dij.inner_vec_to_mat()), + dij_part: self.dij_part.clone().map(|dij| dij.inner_vec_to_mat()), } } @@ -393,7 +404,11 @@ impl IsVector for DualVector { } } - fn scaled(&self, s: DualScalar) -> Self { + fn scaled(&self, s: U) -> Self + where + U: Borrow, + { + let s = s.borrow(); DualVector { real_part: self.real_part * s.real_part, dij_part: Self::binary_vs_dij( @@ -405,11 +420,14 @@ impl IsVector for DualVector { } } - fn dot(self, rhs: Self) -> DualScalar { + fn dot(&self, rhs: V) -> DualScalar + where + V: Borrow, + { let mut sum = ::from_f64(0.0); for i in 0..ROWS { - sum += self.get_elem(i) * rhs.get_elem(i); + sum += self.get_elem(i) * rhs.borrow().get_elem(i); } sum @@ -420,14 +438,21 @@ impl IsVector for DualVector { .scaled(::from_f64(1.0) / self.norm()) } - fn from_f64_array(vals: [f64; ROWS]) -> Self { + fn from_f64_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>, + { DualVector { real_part: VecF64::from_f64_array(vals), dij_part: None, } } - fn from_scalar_array(vals: [DualScalar; ROWS]) -> Self { + fn from_scalar_array(vals: A) -> Self + where + A: Borrow<[DualScalar; ROWS]>, + { + let vals = vals.borrow(); let mut shape = None; let mut val_v = VecF64::::zeros(); for i in 0..vals.len() { @@ -478,28 +503,31 @@ impl IsVector for DualVector { } } - fn to_dual(self) -> >::DualVector { - self + fn to_dual(&self) -> >::DualVector { + self.clone() } - fn outer( - self, - rhs: DualVector, - ) -> >::Matrix { + fn outer(&self, rhs: V) -> >::Matrix + where + V: Borrow>, + { let mut out = DualMatrix::::zeros(); for i in 0..ROWS { for j in 0..R2 { - out.set_elem([i, j], self.get_elem(i) * rhs.get_elem(j)); + out.set_elem([i, j], self.get_elem(i) * rhs.borrow().get_elem(j)); } } out } - fn select(self, mask: &bool, other: Self) -> Self { + fn select(&self, mask: &bool, other: Q) -> Self + where + Q: Borrow, + { if *mask { - self + self.clone() } else { - other + other.borrow().clone() } } diff --git a/crates/sophus_core/src/linalg/batch_matrix.rs b/crates/sophus_core/src/linalg/batch_matrix.rs index fd6c7f16..a47a4a5f 100644 --- a/crates/sophus_core/src/linalg/batch_matrix.rs +++ b/crates/sophus_core/src/linalg/batch_matrix.rs @@ -3,6 +3,7 @@ use crate::linalg::BatchMatF64; use crate::linalg::BatchScalarF64; use crate::linalg::BatchVecF64; use crate::prelude::*; +use core::borrow::Borrow; use core::simd::LaneCount; use core::simd::Mask; use core::simd::SupportedLaneCount; @@ -12,35 +13,57 @@ impl where LaneCount: SupportedLaneCount, { - fn from_scalar(val: BatchScalarF64) -> Self { - Self::from_element(val) + fn from_scalar(val: S) -> Self + where + S: Borrow>, + { + let val = val.borrow(); + Self::from_element(*val) } - fn from_real_matrix(val: Self) -> Self { - val + fn from_real_matrix(val: M) -> Self + where + M: Borrow>, + { + val.borrow().clone() } fn real_matrix(&self) -> &Self { self } - fn scaled(&self, v: BatchScalarF64) -> Self { - self * v + fn scaled(&self, v: S) -> Self + where + S: Borrow>, + { + *self * *v.borrow() } fn identity() -> Self { nalgebra::SMatrix::, ROWS, COLS>::identity() } - fn from_array2(vals: [[BatchScalarF64; COLS]; ROWS]) -> Self { + fn from_array2(vals: A) -> Self + where + A: Borrow<[[BatchScalarF64; COLS]; ROWS]>, + { + let vals = vals.borrow(); Self::from_fn(|r, c| vals[r][c]) } - fn from_real_scalar_array2(vals: [[BatchScalarF64; COLS]; ROWS]) -> Self { + fn from_real_scalar_array2(vals: A) -> Self + where + A: Borrow<[[BatchScalarF64; COLS]; ROWS]>, + { + let vals = vals.borrow(); Self::from_fn(|r, c| vals[r][c]) } - fn from_f64_array2(vals: [[f64; COLS]; ROWS]) -> Self { + fn from_f64_array2(vals: A) -> Self + where + A: Borrow<[[f64; COLS]; ROWS]>, + { + let vals = vals.borrow(); Self::from_fn(|r, c| BatchScalarF64::::from_f64(vals[r][c])) } @@ -93,11 +116,11 @@ where }) } - fn mat_mul( - &self, - other: BatchMatF64, - ) -> BatchMatF64 { - self * other + fn mat_mul(&self, other: M) -> BatchMatF64 + where + M: Borrow>, + { + *self * other.borrow().clone() } fn set_col_vec(&mut self, c: usize, v: BatchVecF64) { @@ -124,19 +147,22 @@ where Self::from_element(BatchScalarF64::::from_f64(val)) } - fn to_dual(self) -> as IsScalar>::DualMatrix { + fn to_dual(&self) -> as IsScalar>::DualMatrix { DualBatchMatrix::from_real_matrix(self) } - fn select(self, mask: &Mask, other: Self) -> Self { - self.zip_map(&other, |a, b| a.select(mask, b)) + fn select(&self, mask: &Mask, other: Q) -> Self + where + Q: Borrow, + { + self.zip_map(other.borrow(), |a, b| a.select(mask, b)) } fn set_elem(&mut self, idx: [usize; 2], val: BatchScalarF64) { self[(idx[0], idx[1])] = val; } - fn transposed(self) -> BatchMatF64 { + fn transposed(&self) -> BatchMatF64 { self.transpose() } } diff --git a/crates/sophus_core/src/linalg/batch_scalar.rs b/crates/sophus_core/src/linalg/batch_scalar.rs index e58324da..56a50086 100644 --- a/crates/sophus_core/src/linalg/batch_scalar.rs +++ b/crates/sophus_core/src/linalg/batch_scalar.rs @@ -14,6 +14,7 @@ use alloc::vec; use alloc::vec::Vec; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::AddAssign; use core::ops::Div; @@ -233,7 +234,7 @@ where } } - fn abs(self) -> Self { + fn abs(&self) -> Self { BatchScalarF64 { 0: SimdFloat::abs(self.0), } @@ -249,57 +250,61 @@ where self.0.to_array() } - fn cos(self) -> Self { + fn cos(&self) -> Self { BatchScalarF64 { 0: sleef::Sleef::cos(self.0), } } - fn sin(self) -> Self { + fn sin(&self) -> Self { BatchScalarF64 { 0: sleef::Sleef::sin(self.0), } } - fn tan(self) -> Self { + fn tan(&self) -> Self { BatchScalarF64 { 0: sleef::Sleef::tan(self.0), } } - fn acos(self) -> Self { + fn acos(&self) -> Self { BatchScalarF64 { 0: sleef::Sleef::acos(self.0), } } - fn asin(self) -> Self { + fn asin(&self) -> Self { BatchScalarF64 { 0: sleef::Sleef::asin(self.0), } } - fn atan(self) -> Self { + fn atan(&self) -> Self { BatchScalarF64 { 0: sleef::Sleef::atan(self.0), } } - fn sqrt(self) -> Self { + fn sqrt(&self) -> Self { BatchScalarF64 { 0: self.0.sqrt() } } - fn atan2(self, x: Self) -> Self { + fn atan2(&self, rhs: S) -> Self + where + S: Borrow, + { + let rhs = rhs.borrow(); BatchScalarF64 { - 0: sleef::Sleef::atan2(self.0, x.0), + 0: sleef::Sleef::atan2(self.0, rhs.0), } } - fn to_vec(self) -> Self::Vector<1> { + fn to_vec(&self) -> Self::Vector<1> { BatchVecF64::<1, BATCH>::from_scalar(self) } - fn fract(self) -> Self { + fn fract(&self) -> Self { BatchScalarF64 { 0: self.0.frfrexp(), } @@ -317,11 +322,11 @@ where type DualMatrix = DualBatchMatrix; - fn to_dual(self) -> Self::DualScalar { - DualBatchScalar::from_real_scalar(self) + fn to_dual(&self) -> Self::DualScalar { + DualBatchScalar::from_real_scalar(self.clone()) } - fn select(self, mask: &Self::Mask, other: Self) -> Self { + fn select(&self, mask: &Self::Mask, other: Self) -> Self { BatchScalarF64 { 0: mask.select(self.0, other.0), } diff --git a/crates/sophus_core/src/linalg/batch_vector.rs b/crates/sophus_core/src/linalg/batch_vector.rs index 1e674d6f..c50c19b6 100644 --- a/crates/sophus_core/src/linalg/batch_vector.rs +++ b/crates/sophus_core/src/linalg/batch_vector.rs @@ -3,6 +3,7 @@ use crate::linalg::BatchMatF64; use crate::linalg::BatchScalarF64; use crate::linalg::BatchVecF64; use crate::prelude::*; +use core::borrow::Borrow; use core::simd::LaneCount; use core::simd::Mask; use core::simd::SupportedLaneCount; @@ -24,28 +25,50 @@ where m } - fn dot(self, rhs: Self) -> BatchScalarF64 { - (self.transpose() * rhs)[0] + fn dot(&self, rhs: V) -> BatchScalarF64 + where + V: Borrow, + { + (self.transpose() * rhs.borrow().clone())[0] } - fn from_array(vals: [BatchScalarF64; ROWS]) -> Self { + fn from_array(vals: A) -> Self + where + A: Borrow<[BatchScalarF64; ROWS]>, + { + let vals = vals.borrow(); Self::from_fn(|i, _| vals[i]) } - fn from_real_array(vals: [BatchScalarF64; ROWS]) -> Self { + fn from_real_array(vals: A) -> Self + where + A: Borrow<[BatchScalarF64; ROWS]>, + { + let vals = vals.borrow(); Self::from_fn(|i, _| vals[i]) } - fn from_f64_array(vals: [f64; ROWS]) -> Self { + fn from_f64_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>, + { + let vals = vals.borrow(); Self::from_fn(|i, _| BatchScalarF64::::from_f64(vals[i])) } - fn from_scalar_array(vals: [BatchScalarF64; ROWS]) -> Self { + fn from_scalar_array(vals: A) -> Self + where + A: Borrow<[BatchScalarF64; ROWS]>, + { + let vals = vals.borrow(); Self::from_fn(|i, _| vals[i]) } - fn from_real_vector(val: BatchVecF64) -> Self { - val + fn from_real_vector(val: A) -> Self + where + A: Borrow>, + { + val.borrow().clone() } fn get_elem(&self, idx: usize) -> BatchScalarF64 { @@ -69,8 +92,11 @@ where self } - fn scaled(&self, v: BatchScalarF64) -> Self { - self * v + fn scaled(&self, v: S) -> Self + where + S: Borrow>, + { + self * v.borrow().clone() } fn set_elem(&mut self, idx: usize, v: BatchScalarF64) { @@ -86,24 +112,30 @@ where squared_norm } - fn to_mat(self) -> BatchMatF64 { - self + fn to_mat(&self) -> BatchMatF64 { + self.clone() } fn from_f64(val: f64) -> Self { Self::from_element(BatchScalarF64::::from_f64(val)) } - fn to_dual(self) -> as IsScalar>::DualVector { + fn to_dual(&self) -> as IsScalar>::DualVector { DualBatchVector::from_real_vector(self) } - fn outer(self, rhs: BatchVecF64) -> BatchMatF64 { - self * rhs.transpose() + fn outer(&self, rhs: V) -> BatchMatF64 + where + V: Borrow>, + { + self * rhs.borrow().transpose() } - fn select(self, mask: &Mask, other: Self) -> Self { - self.zip_map(&other, |a, b| a.select(mask, b)) + fn select(&self, mask: &Mask, other: Q) -> Self + where + Q: Borrow, + { + self.zip_map(other.borrow(), |a, b| a.select(mask, b)) } fn get_fixed_subvec(&self, start_r: usize) -> BatchVecF64 { diff --git a/crates/sophus_core/src/linalg/matrix.rs b/crates/sophus_core/src/linalg/matrix.rs index 46152628..04c60d9e 100644 --- a/crates/sophus_core/src/linalg/matrix.rs +++ b/crates/sophus_core/src/linalg/matrix.rs @@ -4,6 +4,7 @@ use crate::linalg::VecF64; use crate::prelude::*; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::Index; @@ -51,7 +52,9 @@ pub trait IsMatrix< ) -> Self; /// creates matrix from a 2d array of scalars - fn from_array2(vals: [[S; COLS]; ROWS]) -> Self; + fn from_array2(vals: A) -> Self + where + A: Borrow<[[S; COLS]; ROWS]>; /// creates matrix with all real elements (and lanes) set to the given value /// @@ -62,18 +65,26 @@ pub trait IsMatrix< /// /// - all lanes are set to the same value /// - for dual numbers, the infinitesimal part is set to zero - fn from_f64_array2(vals: [[f64; COLS]; ROWS]) -> Self; + fn from_f64_array2(vals: A) -> Self + where + A: Borrow<[[f64; COLS]; ROWS]>; /// creates matrix from a 2d array of real scalars /// /// (for dual numbers, the infinitesimal part is set to zero) - fn from_real_scalar_array2(vals: [[S::RealScalar; COLS]; ROWS]) -> Self; + fn from_real_scalar_array2(vals: A) -> Self + where + A: Borrow<[[S::RealScalar; COLS]; ROWS]>; /// create a constant matrix - fn from_real_matrix(val: S::RealMatrix) -> Self; + fn from_real_matrix(val: A) -> Self + where + A: Borrow>; /// create a constant scalar - fn from_scalar(val: S) -> Self; + fn from_scalar(val: Q) -> Self + where + Q: Borrow; /// extract column vector fn get_col_vec(&self, c: usize) -> S::Vector; @@ -95,7 +106,9 @@ pub trait IsMatrix< fn identity() -> Self; /// matrix multiplication - fn mat_mul(&self, other: S::Matrix) -> S::Matrix; + fn mat_mul(&self, other: M) -> S::Matrix + where + M: Borrow>; /// ones fn ones() -> Self { @@ -106,12 +119,16 @@ pub trait IsMatrix< fn real_matrix(&self) -> &S::RealMatrix; /// return scaled matrix - fn scaled(&self, v: S) -> Self; + fn scaled(&self, v: Q) -> Self + where + Q: Borrow; /// Returns self if mask is true, otherwise returns other /// /// For batch matrices, this is a lane-wise operation - fn select(self, mask: &S::Mask, other: Self) -> Self; + fn select(&self, mask: &S::Mask, other: Q) -> Self + where + Q: Borrow; /// set i-th element fn set_elem(&mut self, idx: [usize; 2], val: S); @@ -120,13 +137,13 @@ pub trait IsMatrix< fn set_col_vec(&mut self, c: usize, v: S::Vector); /// transpose - fn transposed(self) -> S::Matrix; + fn transposed(&self) -> S::Matrix; /// Return dual matrix /// /// If self is a real matrix, this will return a dual matrix with the infinitesimal part set to /// zero: (self, 0ϵ) - fn to_dual(self) -> S::DualMatrix; + fn to_dual(&self) -> S::DualMatrix; /// zeros fn zeros() -> Self { @@ -165,15 +182,25 @@ impl IsSingleMatrix for M } impl IsMatrix for MatF64 { - fn from_real_matrix(val: MatF64) -> Self { - val + fn from_real_matrix(val: A) -> Self + where + A: Borrow>, + { + *val.borrow() } - fn from_scalar(val: f64) -> Self { - Self::from_f64(val) + fn from_scalar(val: S) -> Self + where + S: Borrow, + { + Self::from_f64(*val.borrow()) } - fn from_array2(vals: [[f64; COLS]; ROWS]) -> MatF64 { + fn from_array2(vals: A) -> MatF64 + where + A: Borrow<[[f64; COLS]; ROWS]>, + { + let vals = vals.borrow(); let mut m = MatF64::::zeros(); for c in 0..COLS { @@ -184,7 +211,11 @@ impl IsMatrix for MatF m } - fn from_real_scalar_array2(vals: [[f64; COLS]; ROWS]) -> Self { + fn from_real_scalar_array2(vals: A) -> Self + where + A: Borrow<[[f64; COLS]; ROWS]>, + { + let vals = vals.borrow(); let mut m = MatF64::::zeros(); for c in 0..COLS { for r in 0..ROWS { @@ -206,8 +237,11 @@ impl IsMatrix for MatF self } - fn mat_mul(&self, other: MatF64) -> MatF64 { - self * other + fn mat_mul(&self, other: M) -> MatF64 + where + M: Borrow>, + { + self * other.borrow() } fn block_mat2x1( @@ -267,11 +301,18 @@ impl IsMatrix for MatF self.fixed_view::<1, COLS>(r, 0).transpose() } - fn scaled(&self, v: f64) -> Self { - self * v + fn scaled(&self, v: Q) -> Self + where + Q: Borrow, + { + *self * *v.borrow() } - fn from_f64_array2(vals: [[f64; COLS]; ROWS]) -> Self { + fn from_f64_array2(vals: A) -> Self + where + A: Borrow<[[f64; COLS]; ROWS]>, + { + let vals = vals.borrow(); let mut m = MatF64::::zeros(); for c in 0..COLS { for r in 0..ROWS { @@ -289,15 +330,18 @@ impl IsMatrix for MatF self.fixed_columns_mut::<1>(c).copy_from(&v); } - fn to_dual(self) -> >::DualMatrix { + fn to_dual(&self) -> >::DualMatrix { DualMatrix::from_real_matrix(self) } - fn select(self, mask: &bool, other: Self) -> Self { + fn select(&self, mask: &bool, other: Q) -> Self + where + Q: Borrow, + { if *mask { - self + *self } else { - other + *other.borrow() } } @@ -305,7 +349,7 @@ impl IsMatrix for MatF self[(idx[0], idx[1])] = val; } - fn transposed(self) -> MatF64 { + fn transposed(&self) -> MatF64 { self.transpose() } } diff --git a/crates/sophus_core/src/linalg/scalar.rs b/crates/sophus_core/src/linalg/scalar.rs index 3075856d..2551324b 100644 --- a/crates/sophus_core/src/linalg/scalar.rs +++ b/crates/sophus_core/src/linalg/scalar.rs @@ -8,6 +8,7 @@ use crate::prelude::*; use approx::assert_abs_diff_eq; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::AddAssign; @@ -123,22 +124,24 @@ pub trait IsScalar: >; /// absolute value - fn abs(self) -> Self; + fn abs(&self) -> Self; /// arccosine - fn acos(self) -> Self; + fn acos(&self) -> Self; /// arcsine - fn asin(self) -> Self; + fn asin(&self) -> Self; /// arctangent - fn atan(self) -> Self; + fn atan(&self) -> Self; /// arctangent2 - fn atan2(self, x: Self) -> Self; + fn atan2(&self, x: S) -> Self + where + S: Borrow; /// cosine - fn cos(self) -> Self; + fn cos(&self) -> Self; /// eps fn eps() -> Self; @@ -150,7 +153,7 @@ pub trait IsScalar: fn floor(&self) -> Self::RealScalar; /// fractional part - fn fract(self) -> Self; + fn fract(&self) -> Self; /// Creates a scalar with all real lanes set the given value /// @@ -189,7 +192,7 @@ pub trait IsScalar: /// Return the self if the mask is true, otherwise the other value /// /// This is a lane-wise operation - fn select(self, mask: &Self::Mask, other: Self) -> Self; + fn select(&self, mask: &Self::Mask, other: Self) -> Self; /// Return the sign of the scalar /// @@ -199,15 +202,15 @@ pub trait IsScalar: fn signum(&self) -> Self; /// sine - fn sin(self) -> Self; + fn sin(&self) -> Self; /// square root - fn sqrt(self) -> Self; + fn sqrt(&self) -> Self; /// Returns dual number representation /// /// If self is a real number, the infinitesimal part is zero: (self, 0ϵ) - fn to_dual(self) -> Self::DualScalar; + fn to_dual(&self) -> Self::DualScalar; /// Return as a real array /// @@ -215,10 +218,10 @@ pub trait IsScalar: fn to_real_array(&self) -> [f64; BATCH_SIZE]; /// tangent - fn tan(self) -> Self; + fn tan(&self) -> Self; /// return as a vector - fn to_vec(self) -> Self::Vector<1>; + fn to_vec(&self) -> Self::Vector<1>; /// zeros fn zeros() -> Self { @@ -291,24 +294,24 @@ impl IsScalar<1> for f64 { alloc::vec![1.0, 2.0, 3.0] } - fn abs(self) -> f64 { - f64::abs(self) + fn abs(&self) -> f64 { + f64::abs(*self) } - fn cos(self) -> f64 { - f64::cos(self) + fn cos(&self) -> f64 { + f64::cos(*self) } fn eps() -> f64 { EPS_F64 } - fn sin(self) -> f64 { - f64::sin(self) + fn sin(&self) -> f64 { + f64::sin(*self) } - fn sqrt(self) -> f64 { - f64::sqrt(self) + fn sqrt(&self) -> f64 { + f64::sqrt(*self) } fn from_f64(val: f64) -> f64 { @@ -319,8 +322,11 @@ impl IsScalar<1> for f64 { val } - fn atan2(self, x: Self) -> Self { - self.atan2(x) + fn atan2(&self, x: S) -> Self + where + S: Borrow, + { + f64::atan2(*self, *x.borrow()) } fn from_real_array(arr: [Self::RealScalar; 1]) -> Self { @@ -335,28 +341,28 @@ impl IsScalar<1> for f64 { *self } - fn to_vec(self) -> VecF64<1> { - VecF64::<1>::new(self) + fn to_vec(&self) -> VecF64<1> { + VecF64::<1>::new(*self) } - fn tan(self) -> Self { - self.tan() + fn tan(&self) -> Self { + f64::tan(*self) } - fn acos(self) -> Self { - self.acos() + fn acos(&self) -> Self { + f64::acos(*self) } - fn asin(self) -> Self { - self.asin() + fn asin(&self) -> Self { + f64::asin(*self) } - fn atan(self) -> Self { - self.atan() + fn atan(&self) -> Self { + f64::atan(*self) } - fn fract(self) -> Self { - f64::fract(self) + fn fract(&self) -> Self { + f64::fract(*self) } fn floor(&self) -> f64 { @@ -375,13 +381,13 @@ impl IsScalar<1> for f64 { type DualMatrix = DualMatrix; - fn to_dual(self) -> Self::DualScalar { - DualScalar::from_f64(self) + fn to_dual(&self) -> Self::DualScalar { + DualScalar::from_f64(*self) } - fn select(self, mask: &Self::Mask, other: Self) -> Self { + fn select(&self, mask: &Self::Mask, other: Self) -> Self { if *mask { - self + *self } else { other } diff --git a/crates/sophus_core/src/linalg/vector.rs b/crates/sophus_core/src/linalg/vector.rs index f913af43..efa3478a 100644 --- a/crates/sophus_core/src/linalg/vector.rs +++ b/crates/sophus_core/src/linalg/vector.rs @@ -4,6 +4,7 @@ use crate::linalg::VecF64; use crate::prelude::*; use approx::AbsDiffEq; use approx::RelativeEq; +use core::borrow::Borrow; use core::fmt::Debug; use core::ops::Add; use core::ops::Index; @@ -29,22 +30,32 @@ pub trait IsVector, const ROWS: usize, const BATCH_SIZE: ) -> Self; /// dot product - fn dot(self, rhs: Self) -> S; + fn dot(&self, rhs: V) -> S + where + V: Borrow; /// create a vector from an array - fn from_array(vals: [S; ROWS]) -> Self; + fn from_array(vals: A) -> Self + where + A: Borrow<[S; ROWS]>; /// create a constant vector from an array - fn from_real_array(vals: [S::RealScalar; ROWS]) -> Self; + fn from_real_array(vals: A) -> Self + where + A: Borrow<[S::RealScalar; ROWS]>; /// create a constant vector - fn from_real_vector(val: S::RealVector) -> Self; + fn from_real_vector(val: A) -> Self + where + A: Borrow>; /// create a constant scalar fn from_f64(val: f64) -> Self; /// create a constant vector from an array - fn from_f64_array(vals: [f64; ROWS]) -> Self; + fn from_f64_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>; /// get ith element fn get_elem(&self, idx: usize) -> S; @@ -53,7 +64,9 @@ pub trait IsVector, const ROWS: usize, const BATCH_SIZE: fn get_fixed_subvec(&self, start_r: usize) -> S::Vector; /// create a constant vector from an array - fn from_scalar_array(vals: [S; ROWS]) -> Self; + fn from_scalar_array(vals: A) -> Self + where + A: Borrow<[S; ROWS]>; /// norm fn norm(&self) -> S; @@ -62,7 +75,9 @@ pub trait IsVector, const ROWS: usize, const BATCH_SIZE: fn normalized(&self) -> Self; /// outer product - fn outer(self, rhs: S::Vector) -> S::Matrix; + fn outer(&self, rhs: V) -> S::Matrix + where + V: Borrow>; /// return the real part fn real_vector(&self) -> &S::RealVector; @@ -70,10 +85,14 @@ pub trait IsVector, const ROWS: usize, const BATCH_SIZE: /// Returns self if mask is true, otherwise returns other /// /// For batch vectors, this is a lane-wise operation - fn select(self, mask: &S::Mask, other: Self) -> Self; + fn select(&self, mask: &S::Mask, other: O) -> Self + where + O: Borrow; /// return scaled vector - fn scaled(&self, v: S) -> Self; + fn scaled(&self, v: U) -> Self + where + U: Borrow; /// set ith element to given scalar fn set_elem(&mut self, idx: usize, v: S); @@ -86,11 +105,11 @@ pub trait IsVector, const ROWS: usize, const BATCH_SIZE: /// If self is a real vector, this will return a dual vector with the infinitesimal part set to /// zero: (self, 0ϵ) fn to_dual( - self, + &self, ) -> <>::DualScalar as IsScalar>::Vector; /// return the matrix representation - in self as a column vector - fn to_mat(self) -> S::Matrix; + fn to_mat(&self) -> S::Matrix; /// ones fn ones() -> Self { @@ -146,24 +165,39 @@ impl IsVector for VecF64 { m } - fn from_array(vals: [f64; ROWS]) -> VecF64 { - VecF64::::from_row_slice(&vals[..]) + fn from_array(vals: A) -> VecF64 + where + A: Borrow<[f64; ROWS]>, + { + VecF64::::from_row_slice(&vals.borrow()[..]) } - fn from_real_array(vals: [f64; ROWS]) -> Self { - VecF64::::from_row_slice(&vals[..]) + fn from_real_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>, + { + VecF64::::from_row_slice(&vals.borrow()[..]) } - fn from_real_vector(val: VecF64) -> Self { - val + fn from_real_vector(val: A) -> Self + where + A: Borrow>, + { + *val.borrow() } - fn from_f64_array(vals: [f64; ROWS]) -> Self { - VecF64::::from_row_slice(&vals[..]) + fn from_f64_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>, + { + VecF64::::from_row_slice(&vals.borrow()[..]) } - fn from_scalar_array(vals: [f64; ROWS]) -> Self { - VecF64::::from_row_slice(&vals[..]) + fn from_scalar_array(vals: A) -> Self + where + A: Borrow<[f64; ROWS]>, + { + VecF64::::from_row_slice(&vals.borrow()[..]) } fn get_elem(&self, idx: usize) -> f64 { @@ -186,16 +220,22 @@ impl IsVector for VecF64 { self.norm_squared() } - fn to_mat(self) -> MatF64 { - self + fn to_mat(&self) -> MatF64 { + *self } - fn scaled(&self, v: f64) -> Self { - self * v + fn scaled(&self, v: U) -> Self + where + U: Borrow, + { + self * *v.borrow() } - fn dot(self, rhs: Self) -> f64 { - VecF64::dot(&self, &rhs) + fn dot(&self, rhs: V) -> f64 + where + V: Borrow, + { + VecF64::dot(self, rhs.borrow()) } fn normalized(&self) -> Self { @@ -206,19 +246,25 @@ impl IsVector for VecF64 { VecF64::::from_element(val) } - fn to_dual(self) -> >::DualVector { - DualVector::from_real_vector(self) + fn to_dual(&self) -> >::DualVector { + DualVector::from_real_vector(*self) } - fn outer(self, rhs: VecF64) -> MatF64 { - self * rhs.transpose() + fn outer(&self, rhs: V) -> MatF64 + where + V: Borrow>, + { + self * rhs.borrow().transpose() } - fn select(self, mask: &bool, other: Self) -> Self { + fn select(&self, mask: &bool, other: Q) -> Self + where + Q: Borrow, + { if *mask { - self + *self } else { - other + *other.borrow() } } @@ -229,8 +275,8 @@ impl IsVector for VecF64 { /// cross product pub fn cross, const BATCH: usize>( - lhs: S::Vector<3>, - rhs: S::Vector<3>, + lhs: &S::Vector<3>, + rhs: &S::Vector<3>, ) -> S::Vector<3> { let l0 = lhs.get_elem(0); let l1 = lhs.get_elem(1); diff --git a/crates/sophus_core/src/manifold/unit_vector.rs b/crates/sophus_core/src/manifold/unit_vector.rs index 1da19f3b..732cd564 100644 --- a/crates/sophus_core/src/manifold/unit_vector.rs +++ b/crates/sophus_core/src/manifold/unit_vector.rs @@ -8,6 +8,7 @@ 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; extern crate alloc; @@ -80,7 +81,7 @@ impl, const DOF: usize, const DIM: usize, const BATCH_SI let near_zero = theta.clone().abs().less_equal(&eps); unit_x - .scaled(S::from_f64(0.0).atan2(x.clone())) + .scaled(S::from_f64(0.0).atan2(&x)) .select(&near_zero, tail.scaled(theta.clone().atan2(x) / theta)) } } @@ -96,9 +97,12 @@ impl, const DOF: usize, const DIM: usize, const BATCH_SI impl, const DOF: usize, const DIM: usize, const BATCH_SIZE: usize> ParamsImpl for UnitVectorImpl { - fn are_params_valid(params: &S::Vector) -> S::Mask { + fn are_params_valid

(params: P) -> S::Mask + where + P: Borrow>, + { let eps = S::from_f64(-1e6); - (params.squared_norm() - S::from_f64(1.0)) + (params.borrow().squared_norm() - S::from_f64(1.0)) .abs() .less_equal(&eps) } @@ -242,7 +246,7 @@ impl, const DOF: usize, const DIM: usize, const BATCH_SI if vector.squared_norm().less_equal(&eps).any() { return Err(NearZeroUnitVector); } - Ok(Self::from_params(&vector.normalized())) + Ok(Self::from_params(vector.normalized())) } /// Returns self as vector @@ -259,7 +263,10 @@ impl, const DOF: usize, const DIM: usize, const BATCH_SI impl, const DOF: usize, const DIM: usize, const BATCH_SIZE: usize> ParamsImpl for UnitVector { - fn are_params_valid(params: &S::Vector) -> S::Mask { + fn are_params_valid

(params: P) -> S::Mask + where + P: Borrow>, + { UnitVectorImpl::::are_params_valid(params) } @@ -275,11 +282,17 @@ impl, const DOF: usize, const DIM: usize, const BATCH_SI impl, const DOF: usize, const DIM: usize, const BATCH_SIZE: usize> HasParams for UnitVector { - fn from_params(params: &S::Vector) -> Self { - Self::try_from(params).unwrap() + fn from_params

(params: P) -> Self + where + P: Borrow>, + { + Self::try_from(params.borrow()).unwrap() } - fn set_params(&mut self, params: &S::Vector) { + fn set_params

(&mut self, params: P) + where + P: Borrow>, + { self.params = Self::from_params(params).params().clone(); } diff --git a/crates/sophus_core/src/params.rs b/crates/sophus_core/src/params.rs index 7ba3d16e..08fc65eb 100644 --- a/crates/sophus_core/src/params.rs +++ b/crates/sophus_core/src/params.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use crate::linalg::VecF64; use crate::points::example_points; use crate::prelude::*; @@ -9,7 +11,9 @@ extern crate alloc; /// Parameter implementation. pub trait ParamsImpl, const PARAMS: usize, const BATCH_SIZE: usize> { /// Is the parameter vector valid? - fn are_params_valid(params: &S::Vector) -> S::Mask; + fn are_params_valid

(params: P) -> S::Mask + where + P: Borrow>; /// Examples of valid parameter vectors. fn params_examples() -> alloc::vec::Vec>; /// Examples of invalid parameter vectors. @@ -21,15 +25,22 @@ pub trait HasParams, const PARAMS: usize, const BATCH_SI ParamsImpl { /// Create from parameters. - fn from_params(params: &S::Vector) -> Self; + fn from_params

(params: P) -> Self + where + P: Borrow>; /// Set parameters. - fn set_params(&mut self, params: &S::Vector); + fn set_params

(&mut self, params: P) + where + P: Borrow>; /// Get parameters. fn params(&self) -> &S::Vector; } impl ParamsImpl for VecF64 { - fn are_params_valid(_params: &VecF64) -> bool { + fn are_params_valid

(_params: P) -> bool + where + P: Borrow>, + { true } @@ -43,12 +54,18 @@ impl ParamsImpl for VecF64 { } impl HasParams for VecF64 { - fn from_params(params: &VecF64) -> Self { - *params + fn from_params

(params: P) -> Self + where + P: Borrow>, + { + *params.borrow() } - fn set_params(&mut self, params: &VecF64) { - *self = *params + fn set_params

(&mut self, params: P) + where + P: Borrow>, + { + *self = *params.borrow(); } fn params(&self) -> &VecF64 { diff --git a/crates/sophus_lie/src/factor_lie_group.rs b/crates/sophus_lie/src/factor_lie_group.rs index 75167f6c..66a72076 100644 --- a/crates/sophus_lie/src/factor_lie_group.rs +++ b/crates/sophus_lie/src/factor_lie_group.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use crate::lie_group::LieGroup; use crate::prelude::*; use crate::traits::IsRealLieFactorGroupImpl; @@ -24,31 +26,44 @@ impl< > LieGroup { /// V matrix - used in the exponential map - pub fn mat_v(tangent: &S::Vector) -> S::Matrix { - G::mat_v(tangent) + pub fn mat_v(tangent: T) -> S::Matrix + where + T: Borrow>, + { + G::mat_v(tangent.borrow()) } /// V matrix inverse - used in the logarithmic map - pub fn mat_v_inverse(tangent: &S::Vector) -> S::Matrix { - G::mat_v_inverse(tangent) + pub fn mat_v_inverse(tangent: T) -> S::Matrix + where + T: Borrow>, + { + G::mat_v_inverse(tangent.borrow()) } /// derivative of V matrix - pub fn dx_mat_v(tangent: &S::Vector) -> [S::Matrix; DOF] { - G::dx_mat_v(tangent) + pub fn dx_mat_v(tangent: T) -> [S::Matrix; DOF] + where + T: Borrow>, + { + G::dx_mat_v(tangent.borrow()) } /// derivative of V matrix inverse - pub fn dx_mat_v_inverse(tangent: &S::Vector) -> [S::Matrix; DOF] { - G::dx_mat_v_inverse(tangent) + pub fn dx_mat_v_inverse(tangent: T) -> [S::Matrix; DOF] + where + T: Borrow>, + { + G::dx_mat_v_inverse(tangent.borrow()) } /// derivative of V matrix times point - pub fn dparams_matrix_times_point( - params: &S::Vector, - point: &S::Vector, - ) -> S::Matrix { - G::dparams_matrix_times_point(params, point) + pub fn dparams_matrix_times_point(params: PA, point: PO) -> S::Matrix + where + PA: Borrow>, + PO: Borrow>, + { + G::dparams_matrix_times_point(params.borrow(), point.borrow()) } } diff --git a/crates/sophus_lie/src/groups/isometry2.rs b/crates/sophus_lie/src/groups/isometry2.rs index c0cb8ecd..10c8e0cf 100644 --- a/crates/sophus_lie/src/groups/isometry2.rs +++ b/crates/sophus_lie/src/groups/isometry2.rs @@ -1,7 +1,9 @@ -use crate::average::iterative_average; -use crate::average::IterativeAverageError; +use core::borrow::Borrow; + use crate::groups::rotation2::Rotation2Impl; use crate::groups::translation_product_product::TranslationProductGroupImpl; +use crate::lie_group::average::iterative_average; +use crate::lie_group::average::IterativeAverageError; use crate::lie_group::LieGroup; use crate::prelude::*; use crate::traits::EmptySliceError; @@ -22,40 +24,62 @@ pub type Isometry2F64 = Isometry2; impl, const BATCH: usize> Isometry2 { /// create isometry from translation and rotation - pub fn from_translation_and_rotation( - translation: &S::Vector<2>, - rotation: &Rotation2, - ) -> Self { + pub fn from_translation_and_rotation(translation: P, rotation: F) -> Self + where + P: Borrow>, + F: Borrow>, + { Self::from_translation_and_factor(translation, rotation) } /// create isometry from translation - pub fn from_translation(translation: &S::Vector<2>) -> Self { - Self::from_translation_and_factor(translation, &Rotation2::identity()) + pub fn from_translation

(translation: P) -> Self + where + P: Borrow>, + { + Self::from_translation_and_factor(translation, Rotation2::identity()) } /// create isometry from rotation - pub fn from_rotation(rotation: &Rotation2) -> Self { - Self::from_translation_and_factor(&S::Vector::<2>::zeros(), rotation) + pub fn from_rotation(rotation: F) -> Self + where + F: Borrow>, + { + Self::from_translation_and_factor(S::Vector::<2>::zeros(), rotation) } /// translate along x axis - pub fn trans_x(x: S) -> Self { - Self::from_translation(&S::Vector::from_array([x, S::zero()])) + pub fn trans_x(x: U) -> Self + where + U: Borrow, + { + let x: &S = x.borrow(); + Self::from_translation(S::Vector::from_array([x.clone(), S::zero()])) } /// translate along y axis - pub fn trans_y(y: S) -> Self { - Self::from_translation(&S::Vector::from_array([S::zero(), y])) + pub fn trans_y(y: U) -> Self + where + U: Borrow, + { + let y: &S = y.borrow(); + Self::from_translation(S::Vector::from_array([S::zero(), y.clone()])) } /// Rotate by angle - pub fn rot(theta: S) -> Self { - Self::from_rotation(&Rotation2::rot(theta)) + pub fn rot(theta: U) -> Self + where + U: Borrow, + { + let theta: &S = theta.borrow(); + Self::from_rotation(Rotation2::rot(theta)) } /// set rotation - pub fn set_rotation(&mut self, rotation: &Rotation2) { + pub fn set_rotation(&mut self, rotation: F) + where + F: Borrow>, + { self.set_factor(rotation) } @@ -98,7 +122,7 @@ impl HasAverage for Isometry2; impl, const BATCH: usize> Isometry3 { /// create isometry from translation and rotation - pub fn from_translation_and_rotation( - translation: &S::Vector<3>, - rotation: &Rotation3, - ) -> Self { + pub fn from_translation_and_rotation(translation: P, rotation: F) -> Self + where + P: Borrow>, + F: Borrow>, + { Self::from_translation_and_factor(translation, rotation) } - /// rotate around x axis - pub fn rot_x(theta: S) -> Self { - Self::from_rotation(&Rotation3::rot_x(theta)) - } - - /// rotate around y axis - pub fn rot_y(theta: S) -> Self { - Self::from_rotation(&Rotation3::rot_y(theta)) + /// create isometry from translation + pub fn from_translation

(translation: P) -> Self + where + P: Borrow>, + { + Self::from_translation_and_factor(translation, Rotation3::identity()) } - /// rotate around z axis - pub fn rot_z(theta: S) -> Self { - Self::from_rotation(&Rotation3::rot_z(theta)) + /// create isometry from rotation + pub fn from_rotation(rotation: F) -> Self + where + F: Borrow>, + { + Self::from_translation_and_factor(S::Vector::<3>::zeros(), rotation) } /// translate along x axis - pub fn trans_x(x: S) -> Self { - Self::from_translation(&S::Vector::from_array([x, S::zero(), S::zero()])) + pub fn trans_x(x: U) -> Self + where + U: Borrow, + { + let x: &S = x.borrow(); + Self::from_translation(S::Vector::from_array([x.clone(), S::zero(), S::zero()])) } /// translate along y axis - pub fn trans_y(y: S) -> Self { - Self::from_translation(&S::Vector::from_array([S::zero(), y, S::zero()])) + pub fn trans_y(y: U) -> Self + where + U: Borrow, + { + let y: &S = y.borrow(); + Self::from_translation(S::Vector::from_array([S::zero(), y.clone(), S::zero()])) } /// translate along z axis - pub fn trans_z(z: S) -> Self { - Self::from_translation(&S::Vector::from_array([S::zero(), S::zero(), z])) + pub fn trans_z(z: U) -> Self + where + U: Borrow, + { + let z: &S = z.borrow(); + Self::from_translation(S::Vector::from_array([S::zero(), S::zero(), z.clone()])) } - /// create isometry from translation - pub fn from_translation(translation: &S::Vector<3>) -> Self { - Self::from_translation_and_factor(translation, &Rotation3::identity()) - } - - /// create isometry from rotation - pub fn from_rotation(rotation: &Rotation3) -> Self { - Self::from_translation_and_factor(&S::Vector::<3>::zeros(), rotation) + /// Rotate by angle + pub fn rot_x(theta: U) -> Self + where + U: Borrow, + { + let theta: &S = theta.borrow(); + Self::from_rotation(Rotation3::rot_x(theta)) } /// set rotation - pub fn set_rotation(&mut self, rotation: &Rotation3) { + pub fn set_rotation(&mut self, rotation: F) + where + F: Borrow>, + { self.set_factor(rotation) } @@ -110,7 +128,7 @@ impl HasAverage for Isometry3, const BATCH_SIZE: usize> ParamsImpl::exp(&S::Vector::<1>::from_array([angle])) + Rotation2::::exp(S::Vector::<1>::from_array([angle])) .params() .clone(), ); @@ -51,8 +52,11 @@ impl, const BATCH_SIZE: usize> ParamsImpl) -> S::Mask { - let norm = params.norm(); + fn are_params_valid

(params: P) -> S::Mask + where + P: Borrow>, + { + let norm = params.borrow().norm(); (norm - S::from_f64(1.0)) .abs() .less_equal(&S::from_f64(EPS_F64)) @@ -164,7 +168,7 @@ impl, const BATCH_SIZE: usize> S::Matrix::from_real_scalar_array2([[S::RealScalar::zeros()], [S::RealScalar::ones()]]) } - fn dx_exp_x_times_point_at_0(point: S::Vector<2>) -> S::Matrix<2, 1> { + fn dx_exp_x_times_point_at_0(point: &S::Vector<2>) -> S::Matrix<2, 1> { S::Matrix::from_array2([[-point.get_elem(1)], [point.get_elem(0)]]) } @@ -203,8 +207,12 @@ pub type Rotation2F64 = Rotation2; impl, const BATCH: usize> Rotation2 { /// Rotate by angle - pub fn rot(theta: S) -> Self { - Rotation2::exp(&S::Vector::<1>::from_array([theta])) + pub fn rot(theta: U) -> Self + where + U: Borrow, + { + let theta: &S = theta.borrow(); + Rotation2::exp(S::Vector::<1>::from_array([theta.clone()])) } } @@ -328,21 +336,19 @@ impl HasAverage for Rotation2, const BATCH: usize> ParamsImpl for Rotatio const NEAR_MINUS_PI: f64 = f64::consts::PI - 1e-6; alloc::vec![ - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, 0.0, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, 0.0])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.1, 0.5, -0.1])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.1, 0.5, -0.1])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.1, 2.0, -0.1])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.1, 2.0, -0.1])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, 0.2, 1.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, 0.2, 1.0])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([-0.2, 0.0, 0.8])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([-0.2, 0.0, 0.8])) .params() .clone(), // Test cases around +π and -π - Rotation3::::exp(&S::Vector::<3>::from_f64_array([NEAR_PI, 0.0, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([NEAR_PI, 0.0, 0.0])) .params() .clone(), // +π rotation about the x-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([NEAR_MINUS_PI, 0.0, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([NEAR_MINUS_PI, 0.0, 0.0])) .params() .clone(), // -π rotation about the x-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, NEAR_PI, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, NEAR_PI, 0.0])) .params() .clone(), // +π rotation about the y-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, NEAR_MINUS_PI, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, NEAR_MINUS_PI, 0.0])) .params() .clone(), // -π rotation about the y-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, 0.0, NEAR_PI])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, NEAR_PI])) .params() .clone(), // +π rotation about the z-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, 0.0, NEAR_MINUS_PI])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, NEAR_MINUS_PI])) .params() .clone(), // -π rotation about the z-axis // Close to +π and -π, but not exactly, to test boundary behavior - Rotation3::::exp(&S::Vector::<3>::from_f64_array([3.0, 0.0, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([3.0, 0.0, 0.0])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([-3.0, 0.0, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([-3.0, 0.0, 0.0])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, 3.0, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, 3.0, 0.0])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, -3.0, 0.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, -3.0, 0.0])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, 0.0, 3.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, 3.0])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([0.0, 0.0, -3.0])) + Rotation3::::exp(S::Vector::<3>::from_f64_array([0.0, 0.0, -3.0])) .params() .clone(), // Halfway to π rotations, to cover intermediate rotations - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ f64::consts::FRAC_PI_2, 0.0, 0.0, ])) .params() .clone(), // +π/2 about x-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ 0.0, f64::consts::FRAC_PI_2, 0.0, ])) .params() .clone(), // +π/2 about y-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ 0.0, 0.0, f64::consts::FRAC_PI_2, ])) .params() .clone(), // +π/2 about z-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ -f64::consts::FRAC_PI_2, 0.0, 0.0, ])) .params() .clone(), // -π/2 about x-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ 0.0, -f64::consts::FRAC_PI_2, 0.0, ])) .params() .clone(), // -π/2 about y-axis - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ 0.0, 0.0, -f64::consts::FRAC_PI_2, @@ -139,21 +140,21 @@ impl, const BATCH: usize> ParamsImpl for Rotatio .params() .clone(), // -π/2 about z-axis // Complex combination rotations around the boundary - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ NEAR_PI / 2.0, NEAR_PI / 2.0, 0.0, ])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ NEAR_MINUS_PI / 2.0, 0.0, NEAR_PI / 2.0, ])) .params() .clone(), - Rotation3::::exp(&S::Vector::<3>::from_f64_array([ + Rotation3::::exp(S::Vector::<3>::from_f64_array([ 0.0, NEAR_PI / 2.0, NEAR_PI / 2.0, @@ -171,8 +172,11 @@ impl, const BATCH: usize> ParamsImpl for Rotatio ] } - fn are_params_valid(params: &S::Vector<4>) -> S::Mask { - let norm = params.norm(); + fn are_params_valid

(params: P) -> S::Mask + where + P: Borrow>, + { + let norm = params.borrow().norm(); (norm - S::from_f64(1.0)) .abs() .less_equal(&S::from_f64(EPS_F64)) @@ -255,7 +259,7 @@ impl, const BATCH: usize> IsLieGroupImpl, const BATCH: usize> IsLieGroupImpl) -> S::Matrix<3, 3> { let ivec = params.get_fixed_subvec::<3>(1); - let re = params.get_elem(0); + let re = ¶ms.get_elem(0); let unit_x = S::Vector::from_f64_array([1.0, 0.0, 0.0]); let unit_y = S::Vector::from_f64_array([0.0, 1.0, 0.0]); let unit_z = S::Vector::from_f64_array([0.0, 0.0, 1.0]); - let two = S::from_f64(2.0); + let two = &S::from_f64(2.0); - let uv_x: S::Vector<3> = - cross::(ivec.clone(), unit_x.clone()).scaled(two.clone()); - let uv_y: S::Vector<3> = - cross::(ivec.clone(), unit_y.clone()).scaled(two.clone()); - let uv_z: S::Vector<3> = cross::(ivec.clone(), unit_z.clone()).scaled(two); + let uv_x: S::Vector<3> = cross::(&ivec, &unit_x).scaled(two); + let uv_y: S::Vector<3> = cross::(&ivec, &unit_y).scaled(two); + let uv_z: S::Vector<3> = cross::(&ivec, &unit_z).scaled(two); - let col_x = - unit_x + cross::(ivec.clone(), uv_x.clone()) + uv_x.scaled(re.clone()); - let col_y = - unit_y + cross::(ivec.clone(), uv_y.clone()) + uv_y.scaled(re.clone()); - let col_z = - unit_z + cross::(ivec.clone(), uv_z.clone()) + uv_z.scaled(re.clone()); + let col_x = unit_x + cross::(&ivec, &uv_x) + uv_x.scaled(re); + let col_y = unit_y + cross::(&ivec, &uv_y) + uv_y.scaled(re); + let col_z = unit_z + cross::(&ivec, &uv_z) + uv_z.scaled(re); S::Matrix::block_mat1x2::<1, 2>( col_x.to_mat(), @@ -349,10 +348,10 @@ impl, const BATCH: usize> IsLieGroupImpl(1); let rhs_ivec = rhs_params.get_fixed_subvec::<3>(1); - let re = lhs_re.clone() * rhs_re.clone() - lhs_ivec.clone().dot(rhs_ivec.clone()); + let re = lhs_re.clone() * rhs_re.clone() - lhs_ivec.dot(&rhs_ivec); let ivec = rhs_ivec.scaled(lhs_re) + lhs_ivec.scaled(rhs_re) - + cross::(lhs_ivec, rhs_ivec); + + cross::(&lhs_ivec, &rhs_ivec); let mut params = S::Vector::block_vec2(re.to_vec(), ivec); @@ -387,7 +386,7 @@ impl, const BATCH: usize> IsRealLieGroupImpl(1); let rhs_ivec = b.get_fixed_subvec::<3>(1); - let re = lhs_re * rhs_re - lhs_ivec.clone().dot(rhs_ivec.clone()); + let re = lhs_re * rhs_re - lhs_ivec.dot(rhs_ivec); let is_positive = S::from_f64(0.0).less_equal(&re); @@ -418,7 +417,7 @@ impl, const BATCH: usize> IsRealLieGroupImpl(1); let rhs_ivec = b.get_fixed_subvec::<3>(1); - let re = lhs_re * rhs_re - lhs_ivec.clone().dot(rhs_ivec.clone()); + let re = lhs_re * rhs_re - lhs_ivec.dot(&rhs_ivec); let is_positive = S::from_f64(0.0).less_equal(&re); let a_real = a.get_elem(0); @@ -443,8 +442,8 @@ impl, const BATCH: usize> IsRealLieGroupImpl) -> S::Matrix<3, 3> { - Self::hat(&-point) + fn dx_exp_x_times_point_at_0(point: &S::Vector<3>) -> S::Matrix<3, 3> { + Self::hat(&-point.clone()) } fn dx_exp(omega: &S::Vector<3>) -> S::Matrix<4, 3> { @@ -503,7 +502,7 @@ impl, const BATCH: usize> IsRealLieGroupImpl, const BATCH: usize> crate::traits::IsLieFactorGroupImpl fn mat_v(omega: &S::Vector<3>) -> S::Matrix<3, 3> { let theta_sq = omega.squared_norm(); let mat_omega: S::Matrix<3, 3> = Rotation3Impl::::hat(omega); - let mat_omega_sq = mat_omega.clone().mat_mul(mat_omega.clone()); + let mat_omega_sq = mat_omega.mat_mul(&mat_omega); let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64)); let mat_v0 = S::Matrix::<3, 3>::identity() + mat_omega.scaled(S::from_f64(0.5)); - let theta = theta_sq.clone().sqrt(); + let theta = theta_sq.sqrt(); let mat_v = S::Matrix::<3, 3>::identity() + mat_omega.scaled((S::from_f64(1.0) - theta.clone().cos()) / theta_sq.clone()) + mat_omega_sq.scaled((theta.clone() - theta.clone().sin()) / (theta_sq * theta)); @@ -547,16 +546,16 @@ impl, const BATCH: usize> crate::traits::IsLieFactorGroupImpl } fn mat_v_inverse(omega: &S::Vector<3>) -> S::Matrix<3, 3> { - let theta_sq = omega.clone().dot(omega.clone()); + let theta_sq = omega.dot(omega); let mat_omega: S::Matrix<3, 3> = Rotation3Impl::::hat(omega); - let mat_omega_sq = mat_omega.clone().mat_mul(mat_omega.clone()); + let mat_omega_sq = mat_omega.mat_mul(&mat_omega); let near_zero = theta_sq.less_equal(&S::from_f64(EPS_F64)); let mat_v_inv0 = S::Matrix::<3, 3>::identity() - mat_omega.scaled(S::from_f64(0.5)) + mat_omega_sq.scaled(S::from_f64(1. / 12.)); - let theta = theta_sq.clone().sqrt(); + let theta = theta_sq.sqrt(); let half_theta = S::from_f64(0.5) * theta.clone(); let mat_v_inv = S::Matrix::<3, 3>::identity() - mat_omega.scaled(S::from_f64(0.5)) @@ -591,7 +590,7 @@ impl, const BATCH: usize> IsRealLieFactorGroupImpl = Rotation3Impl::::hat(omega); - let mat_omega_sq = mat_omega.clone().mat_mul(mat_omega.clone()); + let mat_omega_sq = mat_omega.mat_mul(&mat_omega); let omega_x = omega.get_elem(0); let omega_y = omega.get_elem(1); @@ -693,7 +692,7 @@ impl, const BATCH: usize> IsRealLieFactorGroupImpl = Rotation3Impl::::hat(omega); - let mat_omega_sq = mat_omega.clone().mat_mul(mat_omega); + let mat_omega_sq = mat_omega.mat_mul(&mat_omega); let dt_mat_omega_pos_idx = [[2, 1], [0, 2], [1, 0]]; let dt_mat_omega_neg_idx = [[1, 2], [2, 0], [0, 1]]; @@ -763,18 +762,42 @@ pub type Rotation3F64 = Rotation3; impl, const BATCH: usize> Rotation3 { /// Rotation around the x-axis. - pub fn rot_x(theta: S) -> Self { - Rotation3::exp(&S::Vector::<3>::from_array([theta, S::zero(), S::zero()])) + pub fn rot_x(theta: U) -> Self + where + U: Borrow, + { + let theta: &S = theta.borrow(); + Rotation3::exp(S::Vector::<3>::from_array([ + theta.clone(), + S::zero(), + S::zero(), + ])) } /// Rotation around the y-axis. - pub fn rot_y(theta: S) -> Self { - Rotation3::exp(&S::Vector::<3>::from_array([S::zero(), theta, S::zero()])) + pub fn rot_y(theta: U) -> Self + where + U: Borrow, + { + let theta: &S = theta.borrow(); + Rotation3::exp(S::Vector::<3>::from_array([ + S::zero(), + theta.clone(), + S::zero(), + ])) } /// Rotation around the z-axis. - pub fn rot_z(theta: S) -> Self { - Rotation3::exp(&S::Vector::<3>::from_array([S::zero(), S::zero(), theta])) + pub fn rot_z(theta: U) -> Self + where + U: Borrow, + { + let theta: &S = theta.borrow(); + Rotation3::exp(S::Vector::<3>::from_array([ + S::zero(), + S::zero(), + theta.clone(), + ])) } } @@ -790,7 +813,11 @@ impl Rotation3 { /// From a 3x3 rotation matrix. The matrix must be a valid rotation matrix, /// i.e., it must be orthogonal with determinant 1, otherwise None is returned. - pub fn try_from_mat(mat_r: &S::SingleMatrix<3, 3>) -> Option> { + pub fn try_from_mat(mat_r: M) -> Option> + where + M: Borrow>, + { + let mat_r = mat_r.borrow(); if !Self::is_orthogonal_with_positive_det(&mat_r.single_real_matrix(), EPS_F64) { return None; } @@ -924,7 +951,7 @@ impl Rotation3 { #[test] fn rotation3_prop_tests() { use crate::factor_lie_group::RealFactorLieGroupTest; - use crate::real_lie_group::RealLieGroupTest; + use crate::lie_group::real_lie_group::RealLieGroupTest; use sophus_core::calculus::dual::dual_scalar::DualScalar; #[cfg(feature = "simd")] use sophus_core::calculus::dual::DualBatchScalar; @@ -979,7 +1006,7 @@ fn from_matrix_test() { let mat: MatF64<3, 3> = q.matrix(); info!("mat = {:?}", mat); - let q2: Rotation3 = Rotation3::try_from_mat(&mat).unwrap(); + let q2: Rotation3 = Rotation3::try_from_mat(mat).unwrap(); let mat2 = q2.matrix(); info!("mat2 = {:?}", mat2); @@ -988,9 +1015,9 @@ fn from_matrix_test() { // Iterate over all tangent too, just to get more examples. for t in Rotation3::::tangent_examples() { - let mat: MatF64<3, 3> = Rotation3::::exp(&t).matrix(); + let mat: MatF64<3, 3> = Rotation3::::exp(t).matrix(); info!("mat = {:?}", mat); - let t2: Rotation3 = Rotation3::try_from_mat(&mat).unwrap(); + let t2: Rotation3 = Rotation3::try_from_mat(mat).unwrap(); let mat2 = t2.matrix(); info!("mat2 = {:?}", mat2); assert_relative_eq!(mat, mat2, epsilon = EPS_F64); diff --git a/crates/sophus_lie/src/groups/translation_product_product.rs b/crates/sophus_lie/src/groups/translation_product_product.rs index 2d4f535d..48ec7a4c 100644 --- a/crates/sophus_lie/src/groups/translation_product_product.rs +++ b/crates/sophus_lie/src/groups/translation_product_product.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use crate::lie_group::LieGroup; use crate::prelude::*; use crate::traits::HasDisambiguate; @@ -116,8 +118,11 @@ impl< > ParamsImpl for TranslationProductGroupImpl { - fn are_params_valid(params: &S::Vector) -> S::Mask { - F::are_params_valid(&Self::factor_params(params)) + fn are_params_valid

(params: P) -> S::Mask + where + P: Borrow>, + { + F::are_params_valid(Self::factor_params(params.borrow())) } fn params_examples() -> alloc::vec::Vec> { @@ -379,7 +384,7 @@ impl< ) } - fn dx_exp_x_times_point_at_0(point: S::Vector) -> S::Matrix { + fn dx_exp_x_times_point_at_0(point: &S::Vector) -> S::Matrix { S::Matrix::block_mat1x2( S::Matrix::::identity(), Factor::dx_exp_x_times_point_at_0(point), @@ -523,27 +528,40 @@ impl< FactorImpl, >; - fn from_translation_and_factor( - translation: &>::Vector, - factor: &LieGroup, - ) -> Self { - let params = Self::Impl::params_from(translation, factor.params()); + fn from_translation_and_factor(translation: P, factor: F) -> Self + where + P: Borrow>, + F: Borrow>, + { + let params = Self::Impl::params_from(translation.borrow(), factor.borrow().params()); Self::from_params(¶ms) } - fn set_translation(&mut self, translation: &>::Vector) { - self.set_params(&Self::G::params_from(translation, self.factor().params())) + fn set_translation

(&mut self, translation: P) + where + P: Borrow<>::Vector>, + { + self.set_params(Self::G::params_from( + translation.borrow(), + self.factor().params(), + )) } fn translation(&self) -> >::Vector { Self::Impl::translation(self.params()) } - fn set_factor(&mut self, factor: &LieGroup) { - self.set_params(&Self::G::params_from(&self.translation(), factor.params())) + fn set_factor(&mut self, factor: F) + where + F: Borrow>, + { + self.set_params(Self::G::params_from( + &self.translation(), + factor.borrow().params(), + )) } fn factor(&self) -> LieGroup { - LieGroup::from_params(&Self::Impl::factor_params(self.params())) + LieGroup::from_params(Self::Impl::factor_params(self.params())) } } diff --git a/crates/sophus_lie/src/lib.rs b/crates/sophus_lie/src/lib.rs index 33b835fe..35e2a60e 100644 --- a/crates/sophus_lie/src/lib.rs +++ b/crates/sophus_lie/src/lib.rs @@ -22,18 +22,9 @@ pub use crate::lie_group::LieGroup; /// Lie groups pub mod factor_lie_group; -/// Lie group as a manifold -pub mod lie_group_manifold; - /// Lie group traits pub mod traits; -/// Real lie group -pub mod real_lie_group; - -/// Lie group average -pub mod average; - /// sophus_lie prelude pub mod prelude { pub use crate::traits::IsLieGroup; diff --git a/crates/sophus_lie/src/lie_group.rs b/crates/sophus_lie/src/lie_group.rs index ddb8de99..d35feac8 100644 --- a/crates/sophus_lie/src/lie_group.rs +++ b/crates/sophus_lie/src/lie_group.rs @@ -1,12 +1,22 @@ use super::traits::IsLieGroupImpl; 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; extern crate alloc; +/// Lie group average +pub mod average; +/// Group multiplication +pub mod group_mul; +/// Lie group as a manifold +pub mod lie_group_manifold; +/// Real lie group +pub mod real_lie_group; + /// Lie group #[derive(Debug, Copy, Clone, Default)] pub struct LieGroup< @@ -33,7 +43,10 @@ impl< > ParamsImpl for LieGroup { - fn are_params_valid(params: &>::Vector) -> S::Mask { + fn are_params_valid

(params: P) -> S::Mask + where + P: for<'a> Borrow>, + { G::are_params_valid(params) } @@ -56,7 +69,12 @@ impl< G: IsLieGroupImpl, > HasParams for LieGroup { - fn from_params(params: &S::Vector) -> Self { + fn from_params

(params: P) -> Self + where + P: for<'a> Borrow>, + { + let params = params.borrow(); + assert!( G::are_params_valid(params).all(), "Invalid parameters for {:?}", @@ -68,7 +86,11 @@ impl< } } - fn set_params(&mut self, params: &S::Vector) { + fn set_params

(&mut self, params: P) + where + P: for<'a> Borrow>, + { + let params = params.borrow(); self.params = G::disambiguate(params.clone()); } @@ -140,8 +162,11 @@ impl< } /// exponential map - pub fn exp(omega: &S::Vector) -> Self { - Self::from_params(&G::exp(omega)) + pub fn exp(omega: T) -> Self + where + T: Borrow>, + { + Self::from_params(G::exp(omega.borrow())) } /// Interpolate between "(w-1) * self" and "w * other". @@ -149,7 +174,7 @@ impl< /// w is typically in [0, 1]. If w=0, self is returned. If w=1 other is returned. /// pub fn interpolate(&self, other: &Self, w: S) -> Self { - self.group_mul(&Self::exp(&self.inverse().group_mul(other).log().scaled(w))) + self * &Self::exp((self.inverse() * other).log().scaled(w)) } /// logarithmic map @@ -158,38 +183,50 @@ impl< } /// hat operator: hat: R^d -> R^{a x a} - pub fn hat(omega: &S::Vector) -> S::Matrix { - G::hat(omega) + pub fn hat(omega: T) -> S::Matrix + where + T: Borrow>, + { + G::hat(omega.borrow()) } /// vee operator: vee: R^{a x a} -> R^d - pub fn vee(xi: &S::Matrix) -> S::Vector { - G::vee(xi) + pub fn vee(xi: M) -> S::Vector + where + M: Borrow>, + { + G::vee(xi.borrow()) } /// identity element pub fn identity() -> Self { - Self::from_params(&G::identity_params()) + Self::from_params(G::identity_params()) } /// group multiplication pub fn group_mul(&self, other: &Self) -> Self { - Self::from_params(&G::group_mul(&self.params, &other.params)) + Self::from_params(G::group_mul(&self.params, &other.params)) } /// group inverse pub fn inverse(&self) -> Self { - Self::from_params(&G::inverse(&self.params)) + Self::from_params(G::inverse(&self.params)) } /// transform a point - pub fn transform(&self, point: &S::Vector) -> S::Vector { - G::transform(&self.params, point) + pub fn transform(&self, point: T) -> S::Vector + where + T: Borrow>, + { + G::transform(&self.params, point.borrow()) } /// convert point to ambient space - pub fn to_ambient(point: &S::Vector) -> S::Vector { - G::to_ambient(point) + pub fn to_ambient

(point: P) -> S::Vector + where + P: Borrow>, + { + G::to_ambient(point.borrow()) } /// return compact matrix representation @@ -203,8 +240,11 @@ impl< } /// algebra adjoint - pub fn ad(tangent: &S::Vector) -> S::Matrix { - G::ad(tangent) + pub fn ad(tangent: T) -> S::Matrix + where + T: Borrow>, + { + G::ad(tangent.borrow()) } /// group element examples @@ -232,7 +272,7 @@ impl< let mut num = 0; for g in &Self::element_examples() { let o = S::Vector::::zeros(); - let o_transformed = g.transform(&o); + let o_transformed = g.transform(o); let mask = (o_transformed.real_vector()) .norm() .less_equal(&S::RealScalar::from_f64(0.0001)); @@ -257,7 +297,7 @@ impl< let mat_adj_x = mat_adj.clone() * x.clone(); let inv_mat: S::Matrix = g.inverse().matrix(); - let mat_adj_x2 = Self::vee(&mat.mat_mul(Self::hat(x).mat_mul(inv_mat))); + let mat_adj_x2 = Self::vee(mat.mat_mul(Self::hat(x).mat_mul(inv_mat))); assert_relative_eq!( mat_adj_x.real_vector(), mat_adj_x2.real_vector(), @@ -288,7 +328,7 @@ impl< for g in &group_examples { let matrix_before = *g.compact().real_matrix(); - let matrix_after = *Self::exp(&g.log()).compact().real_matrix(); + let matrix_after = *Self::exp(g.log()).compact().real_matrix(); assert_relative_eq!(matrix_before, matrix_after, epsilon = 0.0001); @@ -315,7 +355,7 @@ impl< for omega in &tangent_examples { assert_relative_eq!( omega.real_vector(), - Self::vee(&Self::hat(omega)).real_vector(), + Self::vee(Self::hat(omega)).real_vector(), epsilon = 0.0001 ); } @@ -327,8 +367,8 @@ impl< for g1 in &group_examples { for g2 in &group_examples { for g3 in &group_examples { - let left_hugging = (g1.group_mul(g2)).group_mul(g3); - let right_hugging = g1.group_mul(&g2.group_mul(g3)); + let left_hugging = (g1 * g2) * g3; + let right_hugging = g1 * (g2 * g3); assert_relative_eq!( left_hugging.compact(), right_hugging.compact(), @@ -339,8 +379,8 @@ impl< } for g1 in &group_examples { for g2 in &group_examples { - let daz_from_foo_transform_1 = g2.inverse().group_mul(&g1.inverse()); - let daz_from_foo_transform_2 = g1.group_mul(g2).inverse(); + let daz_from_foo_transform_1 = g2.inverse() * g1.inverse(); + let daz_from_foo_transform_2 = (g1 * g2).inverse(); assert_relative_eq!( daz_from_foo_transform_1.compact(), daz_from_foo_transform_2.compact(), diff --git a/crates/sophus_lie/src/average.rs b/crates/sophus_lie/src/lie_group/average.rs similarity index 87% rename from crates/sophus_lie/src/average.rs rename to crates/sophus_lie/src/lie_group/average.rs index 0ae441f9..380629df 100644 --- a/crates/sophus_lie/src/average.rs +++ b/crates/sophus_lie/src/lie_group/average.rs @@ -53,18 +53,13 @@ pub fn iterative_average< for parent_from_body in parent_from_body_transforms { average_tangent = average_tangent - + parent_from_body_average - .inverse() - .group_mul(parent_from_body) + + (parent_from_body_average.inverse() * parent_from_body) .log() .scaled(w.clone()); } let refined_parent_from_body_average = - parent_from_body_average.group_mul(&LieGroup::exp(&average_tangent)); - let step = refined_parent_from_body_average - .inverse() - .group_mul(&parent_from_body_average) - .log(); + parent_from_body_average.clone() * LieGroup::exp(&average_tangent); + let step = (refined_parent_from_body_average.inverse() * parent_from_body_average).log(); if step.squared_norm() < S::from_f64(EPS_F64) { return Ok(refined_parent_from_body_average); } @@ -109,11 +104,7 @@ macro_rules! def_average_test_template { for parent_from_b in Self::element_examples() { // test: average of two elements is identical to interpolation - if parent_from_a - .inverse() - .group_mul(&parent_from_b) - .has_shortest_path_ambiguity() - { + if (parent_from_a.inverse() * parent_from_b).has_shortest_path_ambiguity() { continue; } let averaged_element = @@ -135,9 +126,7 @@ macro_rules! def_average_test_template { let w = 1.0 / (list.len() as f64); for parent_from_x in list { - average_tangent += parent_from_average - .inverse() - .group_mul(&parent_from_x) + average_tangent += (parent_from_average.inverse() * parent_from_x) .log() .scaled(w); } diff --git a/crates/sophus_lie/src/lie_group/group_mul.rs b/crates/sophus_lie/src/lie_group/group_mul.rs new file mode 100644 index 00000000..dc0e3e7f --- /dev/null +++ b/crates/sophus_lie/src/lie_group/group_mul.rs @@ -0,0 +1,80 @@ +use crate::lie_group::LieGroup; +use crate::traits::IsLieGroupImpl; +use core::ops::Mul; +use sophus_core::prelude::IsScalar; + +// a * b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul> + for LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: Self) -> Self::Output { + self.group_mul(&rhs) + } +} + +// a * &b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul<&LieGroup> + for LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: &Self) -> Self::Output { + self.group_mul(rhs) + } +} + +// &a * &b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul> + for &LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: LieGroup) -> Self::Output { + self.group_mul(&rhs) + } +} + +// a * &b +impl< + S: IsScalar, + const DOF: usize, + const PARAMS: usize, + const POINT: usize, + const AMBIENT: usize, + const BATCH_SIZE: usize, + G: IsLieGroupImpl, + > Mul<&LieGroup> + for &LieGroup +{ + type Output = LieGroup; + + fn mul(self, rhs: &LieGroup) -> Self::Output { + self.group_mul(rhs) + } +} diff --git a/crates/sophus_lie/src/lie_group_manifold.rs b/crates/sophus_lie/src/lie_group/lie_group_manifold.rs similarity index 80% rename from crates/sophus_lie/src/lie_group_manifold.rs rename to crates/sophus_lie/src/lie_group/lie_group_manifold.rs index 2e32eff4..9eb0f622 100644 --- a/crates/sophus_lie/src/lie_group_manifold.rs +++ b/crates/sophus_lie/src/lie_group/lie_group_manifold.rs @@ -1,6 +1,7 @@ use crate::lie_group::LieGroup; use crate::prelude::*; use crate::traits::IsLieGroupImpl; +use core::borrow::Borrow; use core::fmt::Debug; use sophus_core::params::ParamsImpl; @@ -34,7 +35,10 @@ impl< > ParamsImpl for LeftGroupManifold { - fn are_params_valid(params: &>::Vector) -> S::Mask { + fn are_params_valid

(params: P) -> S::Mask + where + P: for<'a> Borrow<>::Vector>, + { G::are_params_valid(params) } @@ -58,13 +62,19 @@ impl< > HasParams for LeftGroupManifold { - fn from_params(params: &>::Vector) -> Self { + fn from_params

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

(&mut self, params: P) + where + P: for<'a> Borrow<>::Vector>, + { self.group.set_params(params) } @@ -86,12 +96,12 @@ impl< { fn oplus(&self, tangent: &>::Vector) -> Self { Self { - group: LieGroup::::exp(tangent) - .group_mul(&self.group), + group: &LieGroup::::exp(tangent) + * &self.group, } } fn ominus(&self, rhs: &Self) -> >::Vector { - self.group.inverse().group_mul(&rhs.group).log() + (&self.group.inverse() * &rhs.group).log() } } diff --git a/crates/sophus_lie/src/real_lie_group.rs b/crates/sophus_lie/src/lie_group/real_lie_group.rs similarity index 94% rename from crates/sophus_lie/src/real_lie_group.rs rename to crates/sophus_lie/src/lie_group/real_lie_group.rs index 9eb9a194..fbb0dba5 100644 --- a/crates/sophus_lie/src/real_lie_group.rs +++ b/crates/sophus_lie/src/lie_group/real_lie_group.rs @@ -1,9 +1,9 @@ -use super::traits::IsLieGroupImpl; -use super::traits::IsRealLieGroupImpl; use crate::groups::isometry2::Isometry2F64; use crate::groups::isometry3::Isometry3F64; use crate::lie_group::LieGroup; use crate::prelude::*; +use crate::traits::IsLieGroupImpl; +use crate::traits::IsRealLieGroupImpl; use crate::Isometry2; use crate::Isometry3; use crate::Rotation2; @@ -22,6 +22,7 @@ use sophus_core::calculus::dual::dual_batch_scalar::DualBatchScalar; #[cfg(feature = "simd")] use sophus_core::linalg::BatchScalarF64; +use core::borrow::Borrow; use core::fmt::Display; use core::fmt::Formatter; @@ -43,8 +44,11 @@ where } /// derivative of exponential map times point at the identity - pub fn dx_exp_x_times_point_at_0(point: S::Vector) -> S::Matrix { - G::dx_exp_x_times_point_at_0(point) + pub fn dx_exp_x_times_point_at_0

(point: P) -> S::Matrix + where + P: Borrow>, + { + G::dx_exp_x_times_point_at_0(point.borrow()) } /// are there multiple shortest paths to the identity? @@ -53,38 +57,62 @@ where } /// derivative of exponential map - pub fn dx_exp(tangent: &S::Vector) -> S::Matrix { - G::dx_exp(tangent) + pub fn dx_exp(tangent: T) -> S::Matrix + where + T: Borrow>, + { + G::dx_exp(tangent.borrow()) } /// derivative of logarithmic map - pub fn dx_log_x(params: &S::Vector) -> S::Matrix { - G::dx_log_x(params) + pub fn dx_log_x

(params: P) -> S::Matrix + where + P: Borrow>, + { + G::dx_log_x(params.borrow()) } /// dual representation of the group pub fn to_dual_c( self, ) -> LieGroup { - LieGroup::from_params(&self.params.to_dual()) + LieGroup::from_params(self.params.to_dual()) } /// derivative of log(exp(x)) at the identity - pub fn dx_log_a_exp_x_b_at_0(a: &Self, b: &Self) -> S::Matrix { - let ab = a.group_mul(b); + pub fn dx_log_a_exp_x_b_at_0(a: A, b: B) -> S::Matrix + where + A: Borrow, + B: Borrow, + { + let a = a.borrow(); + let b = b.borrow(); + let ab = a * b; Self::dx_log_x(ab.params()) - .mat_mul(Self::da_a_mul_b(&Self::identity(), &ab)) + .mat_mul(Self::da_a_mul_b(Self::identity(), ab)) .mat_mul(Self::dx_exp_x_at_0()) .mat_mul(Self::adj(a)) } /// derivative of group multiplication with respect to the first argument - pub fn da_a_mul_b(a: &Self, b: &Self) -> S::Matrix { + pub fn da_a_mul_b(a: A, b: B) -> S::Matrix + where + A: Borrow, + B: Borrow, + { + let a = a.borrow(); + let b = b.borrow(); G::da_a_mul_b(a.params(), b.params()) } /// derivative of group multiplication with respect to the second argument - pub fn db_a_mul_b(a: &Self, b: &Self) -> S::Matrix { + pub fn db_a_mul_b(a: A, b: B) -> S::Matrix + where + A: Borrow, + B: Borrow, + { + let a = a.borrow(); + let b = b.borrow(); G::db_a_mul_b(a.params(), b.params()) } } @@ -284,7 +312,7 @@ macro_rules! def_real_group_test_template { let exp_t = |t: <$scalar as IsScalar<$batch>>::Vector| -> <$scalar as IsScalar<$batch>>::Vector { - Self::exp(&t).transform(&point) + Self::exp(&t).transform(point) }; let dual_exp_t = |vv: <$dual_scalar as IsScalar<$batch>>::Vector| -> <$dual_scalar as IsScalar<$batch>>::Vector @@ -322,7 +350,7 @@ macro_rules! def_real_group_test_template { let log_x = |t: <$scalar as IsScalar<$batch>>::Vector| -> <$scalar as IsScalar<$batch>>::Vector { - Self::exp(&t).group_mul(&g).log() + (Self::exp(&t) * g).log() }; let o = <$scalar as IsScalar<$batch>>::Vector::zeros(); @@ -381,7 +409,7 @@ macro_rules! def_real_group_test_template { let dual_log_x = |t: <$dual_scalar as IsScalar<$batch>>::Vector| -> <$dual_scalar as IsScalar<$batch>>::Vector { - dual_a.group_mul( + (&dual_a * &<$dual_group>::exp(&t) .group_mul(&dual_b) ).log() diff --git a/crates/sophus_lie/src/traits.rs b/crates/sophus_lie/src/traits.rs index 28c70e44..41c4ece5 100644 --- a/crates/sophus_lie/src/traits.rs +++ b/crates/sophus_lie/src/traits.rs @@ -1,4 +1,5 @@ use crate::prelude::*; +use core::borrow::Borrow; use core::fmt::Debug; use sophus_core::manifold::traits::TangentImpl; use sophus_core::params::ParamsImpl; @@ -141,7 +142,7 @@ pub trait IsRealLieGroupImpl< fn dx_log_x(params: &S::Vector) -> S::Matrix; /// derivative of exponential map times a point at the identity - fn dx_exp_x_times_point_at_0(point: S::Vector) -> S::Matrix; + fn dx_exp_x_times_point_at_0(point: &S::Vector) -> S::Matrix; /// are there multiple shortest paths to the identity? fn has_shortest_path_ambiguity(params: &S::Vector) -> S::Mask; @@ -272,15 +273,22 @@ pub trait IsTranslationProductGroup< type Impl; /// Create from translation and factor group element - fn from_translation_and_factor(translation: &S::Vector, factor: &FactorGroup) -> Self; + fn from_translation_and_factor(translation: P, factor: F) -> Self + where + P: Borrow>, + F: Borrow; /// set translation - fn set_translation(&mut self, translation: &S::Vector); + fn set_translation

(&mut self, translation: P) + where + P: Borrow>; /// get translation fn translation(&self) -> S::Vector; /// set factor group element - fn set_factor(&mut self, factor: &FactorGroup); + fn set_factor(&mut self, factor: F) + where + F: Borrow; /// get factor group element fn factor(&self) -> FactorGroup; diff --git a/crates/sophus_opt/src/example_problems/cam_calib.rs b/crates/sophus_opt/src/example_problems/cam_calib.rs index 2f5ede38..2a13fde3 100644 --- a/crates/sophus_opt/src/example_problems/cam_calib.rs +++ b/crates/sophus_opt/src/example_problems/cam_calib.rs @@ -47,12 +47,12 @@ impl CamCalibProblem { let true_world_from_cameras = alloc::vec![ Isometry3::identity(), Isometry3::from_translation_and_factor( - &VecF64::<3>::new(0.0, 1.0, 0.0), - &Rotation3::identity(), + VecF64::<3>::new(0.0, 1.0, 0.0), + Rotation3::identity(), ), Isometry3::from_translation_and_factor( - &VecF64::<3>::new(0.0, 2.0, 0.0), - &Rotation3::identity(), + VecF64::<3>::new(0.0, 2.0, 0.0), + Rotation3::identity(), ), ]; use rand::prelude::*; @@ -64,7 +64,7 @@ impl CamCalibProblem { }; let true_intrinsics = PinholeCamera::::from_params_and_size( - &VecF64::<4>::new(600.0, 600.0, 320.0, 240.0), + VecF64::<4>::new(600.0, 600.0, 320.0, 240.0), image_size, ); @@ -81,16 +81,15 @@ impl CamCalibProblem { entity_indices: [0, 0, i], }); - let true_point_in_cam0 = true_intrinsics.cam_unproj_with_z(&true_uv_in_img0, 10.0); - true_points_in_world.push(true_world_from_cameras[0].transform(&true_point_in_cam0)); - let true_uv_in_img0_proof = true_intrinsics.cam_proj(&true_point_in_cam0); + let true_point_in_cam0 = true_intrinsics.cam_unproj_with_z(true_uv_in_img0, 10.0); + true_points_in_world.push(true_world_from_cameras[0].transform(true_point_in_cam0)); + let true_uv_in_img0_proof = true_intrinsics.cam_proj(true_point_in_cam0); approx::assert_abs_diff_eq!(true_uv_in_img0, true_uv_in_img0_proof, epsilon = 0.1); - let true_cam1_from_cam0 = true_world_from_cameras[1] - .inverse() - .group_mul(&true_world_from_cameras[0]); - let true_point_in_cam1 = true_cam1_from_cam0.transform(&true_point_in_cam0); - let true_uv_in_img1 = true_intrinsics.cam_proj(&true_point_in_cam1); + let true_cam1_from_cam0 = + true_world_from_cameras[1].inverse() * true_world_from_cameras[0]; + let true_point_in_cam1 = true_cam1_from_cam0.transform(true_point_in_cam0); + let true_uv_in_img1 = true_intrinsics.cam_proj(true_point_in_cam1); let img_noise = VecF64::<2>::new(rng.gen::() - 0.5, rng.gen::() - 0.5); if spurious_matches && i == 0 { @@ -107,11 +106,10 @@ impl CamCalibProblem { }); } - let true_cam2_from_cam0 = true_world_from_cameras[2] - .inverse() - .group_mul(&true_world_from_cameras[0]); - let true_point_in_cam2 = true_cam2_from_cam0.transform(&true_point_in_cam0); - let true_uv_in_img2 = true_intrinsics.cam_proj(&true_point_in_cam2); + let true_cam2_from_cam0 = + true_world_from_cameras[2].inverse() * true_world_from_cameras[0]; + let true_point_in_cam2 = true_cam2_from_cam0.transform(true_point_in_cam0); + let true_uv_in_img2 = true_intrinsics.cam_proj(true_point_in_cam2); let img_noise = VecF64::<2>::new(rng.gen::() - 0.5, rng.gen::() - 0.5); observations.push(ReprojTermSignature { uv_in_image: true_uv_in_img2 + img_noise, 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 0c1bd428..d4c4c982 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 @@ -43,7 +43,7 @@ fn res_fn>( isometry: Isometry2, isometry_prior_mean: Isometry2, ) -> Scalar::Vector<3> { - Isometry2::::group_mul(&isometry, &isometry_prior_mean.inverse()).log() + (isometry * isometry_prior_mean.inverse()).log() } impl IsResidualFn<3, 1, (), Isometry2F64, Isometry2F64> for Isometry2PriorCostFn { @@ -60,12 +60,10 @@ impl IsResidualFn<3, 1, (), Isometry2F64, Isometry2F64> for Isometry2PriorCostFn let residual = res_fn(isometry, *isometry_prior_mean); let dx_res_fn = |x: DualVector<3>| -> DualVector<3> { - let pp = Isometry2::::exp(&x).group_mul(&isometry.to_dual_c()); + let pp = Isometry2::::exp(&x) * isometry.to_dual_c(); res_fn( pp, - Isometry2::from_params(&DualVector::from_real_vector( - *isometry_prior_mean.params(), - )), + Isometry2::from_params(DualVector::from_real_vector(*isometry_prior_mean.params())), ) }; 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 29b1ddf6..0650039d 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 @@ -44,7 +44,7 @@ fn res_fn + IsSingleScalar>( isometry: Isometry3, isometry_prior_mean: Isometry3, ) -> Scalar::Vector<6> { - Isometry3::::group_mul(&isometry, &isometry_prior_mean.inverse()).log() + (isometry * isometry_prior_mean.inverse()).log() } impl IsResidualFn<6, 1, (), Isometry3F64, (Isometry3F64, MatF64<6, 6>)> for Isometry3PriorCostFn { @@ -61,10 +61,10 @@ impl IsResidualFn<6, 1, (), Isometry3F64, (Isometry3F64, MatF64<6, 6>)> for Isom let residual = res_fn(isometry, isometry_prior.0); let dx_res_fn = |x: DualVector<6>| -> DualVector<6> { - let pp = Isometry3::::exp(&x).group_mul(&isometry.to_dual_c()); + let pp = Isometry3::::exp(x) * isometry.to_dual_c(); res_fn( pp, - Isometry3::from_params(&DualVector::from_real_vector(*isometry_prior.0.params())), + Isometry3::from_params(DualVector::from_real_vector(isometry_prior.0.params())), ) }; diff --git a/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs b/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs index f3b2a46e..d14752ac 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/pose_graph.rs @@ -14,10 +14,7 @@ pub fn res_fn( world_from_pose_b: Isometry2, pose_a_from_pose_b: Isometry2, ) -> S::Vector<3> { - (world_from_pose_a - .inverse() - .group_mul(&world_from_pose_b.group_mul(&pose_a_from_pose_b.inverse()))) - .log() + (world_from_pose_a.inverse() * world_from_pose_b * pose_a_from_pose_b.inverse()).log() } /// Cost function for 2d pose graph @@ -42,14 +39,14 @@ impl IsResidualFn<12, 2, (), (Isometry2F64, Isometry2F64), Isometry2F64> for Pos ( || { -Isometry2::dx_log_a_exp_x_b_at_0( - &world_from_pose_a.inverse(), - &world_from_pose_b.group_mul(&obs.inverse()), + world_from_pose_a.inverse(), + world_from_pose_b * obs.inverse(), ) }, || { Isometry2::dx_log_a_exp_x_b_at_0( - &world_from_pose_a.inverse(), - &world_from_pose_b.group_mul(&obs.inverse()), + world_from_pose_a.inverse(), + world_from_pose_b * obs.inverse(), ) }, ) 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 7a1eef64..3e64bd65 100644 --- a/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs +++ b/crates/sophus_opt/src/example_problems/cost_fn/reprojection.rs @@ -87,10 +87,10 @@ impl IsResidualFn<13, 3, (), (PinholeCamera, Isometry3F64, VecF64<3>), V let d1_res_fn = |x: DualVector<6>| -> DualVector<2> { res_fn( PinholeCamera::::from_params_and_size( - &DualVector::from_real_vector(*intrinsics.params()), + DualVector::from_real_vector(*intrinsics.params()), intrinsics.image_size(), ), - Isometry3::::exp(&x).group_mul(&world_from_camera_pose.to_dual_c()), + Isometry3::::exp(&x) * world_from_camera_pose.to_dual_c(), DualVector::from_real_vector(point_in_world), DualVector::from_real_vector(*uv_in_image), ) @@ -99,7 +99,7 @@ impl IsResidualFn<13, 3, (), (PinholeCamera, Isometry3F64, VecF64<3>), V let d2_res_fn = |x: DualVector<3>| -> DualVector<2> { res_fn( PinholeCamera::::from_params_and_size( - &DualVector::from_real_vector(*intrinsics.params()), + DualVector::from_real_vector(*intrinsics.params()), intrinsics.image_size(), ), world_from_camera_pose.to_dual_c(), diff --git a/crates/sophus_opt/src/example_problems/pose_circle.rs b/crates/sophus_opt/src/example_problems/pose_circle.rs index 5496e6e8..0fa95aac 100644 --- a/crates/sophus_opt/src/example_problems/pose_circle.rs +++ b/crates/sophus_opt/src/example_problems/pose_circle.rs @@ -51,7 +51,7 @@ impl PoseCircleProblem { let x = radius * angle.cos(); let y = radius * angle.sin(); let p = VecF64::<3>::from_real_array([x, y, 0.1 * angle]); - true_world_from_robot_poses.push(Isometry2::exp(&p)); + true_world_from_robot_poses.push(Isometry2::exp(p)); } for i in 0..len { @@ -61,11 +61,8 @@ impl PoseCircleProblem { let true_world_from_pose_b = true_world_from_robot_poses[b_idx]; let p = VecF64::<3>::from_real_array([0.001, 0.001, 0.0001]); - let pose_a_from_pose_b = Isometry2::exp(&p).group_mul( - &true_world_from_pose_a - .inverse() - .group_mul(&true_world_from_pose_b), - ); + let pose_a_from_pose_b = + Isometry2::exp(p) * true_world_from_pose_a.inverse() * true_world_from_pose_b; obs_pose_a_from_pose_b_poses .terms @@ -87,8 +84,7 @@ impl PoseCircleProblem { let world_from_pose_a = est_world_from_robot_poses[a_idx]; let pose_a_from_pose_b = obs.pose_a_from_pose_b; let p = VecF64::<3>::from_real_array([0.1, 0.1, 0.1]); - let world_from_pose_b = - Isometry2::exp(&p).group_mul(&world_from_pose_a.group_mul(&pose_a_from_pose_b)); + let world_from_pose_b = Isometry2::exp(p) * world_from_pose_a * pose_a_from_pose_b; est_world_from_robot_poses.push(world_from_pose_b); } @@ -114,7 +110,7 @@ impl PoseCircleProblem { est_world_from_robot[obs.entity_indices[1]], obs.pose_a_from_pose_b, ); - res_err += residual.dot(residual); + res_err += residual.dot(&residual); } res_err /= self.obs_pose_a_from_pose_b_poses.terms.len() as f64; res_err diff --git a/crates/sophus_opt/src/example_problems/simple_prior.rs b/crates/sophus_opt/src/example_problems/simple_prior.rs index 133d8c17..d1bb528d 100644 --- a/crates/sophus_opt/src/example_problems/simple_prior.rs +++ b/crates/sophus_opt/src/example_problems/simple_prior.rs @@ -36,7 +36,7 @@ impl Default for SimpleIso2PriorProblem { impl SimpleIso2PriorProblem { fn new() -> Self { let p = VecF64::<3>::from_f64_array([0.2, 0.0, 1.0]); - let true_world_from_robot = Isometry2::::exp(&p); + let true_world_from_robot = Isometry2::::exp(p); Self { true_world_from_robot, est_world_from_robot: Isometry2::::identity(), @@ -108,7 +108,7 @@ impl Default for SimpleIso3PriorProblem { impl SimpleIso3PriorProblem { fn new() -> Self { let p = VecF64::<6>::from_real_array([0.2, 0.0, 1.0, 0.2, 0.0, 1.0]); - let true_world_from_robot = Isometry3::::exp(&p); + let true_world_from_robot = Isometry3::::exp(p); Self { true_world_from_robot, est_world_from_robot: Isometry3::::identity(), diff --git a/crates/sophus_opt/src/variables.rs b/crates/sophus_opt/src/variables.rs index 89c62136..4e41c673 100644 --- a/crates/sophus_opt/src/variables.rs +++ b/crates/sophus_opt/src/variables.rs @@ -38,7 +38,7 @@ impl IsVariable for KannalaBrandtCamera { fn update(&mut self, delta: nalgebra::DVectorView) { let new_params = *self.params() + delta; - self.set_params(&new_params); + self.set_params(new_params); } } @@ -47,7 +47,7 @@ impl IsVariable for PinholeCamera { fn update(&mut self, delta: nalgebra::DVectorView) { let new_params = *self.params() + delta; - self.set_params(&new_params); + self.set_params(new_params); } } @@ -56,7 +56,7 @@ impl IsVariable for BrownConradyCamera { fn update(&mut self, delta: nalgebra::DVectorView) { let new_params = *self.params() + delta; - self.set_params(&new_params); + self.set_params(new_params); } } @@ -65,7 +65,7 @@ impl IsVariable for UnifiedCameraF64 { fn update(&mut self, delta: nalgebra::DVectorView) { let new_params = *self.params() + delta; - self.set_params(&new_params); + self.set_params(new_params); } } @@ -421,10 +421,7 @@ impl IsVariable for Isometry2F64 { for d in 0..::DOF { delta_vec[d] = delta[d]; } - self.set_params( - (Isometry2::::group_mul(&Isometry2::::exp(&delta_vec), &self.clone())) - .params(), - ); + self.set_params((Isometry2::::exp(delta_vec) * *self).params()); } } @@ -436,10 +433,7 @@ impl IsVariable for Isometry3F64 { for d in 0..::DOF { delta_vec[d] = delta[d]; } - self.set_params( - (Isometry3::::group_mul(&Isometry3::::exp(&delta_vec), &self.clone())) - .params(), - ); + self.set_params((Isometry3::::exp(delta_vec) * *self).params()); } } diff --git a/crates/sophus_renderer/src/camera.rs b/crates/sophus_renderer/src/camera.rs index 5173002a..376073be 100644 --- a/crates/sophus_renderer/src/camera.rs +++ b/crates/sophus_renderer/src/camera.rs @@ -32,7 +32,7 @@ impl RenderCamera { pub fn default_from(image_size: ImageSize) -> RenderCamera { RenderCamera { properties: RenderCameraProperties::default_from(image_size), - scene_from_camera: Isometry3::from_translation(&VecF64::<3>::new(0.0, 0.0, -5.0)), + scene_from_camera: Isometry3::from_translation(VecF64::<3>::new(0.0, 0.0, -5.0)), } } } diff --git a/crates/sophus_renderer/src/camera/intrinsics.rs b/crates/sophus_renderer/src/camera/intrinsics.rs index b44218f6..0e8a3657 100644 --- a/crates/sophus_renderer/src/camera/intrinsics.rs +++ b/crates/sophus_renderer/src/camera/intrinsics.rs @@ -49,7 +49,7 @@ impl RenderIntrinsics { match self { RenderIntrinsics::Pinhole(camera) => *camera, RenderIntrinsics::UnifiedExtended(camera) => PinholeCameraF64::new( - &VecF64::<4>::from_array([ + VecF64::<4>::from_array([ 0.5 * camera.params()[0], 0.5 * camera.params()[1], camera.params()[2], diff --git a/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs b/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs index 1b92c9fa..ac1f56de 100644 --- a/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs +++ b/crates/sophus_renderer/src/renderables/scene_renderable/axes.rs @@ -49,10 +49,10 @@ impl Axes3Builder { let mut lines = vec![]; for world_from_local in self.world_from_local_axes.iter() { - let zero_in_world = world_from_local.transform(&zero_in_local); - let axis_x_in_world = world_from_local.transform(&x_axis_local); - let axis_y_in_world = world_from_local.transform(&y_axis_local); - let axis_z_in_world = world_from_local.transform(&z_axis_local); + let zero_in_world = world_from_local.transform(zero_in_local); + let axis_x_in_world = world_from_local.transform(x_axis_local); + let axis_y_in_world = world_from_local.transform(y_axis_local); + let axis_z_in_world = world_from_local.transform(z_axis_local); lines.push(LineSegment3 { p0: zero_in_world.cast(), diff --git a/crates/sophus_renderer/src/scene_renderer/line.rs b/crates/sophus_renderer/src/scene_renderer/line.rs index 23f5e680..13965297 100644 --- a/crates/sophus_renderer/src/scene_renderer/line.rs +++ b/crates/sophus_renderer/src/scene_renderer/line.rs @@ -103,7 +103,7 @@ impl SceneLineRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &line.world_from_entity, ); render_pass.set_vertex_buffer(0, line.vertex_buffer.slice(..)); diff --git a/crates/sophus_renderer/src/scene_renderer/mesh.rs b/crates/sophus_renderer/src/scene_renderer/mesh.rs index 3e4fcced..02e4178e 100644 --- a/crates/sophus_renderer/src/scene_renderer/mesh.rs +++ b/crates/sophus_renderer/src/scene_renderer/mesh.rs @@ -114,7 +114,7 @@ impl MeshRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &mesh.world_from_entity, ); render_pass.set_vertex_buffer(0, mesh.vertex_buffer.slice(..)); diff --git a/crates/sophus_renderer/src/scene_renderer/point.rs b/crates/sophus_renderer/src/scene_renderer/point.rs index d7d49a8c..b25f7818 100644 --- a/crates/sophus_renderer/src/scene_renderer/point.rs +++ b/crates/sophus_renderer/src/scene_renderer/point.rs @@ -89,7 +89,7 @@ impl ScenePointRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &point.world_from_entity, ); render_pass.set_vertex_buffer(0, point.vertex_buffer.slice(..)); diff --git a/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs b/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs index 990322f3..4fc8fc82 100644 --- a/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs +++ b/crates/sophus_renderer/src/scene_renderer/textured_mesh.rs @@ -201,7 +201,7 @@ impl TexturedMeshRenderer { .camera_from_entity_pose_buffer .update_given_camera_and_entity( &render_context.wgpu_queue, - &world_from_scene.group_mul(scene_from_camera), + &(world_from_scene * scene_from_camera), &mesh._scene_from_entity, ); render_pass.set_bind_group(1, &mesh._texture_bind_group, &[]); diff --git a/crates/sophus_renderer/src/uniform_buffers.rs b/crates/sophus_renderer/src/uniform_buffers.rs index 6673b071..2bf4b963 100644 --- a/crates/sophus_renderer/src/uniform_buffers.rs +++ b/crates/sophus_renderer/src/uniform_buffers.rs @@ -57,8 +57,7 @@ impl CameraFromEntityPoseUniform { scene_from_camera: &Isometry3F64, world_from_entity: &Isometry3F64, ) { - let camera_from_entity_mat4x4 = - (scene_from_camera.inverse().group_mul(world_from_entity)).matrix(); + let camera_from_entity_mat4x4 = (scene_from_camera.inverse() * world_from_entity).matrix(); let mut camera_from_entity_uniform: [[f32; 4]; 4] = [[0.0; 4]; 4]; for i in 0..4 { diff --git a/crates/sophus_sensor/src/camera.rs b/crates/sophus_sensor/src/camera.rs index 97dfdd8e..fb412006 100644 --- a/crates/sophus_sensor/src/camera.rs +++ b/crates/sophus_sensor/src/camera.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use super::traits::IsCameraDistortionImpl; use crate::prelude::*; use sophus_image::ImageSize; @@ -29,12 +31,19 @@ impl< > Camera { /// Creates a new camera - pub fn new(params: &S::Vector, image_size: ImageSize) -> Self { + pub fn new(params: Q, image_size: ImageSize) -> Self + where + Q: Borrow>, + { Self::from_params_and_size(params, image_size) } /// Creates a new camera from parameters and image size - pub fn from_params_and_size(params: &S::Vector, size: ImageSize) -> Self { + pub fn from_params_and_size(params: Q, size: ImageSize) -> Self + where + Q: Borrow>, + { + let params = params.borrow(); assert!( Distort::are_params_valid(params).all(), "Invalid parameters for {:?}", @@ -53,46 +62,67 @@ impl< } /// Distortion - maps a point in the camera z=1 plane to a distorted point - pub fn distort(&self, proj_point_in_camera_z1_plane: &S::Vector<2>) -> S::Vector<2> { + pub fn distort(&self, proj_point_in_camera_z1_plane: Q) -> S::Vector<2> + where + Q: Borrow>, + { Distort::distort(&self.params, proj_point_in_camera_z1_plane) } /// Undistortion - maps a distorted pixel to a point in the camera z=1 plane - pub fn undistort(&self, pixel: &S::Vector<2>) -> S::Vector<2> { + pub fn undistort(&self, pixel: Q) -> S::Vector<2> + where + Q: Borrow>, + { Distort::undistort(&self.params, pixel) } /// Derivative of the distortion w.r.t. the point in the camera z=1 plane - pub fn dx_distort_x(&self, proj_point_in_camera_z1_plane: &S::Vector<2>) -> S::Matrix<2, 2> { + pub fn dx_distort_x(&self, proj_point_in_camera_z1_plane: Q) -> S::Matrix<2, 2> + where + Q: Borrow>, + { Distort::dx_distort_x(&self.params, proj_point_in_camera_z1_plane) } /// Derivative of the distortion w.r.t. the parameters - pub fn dx_distort_params( - &self, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, PARAMS> { + pub fn dx_distort_params(&self, proj_point_in_camera_z1_plane: Q) -> S::Matrix<2, PARAMS> + where + Q: Borrow>, + { Distort::dx_distort_params(&self.params, proj_point_in_camera_z1_plane) } /// Projects a 3D point in the camera frame to a pixel in the image - pub fn cam_proj(&self, point_in_camera: &S::Vector<3>) -> S::Vector<2> { - self.distort(&Proj::proj(point_in_camera)) + pub fn cam_proj(&self, point_in_camera: Q) -> S::Vector<2> + where + Q: Borrow>, + { + self.distort(Proj::proj(point_in_camera)) } /// Unprojects a pixel in the image to a 3D point in the camera frame - assuming z=1 - pub fn cam_unproj(&self, point_in_camera: &S::Vector<2>) -> S::Vector<3> { + pub fn cam_unproj(&self, point_in_camera: Q) -> S::Vector<3> + where + Q: Borrow>, + { self.cam_unproj_with_z(point_in_camera, S::ones()) } /// Unprojects a pixel in the image to a 3D point in the camera frame - pub fn cam_unproj_with_z(&self, point_in_camera: &S::Vector<2>, z: S) -> S::Vector<3> { - Proj::unproj(&self.undistort(point_in_camera), z) + pub fn cam_unproj_with_z(&self, point_in_camera: Q, z: S) -> S::Vector<3> + where + Q: Borrow>, + { + Proj::unproj(self.undistort(point_in_camera), z) } /// Sets the camera parameters - pub fn set_params(&mut self, params: &S::Vector) { - self.params = params.clone(); + pub fn set_params(&mut self, params: Q) + where + Q: Borrow>, + { + self.params = params.borrow().clone(); } /// Returns the camera parameters @@ -126,6 +156,6 @@ impl< > Default for Camera { fn default() -> Self { - Self::from_params_and_size(&Distort::identity_params(), ImageSize::default()) + Self::from_params_and_size(Distort::identity_params(), ImageSize::default()) } } diff --git a/crates/sophus_sensor/src/camera_enum/general_camera.rs b/crates/sophus_sensor/src/camera_enum/general_camera.rs index 5524deaa..73f9e964 100644 --- a/crates/sophus_sensor/src/camera_enum/general_camera.rs +++ b/crates/sophus_sensor/src/camera_enum/general_camera.rs @@ -1,7 +1,12 @@ +use core::borrow::Borrow; + use crate::camera_enum::perspective_camera::PerspectiveCameraEnum; use crate::camera_enum::perspective_camera::UnifiedCamera; use crate::prelude::*; use crate::projections::orthographic::OrthographicCamera; +use crate::BrownConradyCamera; +use crate::KannalaBrandtCamera; +use crate::PinholeCamera; use sophus_image::ImageSize; /// Generalized camera enum @@ -20,94 +25,134 @@ impl, const BATCH: usize> GeneralCameraEnum { } } -impl, const BATCH: usize> IsCameraEnum - for GeneralCameraEnum -{ - fn new_pinhole(params: &S::Vector<4>, image_size: ImageSize) -> Self { +impl, const BATCH: usize> IsCamera for GeneralCameraEnum { + fn new_pinhole

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { Self::Perspective(PerspectiveCameraEnum::new_pinhole(params, image_size)) } - fn new_kannala_brandt(params: &S::Vector<8>, image_size: ImageSize) -> Self { + fn new_kannala_brandt

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { Self::Perspective(PerspectiveCameraEnum::new_kannala_brandt( params, image_size, )) } - fn new_brown_conrady(params: &S::Vector<12>, image_size: ImageSize) -> Self { + fn new_brown_conrady

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { Self::Perspective(PerspectiveCameraEnum::new_brown_conrady(params, image_size)) } - fn new_unified(params: &>::Vector<6>, image_size: ImageSize) -> Self { + fn new_unified

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { Self::Perspective(PerspectiveCameraEnum::new_unified(params, image_size)) } - fn cam_proj(&self, point_in_camera: &S::Vector<3>) -> S::Vector<2> { + fn image_size(&self) -> ImageSize { match self { - GeneralCameraEnum::Perspective(camera) => camera.cam_proj(point_in_camera), - GeneralCameraEnum::Orthographic(camera) => camera.cam_proj(point_in_camera), + GeneralCameraEnum::Perspective(camera) => camera.image_size(), + GeneralCameraEnum::Orthographic(camera) => camera.image_size(), } } - fn cam_unproj_with_z(&self, point_in_camera: &S::Vector<2>, z: S) -> S::Vector<3> { + fn cam_proj

(&self, point_in_camera: P) -> S::Vector<2> + where + P: Borrow>, + { + let point_in_camera = point_in_camera.borrow(); match self { - GeneralCameraEnum::Perspective(camera) => camera.cam_unproj_with_z(point_in_camera, z), - GeneralCameraEnum::Orthographic(camera) => camera.cam_unproj_with_z(point_in_camera, z), + GeneralCameraEnum::Perspective(camera) => camera.cam_proj(point_in_camera), + GeneralCameraEnum::Orthographic(camera) => camera.cam_proj(point_in_camera), } } - fn distort(&self, point_in_camera: &S::Vector<2>) -> S::Vector<2> { + fn cam_unproj_with_z

(&self, pixel: P, z: S) -> S::Vector<3> + where + P: Borrow>, + { + let pixel = pixel.borrow(); match self { - GeneralCameraEnum::Perspective(camera) => camera.distort(point_in_camera), - GeneralCameraEnum::Orthographic(camera) => camera.distort(point_in_camera), + GeneralCameraEnum::Perspective(camera) => camera.cam_unproj_with_z(pixel, z), + GeneralCameraEnum::Orthographic(camera) => camera.cam_unproj_with_z(pixel, z), } } - fn undistort(&self, point_in_camera: &S::Vector<2>) -> S::Vector<2> { + fn distort

(&self, proj_point_in_camera_z1_plane: P) -> S::Vector<2> + where + P: Borrow>, + { + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); match self { - GeneralCameraEnum::Perspective(camera) => camera.undistort(point_in_camera), - GeneralCameraEnum::Orthographic(camera) => camera.undistort(point_in_camera), + GeneralCameraEnum::Perspective(camera) => camera.distort(proj_point_in_camera_z1_plane), + GeneralCameraEnum::Orthographic(camera) => { + camera.distort(proj_point_in_camera_z1_plane) + } } } - fn dx_distort_x(&self, point_in_camera: &S::Vector<2>) -> S::Matrix<2, 2> { + fn undistort

(&self, pixel: P) -> S::Vector<2> + where + P: Borrow>, + { + let pixel = pixel.borrow(); match self { - GeneralCameraEnum::Perspective(camera) => camera.dx_distort_x(point_in_camera), - GeneralCameraEnum::Orthographic(camera) => camera.dx_distort_x(point_in_camera), + GeneralCameraEnum::Perspective(camera) => camera.undistort(pixel), + GeneralCameraEnum::Orthographic(camera) => camera.undistort(pixel), } } - fn image_size(&self) -> ImageSize { + fn dx_distort_x

(&self, proj_point_in_camera_z1_plane: P) -> S::Matrix<2, 2> + where + P: Borrow>, + { + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); match self { - GeneralCameraEnum::Perspective(camera) => camera.image_size(), - GeneralCameraEnum::Orthographic(camera) => camera.image_size(), + GeneralCameraEnum::Perspective(camera) => { + camera.dx_distort_x(proj_point_in_camera_z1_plane) + } + GeneralCameraEnum::Orthographic(camera) => { + camera.dx_distort_x(proj_point_in_camera_z1_plane) + } } } - fn try_get_brown_conrady(self) -> Option> { - match self { - GeneralCameraEnum::Perspective(camera) => camera.try_get_brown_conrady(), - GeneralCameraEnum::Orthographic(_) => None, + fn try_get_brown_conrady(self) -> Option> { + if let GeneralCameraEnum::Perspective(camera) = self { + camera.try_get_brown_conrady() + } else { + None } } - fn try_get_kannala_brandt(self) -> Option> { - match self { - GeneralCameraEnum::Perspective(camera) => camera.try_get_kannala_brandt(), - GeneralCameraEnum::Orthographic(_) => None, + fn try_get_kannala_brandt(self) -> Option> { + if let GeneralCameraEnum::Perspective(camera) = self { + camera.try_get_kannala_brandt() + } else { + None } } - fn try_get_pinhole(self) -> Option> { - match self { - GeneralCameraEnum::Perspective(camera) => camera.try_get_pinhole(), - GeneralCameraEnum::Orthographic(_) => None, + fn try_get_pinhole(self) -> Option> { + if let GeneralCameraEnum::Perspective(camera) = self { + camera.try_get_pinhole() + } else { + None } } fn try_get_unified_extended(self) -> Option> { - match self { - GeneralCameraEnum::Perspective(camera) => camera.try_get_unified_extended(), - GeneralCameraEnum::Orthographic(_) => None, + if let GeneralCameraEnum::Perspective(camera) = self { + camera.try_get_unified_extended() + } else { + None } } } diff --git a/crates/sophus_sensor/src/camera_enum/perspective_camera.rs b/crates/sophus_sensor/src/camera_enum/perspective_camera.rs index eb316d71..bec85305 100644 --- a/crates/sophus_sensor/src/camera_enum/perspective_camera.rs +++ b/crates/sophus_sensor/src/camera_enum/perspective_camera.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use crate::distortions::affine::AffineDistortionImpl; use crate::distortions::brown_conrady::BrownConradyDistortionImpl; use crate::distortions::kannala_brandt::KannalaBrandtDistortionImpl; @@ -54,25 +56,39 @@ pub enum PerspectiveCameraEnum, const BATCH: usize> { UnifiedExtended(UnifiedCamera), } -impl, const BATCH: usize> IsCameraEnum +impl, const BATCH: usize> IsCamera for PerspectiveCameraEnum { - fn new_pinhole(params: &S::Vector<4>, image_size: ImageSize) -> Self { - Self::Pinhole(PinholeCamera::from_params_and_size(params, image_size)) + fn new_pinhole

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { + let params = params.borrow(); + PerspectiveCameraEnum::Pinhole(PinholeCamera::new(params, image_size)) } - fn new_kannala_brandt(params: &S::Vector<8>, image_size: ImageSize) -> Self { - Self::KannalaBrandt(KannalaBrandtCamera::from_params_and_size( - params, image_size, - )) + fn new_kannala_brandt

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { + let params = params.borrow(); + PerspectiveCameraEnum::KannalaBrandt(KannalaBrandtCamera::new(params, image_size)) } - fn new_brown_conrady(params: &S::Vector<12>, image_size: ImageSize) -> Self { - Self::BrownConrady(BrownConradyCamera::from_params_and_size(params, image_size)) + fn new_brown_conrady

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { + let params = params.borrow(); + PerspectiveCameraEnum::BrownConrady(BrownConradyCamera::new(params, image_size)) } - fn new_unified(params: &>::Vector<6>, image_size: ImageSize) -> Self { - Self::UnifiedExtended(UnifiedCamera::from_params_and_size(params, image_size)) + fn new_unified

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>, + { + let params = params.borrow(); + PerspectiveCameraEnum::UnifiedExtended(UnifiedCamera::new(params, image_size)) } fn image_size(&self) -> ImageSize { @@ -84,7 +100,12 @@ impl, const BATCH: usize> IsCameraEnum } } - fn cam_proj(&self, point_in_camera: &S::Vector<3>) -> S::Vector<2> { + fn cam_proj

(&self, point_in_camera: P) -> S::Vector<2> + where + P: Borrow>, + { + let point_in_camera = point_in_camera.borrow(); + match self { PerspectiveCameraEnum::Pinhole(camera) => camera.cam_proj(point_in_camera), PerspectiveCameraEnum::KannalaBrandt(camera) => camera.cam_proj(point_in_camera), @@ -93,97 +114,119 @@ impl, const BATCH: usize> IsCameraEnum } } - fn cam_unproj_with_z(&self, point_in_camera: &S::Vector<2>, z: S) -> S::Vector<3> { + fn cam_unproj_with_z

(&self, pixel: P, z: S) -> S::Vector<3> + where + P: Borrow>, + { + let pixel = pixel.borrow(); + match self { + PerspectiveCameraEnum::Pinhole(camera) => camera.cam_unproj_with_z(pixel, z), + PerspectiveCameraEnum::KannalaBrandt(camera) => camera.cam_unproj_with_z(pixel, z), + PerspectiveCameraEnum::BrownConrady(camera) => camera.cam_unproj_with_z(pixel, z), + PerspectiveCameraEnum::UnifiedExtended(camera) => camera.cam_unproj_with_z(pixel, z), + } + } + + fn distort

(&self, proj_point_in_camera_z1_plane: P) -> S::Vector<2> + where + P: Borrow>, + { + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); match self { - PerspectiveCameraEnum::Pinhole(camera) => camera.cam_unproj_with_z(point_in_camera, z), + PerspectiveCameraEnum::Pinhole(camera) => camera.distort(proj_point_in_camera_z1_plane), PerspectiveCameraEnum::KannalaBrandt(camera) => { - camera.cam_unproj_with_z(point_in_camera, z) + camera.distort(proj_point_in_camera_z1_plane) } PerspectiveCameraEnum::BrownConrady(camera) => { - camera.cam_unproj_with_z(point_in_camera, z) + camera.distort(proj_point_in_camera_z1_plane) } PerspectiveCameraEnum::UnifiedExtended(camera) => { - camera.cam_unproj_with_z(point_in_camera, z) + camera.distort(proj_point_in_camera_z1_plane) } } } - fn distort(&self, point_in_camera: &S::Vector<2>) -> S::Vector<2> { + fn undistort

(&self, pixel: P) -> S::Vector<2> + where + P: Borrow>, + { + let pixel = pixel.borrow(); match self { - PerspectiveCameraEnum::Pinhole(camera) => camera.distort(point_in_camera), - PerspectiveCameraEnum::KannalaBrandt(camera) => camera.distort(point_in_camera), - PerspectiveCameraEnum::BrownConrady(camera) => camera.distort(point_in_camera), - PerspectiveCameraEnum::UnifiedExtended(camera) => camera.distort(point_in_camera), + PerspectiveCameraEnum::Pinhole(camera) => camera.undistort(pixel), + PerspectiveCameraEnum::KannalaBrandt(camera) => camera.undistort(pixel), + PerspectiveCameraEnum::BrownConrady(camera) => camera.undistort(pixel), + PerspectiveCameraEnum::UnifiedExtended(camera) => camera.undistort(pixel), } } - fn undistort(&self, point_in_camera: &S::Vector<2>) -> S::Vector<2> { + fn dx_distort_x

(&self, proj_point_in_camera_z1_plane: P) -> S::Matrix<2, 2> + where + P: Borrow>, + { + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); match self { - PerspectiveCameraEnum::Pinhole(camera) => camera.undistort(point_in_camera), - PerspectiveCameraEnum::KannalaBrandt(camera) => camera.undistort(point_in_camera), - PerspectiveCameraEnum::BrownConrady(camera) => camera.undistort(point_in_camera), - PerspectiveCameraEnum::UnifiedExtended(camera) => camera.undistort(point_in_camera), - } - } - - fn dx_distort_x(&self, point_in_camera: &S::Vector<2>) -> S::Matrix<2, 2> { - match self { - PerspectiveCameraEnum::Pinhole(camera) => camera.dx_distort_x(point_in_camera), - PerspectiveCameraEnum::KannalaBrandt(camera) => camera.dx_distort_x(point_in_camera), - PerspectiveCameraEnum::BrownConrady(camera) => camera.dx_distort_x(point_in_camera), - PerspectiveCameraEnum::UnifiedExtended(camera) => camera.dx_distort_x(point_in_camera), + PerspectiveCameraEnum::Pinhole(camera) => { + camera.dx_distort_x(proj_point_in_camera_z1_plane) + } + PerspectiveCameraEnum::KannalaBrandt(camera) => { + camera.dx_distort_x(proj_point_in_camera_z1_plane) + } + PerspectiveCameraEnum::BrownConrady(camera) => { + camera.dx_distort_x(proj_point_in_camera_z1_plane) + } + PerspectiveCameraEnum::UnifiedExtended(camera) => { + camera.dx_distort_x(proj_point_in_camera_z1_plane) + } } } fn try_get_brown_conrady(self) -> Option> { - match self { - PerspectiveCameraEnum::Pinhole(_) => None, - PerspectiveCameraEnum::KannalaBrandt(_) => None, - PerspectiveCameraEnum::BrownConrady(camera) => Some(camera), - PerspectiveCameraEnum::UnifiedExtended(_) => None, + if let PerspectiveCameraEnum::BrownConrady(camera) = self { + Some(camera) + } else { + None } } fn try_get_kannala_brandt(self) -> Option> { - match self { - PerspectiveCameraEnum::Pinhole(_) => None, - PerspectiveCameraEnum::KannalaBrandt(camera) => Some(camera), - PerspectiveCameraEnum::BrownConrady(_) => None, - PerspectiveCameraEnum::UnifiedExtended(_) => None, + if let PerspectiveCameraEnum::KannalaBrandt(camera) = self { + Some(camera) + } else { + None } } fn try_get_pinhole(self) -> Option> { - match self { - PerspectiveCameraEnum::Pinhole(camera) => Some(camera), - PerspectiveCameraEnum::KannalaBrandt(_) => None, - PerspectiveCameraEnum::BrownConrady(_) => None, - PerspectiveCameraEnum::UnifiedExtended(_) => None, + if let PerspectiveCameraEnum::Pinhole(camera) = self { + Some(camera) + } else { + None } } fn try_get_unified_extended(self) -> Option> { - match self { - PerspectiveCameraEnum::Pinhole(_) => None, - PerspectiveCameraEnum::KannalaBrandt(_) => None, - PerspectiveCameraEnum::BrownConrady(_) => None, - PerspectiveCameraEnum::UnifiedExtended(camera) => Some(camera), + if let PerspectiveCameraEnum::UnifiedExtended(camera) = self { + Some(camera) + } else { + None } } } -impl, const BATCH: usize> IsPerspectiveCameraEnum +impl, const BATCH: usize> IsPerspectiveCamera for PerspectiveCameraEnum { fn pinhole_params(&self) -> S::Vector<4> { match self { PerspectiveCameraEnum::Pinhole(camera) => camera.params().clone(), PerspectiveCameraEnum::KannalaBrandt(camera) => { - camera.params().get_fixed_subvec::<4>(0) + camera.params().clone().get_fixed_subvec(0) + } + PerspectiveCameraEnum::BrownConrady(camera) => { + camera.params().clone().get_fixed_subvec(0) } - PerspectiveCameraEnum::BrownConrady(camera) => camera.params().get_fixed_subvec::<4>(0), PerspectiveCameraEnum::UnifiedExtended(camera) => { - camera.params().get_fixed_subvec::<4>(0) + camera.params().clone().get_fixed_subvec(0) } } } diff --git a/crates/sophus_sensor/src/distortion_table.rs b/crates/sophus_sensor/src/distortion_table.rs index d1c2feb4..c7df005e 100644 --- a/crates/sophus_sensor/src/distortion_table.rs +++ b/crates/sophus_sensor/src/distortion_table.rs @@ -61,16 +61,16 @@ pub fn distort_table(cam: &DynCamera) -> DistortTable { for i in 0..=cam.image_size().width { let u = i as f64 - 0.5; // top border - region.extend(&cam.undistort(&VecF64::<2>::new(u, v_top))); + region.extend(&cam.undistort(VecF64::<2>::new(u, v_top))); // bottom border - region.extend(&cam.undistort(&VecF64::<2>::new(u, v_bottom))); + region.extend(&cam.undistort(VecF64::<2>::new(u, v_bottom))); } for i in 0..=cam.image_size().height { let v = i as f64 - 0.5; // left border - region.extend(&cam.undistort(&VecF64::<2>::new(u_left, v))); + region.extend(&cam.undistort(VecF64::<2>::new(u_left, v))); // right border - region.extend(&cam.undistort(&VecF64::<2>::new(u_right, v))); + region.extend(&cam.undistort(VecF64::<2>::new(u_right, v))); } let mid = region.mid(); @@ -91,7 +91,7 @@ pub fn distort_table(cam: &DynCamera) -> DistortTable { distort_table.offset().x + (u as f64) * distort_table.incr().x, distort_table.offset().y + (v as f64) * distort_table.incr().y, ); - let pixel = cam.distort(&point_proj); + let pixel = cam.distort(point_proj); *table.mut_pixel(u, v) = SVector::::new(pixel.cast().x, pixel.cast().y); } } @@ -115,7 +115,7 @@ fn camera_distortion_table_tests() { { let mut cameras: alloc::vec::Vec = alloc::vec![]; cameras.push(DynCameraF64::new_pinhole( - &VecF64::<4>::new(600.0, 600.0, 1.0, 0.5), + VecF64::<4>::new(600.0, 600.0, 1.0, 0.5), ImageSize { width: 3, height: 2, @@ -123,7 +123,7 @@ fn camera_distortion_table_tests() { )); cameras.push(DynCamera::new_kannala_brandt( - &VecF64::<8>::from_vec(alloc::vec![ + VecF64::<8>::from_vec(alloc::vec![ 1000.0, 1000.0, 320.0, 280.0, 0.1, 0.01, 0.001, 0.0001 ]), ImageSize { @@ -155,20 +155,20 @@ fn camera_distortion_table_tests() { } for d in [1.0, 0.1, 0.5, 1.1, 3.0, 15.0] { - let point_in_camera = camera.cam_unproj_with_z(&pixel, d); + let point_in_camera = camera.cam_unproj_with_z(pixel, d); assert_relative_eq!(point_in_camera[2], d, epsilon = EPS_F64); - let pixel_in_image2 = camera.cam_proj(&point_in_camera); + let pixel_in_image2 = camera.cam_proj(point_in_camera); assert_relative_eq!(pixel_in_image2, pixel, epsilon = EPS_F64); } - let ab_in_z1plane = camera.undistort(&pixel); + let ab_in_z1plane = camera.undistort(pixel); - let pixel_in_image3 = camera.distort(&ab_in_z1plane); + let pixel_in_image3 = camera.distort(ab_in_z1plane); assert_relative_eq!(pixel_in_image3, pixel, epsilon = EPS_F64); - let dx = camera.dx_distort_x(&pixel); + let dx = camera.dx_distort_x(pixel); let numeric_dx = VectorValuedMapFromVector::static_sym_diff_quotient( - |x: VecF64<2>| camera.distort(&x), + |x: VecF64<2>| camera.distort(x), pixel, EPS_F64, ); @@ -184,8 +184,8 @@ fn camera_distortion_table_tests() { { continue; } - let proj = camera.undistort(&pixel); - let analytic_pixel = camera.distort(&proj); + let proj = camera.undistort(pixel); + let analytic_pixel = camera.distort(proj); let lut_pixel = table.lookup(&proj); assert_abs_diff_eq!(analytic_pixel, pixel, epsilon = 1e-3); diff --git a/crates/sophus_sensor/src/distortions/affine.rs b/crates/sophus_sensor/src/distortions/affine.rs index cc04003d..e884a298 100644 --- a/crates/sophus_sensor/src/distortions/affine.rs +++ b/crates/sophus_sensor/src/distortions/affine.rs @@ -1,5 +1,6 @@ use crate::prelude::*; use crate::traits::IsCameraDistortionImpl; +use core::borrow::Borrow; use core::marker::PhantomData; use sophus_core::params::ParamsImpl; @@ -16,7 +17,10 @@ pub struct AffineDistortionImpl, const BATCH: usize> { impl, const BATCH: usize> ParamsImpl for AffineDistortionImpl { - fn are_params_valid(_params: &S::Vector<4>) -> S::Mask { + fn are_params_valid

(_params: P) -> S::Mask + where + P: Borrow>, + { S::Mask::all_true() } @@ -35,37 +39,53 @@ impl, const BATCH: usize> ParamsImpl impl, const BATCH: usize> IsCameraDistortionImpl for AffineDistortionImpl { - fn distort( - params: &S::Vector<4>, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Vector<2> { + fn distort(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); S::Vector::<2>::from_array([ proj_point_in_camera_z1_plane.get_elem(0) * params.get_elem(0) + params.get_elem(2), proj_point_in_camera_z1_plane.get_elem(1) * params.get_elem(1) + params.get_elem(3), ]) } - fn undistort(params: &S::Vector<4>, distorted_point: &S::Vector<2>) -> S::Vector<2> { + fn undistort(params: PA, distorted_point: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let distorted_point = distorted_point.borrow(); + S::Vector::<2>::from_array([ (distorted_point.get_elem(0) - params.get_elem(2)) / params.get_elem(0), (distorted_point.get_elem(1) - params.get_elem(3)) / params.get_elem(1), ]) } - fn dx_distort_x( - params: &S::Vector<4>, - _proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, 2> { + fn dx_distort_x(params: PA, _proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + S::Matrix::<2, 2>::from_array2([ [params.get_elem(0), S::zeros()], [S::zeros(), params.get_elem(1)], ]) } - fn dx_distort_params( - _params: &S::Vector<4>, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, 4> { + fn dx_distort_params(_params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 4> + where + PA: Borrow>, + PO: Borrow>, + { + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + S::Matrix::<2, 4>::from_array2([ [ proj_point_in_camera_z1_plane.get_elem(0), diff --git a/crates/sophus_sensor/src/distortions/brown_conrady.rs b/crates/sophus_sensor/src/distortions/brown_conrady.rs index 5586d576..9debcd1c 100644 --- a/crates/sophus_sensor/src/distortions/brown_conrady.rs +++ b/crates/sophus_sensor/src/distortions/brown_conrady.rs @@ -1,6 +1,7 @@ use crate::distortions::affine::AffineDistortionImpl; use crate::prelude::*; use crate::traits::IsCameraDistortionImpl; +use core::borrow::Borrow; use core::marker::PhantomData; use log::warn; use sophus_core::linalg::EPS_F64; @@ -17,7 +18,10 @@ pub struct BrownConradyDistortionImpl, const BATCH: usize> { impl, const BATCH: usize> ParamsImpl for BrownConradyDistortionImpl { - fn are_params_valid(_params: &S::Vector<12>) -> S::Mask { + fn are_params_valid

(_params: P) -> S::Mask + where + P: Borrow>, + { S::Mask::all_true() } @@ -230,22 +234,32 @@ impl, const BATCH: usize> BrownConradyDistortionImpl, const BATCH: usize> IsCameraDistortionImpl for BrownConradyDistortionImpl { - fn distort( - params: &S::Vector<12>, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Vector<2> { + fn distort(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + let distorted_point_in_camera_z1_plane = Self::distortion_impl(¶ms.get_fixed_subvec(4), proj_point_in_camera_z1_plane); AffineDistortionImpl::::distort( - ¶ms.get_fixed_subvec(0), - &distorted_point_in_camera_z1_plane, + params.get_fixed_subvec(0), + distorted_point_in_camera_z1_plane, ) } - fn undistort(params: &S::Vector<12>, distorted_point: &S::Vector<2>) -> S::Vector<2> { + fn undistort(params: PA, distorted_point: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let distorted_point = distorted_point.borrow(); let undistorted_point_in_camera_z1_plane = AffineDistortionImpl::::undistort( - ¶ms.get_fixed_subvec(0), + params.get_fixed_subvec(0), distorted_point, ); @@ -256,10 +270,14 @@ impl, const BATCH: usize> IsCameraDistortionImpl, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, 2> { + fn dx_distort_x(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + const OFFSET: usize = 4; let fx = params.get_elem(0); @@ -328,10 +346,14 @@ impl, const BATCH: usize> IsCameraDistortionImpl, - _proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, 12> { + fn dx_distort_params( + _params: PA, + _proj_point_in_camera_z1_plane: PO, + ) -> S::Matrix<2, 12> + where + PA: Borrow>, + PO: Borrow>, + { todo!() } } diff --git a/crates/sophus_sensor/src/distortions/kannala_brandt.rs b/crates/sophus_sensor/src/distortions/kannala_brandt.rs index 6b99cfa2..658067f9 100644 --- a/crates/sophus_sensor/src/distortions/kannala_brandt.rs +++ b/crates/sophus_sensor/src/distortions/kannala_brandt.rs @@ -1,5 +1,6 @@ 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; @@ -15,7 +16,10 @@ pub struct KannalaBrandtDistortionImpl, const BATCH: usize> { impl, const BATCH: usize> ParamsImpl for KannalaBrandtDistortionImpl { - fn are_params_valid(_params: &S::Vector<8>) -> S::Mask { + fn are_params_valid

(_params: P) -> S::Mask + where + P: Borrow>, + { S::Mask::all_true() } @@ -36,10 +40,14 @@ impl, const BATCH: usize> ParamsImpl impl, const BATCH: usize> IsCameraDistortionImpl for KannalaBrandtDistortionImpl { - fn distort( - params: &S::Vector<8>, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Vector<2> { + fn distort(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + let k0 = params.get_elem(4); let k1 = params.get_elem(5); let k2 = params.get_elem(6); @@ -73,7 +81,13 @@ impl, const BATCH: usize> IsCameraDistortionImpl, distorted_point: &S::Vector<2>) -> S::Vector<2> { + fn undistort(params: PA, distorted_point: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let distorted_point = distorted_point.borrow(); let fu = params.get_elem(0); let fv = params.get_elem(1); let u0 = params.get_elem(2); @@ -149,10 +163,14 @@ impl, const BATCH: usize> IsCameraDistortionImpl, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, 2> { + fn dx_distort_x(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + let a = proj_point_in_camera_z1_plane.get_elem(0); let b = proj_point_in_camera_z1_plane.get_elem(1); let fx = params.get_elem(0); @@ -221,10 +239,14 @@ impl, const BATCH: usize> IsCameraDistortionImpl, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, 8> { + fn dx_distort_params(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 8> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + let x = proj_point_in_camera_z1_plane.get_elem(0); let y = proj_point_in_camera_z1_plane.get_elem(1); let fx = params.get_elem(0); diff --git a/crates/sophus_sensor/src/distortions/unified.rs b/crates/sophus_sensor/src/distortions/unified.rs index 5f2ea41e..7ea41105 100644 --- a/crates/sophus_sensor/src/distortions/unified.rs +++ b/crates/sophus_sensor/src/distortions/unified.rs @@ -1,5 +1,6 @@ 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; @@ -17,7 +18,10 @@ pub struct UnifiedDistortionImpl, const BATCH: usize> { impl, const BATCH: usize> ParamsImpl for UnifiedDistortionImpl { - fn are_params_valid(_params: &S::Vector<6>) -> S::Mask { + fn are_params_valid

(_params: P) -> S::Mask + where + P: Borrow>, + { S::Mask::all_true() } @@ -39,10 +43,14 @@ impl, const BATCH: usize> ParamsImpl impl, const BATCH: usize> IsCameraDistortionImpl for UnifiedDistortionImpl { - fn distort( - params: &>::Vector<6>, - proj_point_in_camera_z1_plane: &>::Vector<2>, - ) -> >::Vector<2> { + fn distort(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + // https://gitlab.com/VladyslavUsenko/basalt-headers/-/blob/master/include/basalt/camera/extended_camera.hpp?ref_type=heads#L125 let fx = params.get_elem(0); let fy = params.get_elem(1); @@ -66,10 +74,13 @@ impl, const BATCH: usize> IsCameraDistortionImpl::from_array([fx * mx + cx, fy * my + cy]) } - fn undistort( - params: &>::Vector<6>, - distorted_point: &>::Vector<2>, - ) -> >::Vector<2> { + fn undistort(params: PA, distorted_point: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let distorted_point = distorted_point.borrow(); let fx = params.get_elem(0); let fy = params.get_elem(1); let cx = params.get_elem(2); @@ -94,11 +105,14 @@ impl, const BATCH: usize> IsCameraDistortionImpl::from_array([mx / k.clone(), my / k.clone()]) } + fn dx_distort_x(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); - fn dx_distort_x( - params: &>::Vector<6>, - proj_point_in_camera_z1_plane: &>::Vector<2>, - ) -> >::Matrix<2, 2> { let fx = params.get_elem(0); let fy = params.get_elem(1); let alpha = params.get_elem(4); @@ -135,10 +149,14 @@ impl, const BATCH: usize> IsCameraDistortionImpl>::Vector<6>, - proj_point_in_camera_z1_plane: &>::Vector<2>, - ) -> >::Matrix<2, 6> { + fn dx_distort_params(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 6> + where + PA: Borrow>, + PO: Borrow>, + { + let params = params.borrow(); + let proj_point_in_camera_z1_plane = proj_point_in_camera_z1_plane.borrow(); + let fx = params.get_elem(0); let fy = params.get_elem(1); let alpha = params.get_elem(4); diff --git a/crates/sophus_sensor/src/dyn_camera.rs b/crates/sophus_sensor/src/dyn_camera.rs index 7aca2254..3609b097 100644 --- a/crates/sophus_sensor/src/dyn_camera.rs +++ b/crates/sophus_sensor/src/dyn_camera.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use crate::camera_enum::perspective_camera::UnifiedCamera; use crate::camera_enum::GeneralCameraEnum; use crate::camera_enum::PerspectiveCameraEnum; @@ -14,7 +16,7 @@ extern crate alloc; pub struct DynCameraFacade< S: IsScalar, const BATCH: usize, - CameraType: IsCameraEnum, + CameraType: IsPerspectiveCamera, > { camera_type: CameraType, phantom: core::marker::PhantomData, @@ -30,8 +32,11 @@ pub type DynCamera = /// Create a new dynamic camera facade from a camera model pub type DynCameraF64 = DynCamera; -impl, const BATCH: usize, CameraType: IsCameraEnum> - DynCameraFacade +impl< + S: IsScalar, + const BATCH: usize, + CameraType: IsPerspectiveCamera + IsCamera, + > DynCameraFacade { /// Create default pinhole from Image Size pub fn default_pinhole(image_size: ImageSize) -> Self { @@ -41,7 +46,7 @@ impl, const BATCH: usize, CameraType: IsCameraEnum> let focal_length = (w + h) * 0.5; Self { camera_type: CameraType::new_pinhole( - &S::Vector::<4>::from_f64_array([ + S::Vector::<4>::from_f64_array([ focal_length, focal_length, 0.5 * w - 0.5, @@ -61,7 +66,7 @@ impl, const BATCH: usize, CameraType: IsCameraEnum> let focal_length = (w + h) * 0.5; Self { camera_type: CameraType::new_kannala_brandt( - &S::Vector::<8>::from_f64_array([ + S::Vector::<8>::from_f64_array([ focal_length, focal_length, 0.5 * w - 0.5, @@ -86,53 +91,57 @@ impl, const BATCH: usize, CameraType: IsCameraEnum> } /// Create a pinhole camera instance - pub fn new_pinhole(params: &S::Vector<4>, image_size: ImageSize) -> Self { + pub fn new_pinhole(params: impl Borrow>, image_size: ImageSize) -> Self { Self::from_model(CameraType::new_pinhole(params, image_size)) } /// Create a Kannala-Brandt camera instance - pub fn new_kannala_brandt(params: &S::Vector<8>, image_size: ImageSize) -> Self { + pub fn new_kannala_brandt(params: impl Borrow>, image_size: ImageSize) -> Self { Self::from_model(CameraType::new_kannala_brandt(params, image_size)) } /// Create a Brown-Conrady camera instance - pub fn new_brown_conrady(params: &S::Vector<12>, image_size: ImageSize) -> Self { + pub fn new_brown_conrady(params: impl Borrow>, image_size: ImageSize) -> Self { Self::from_model(CameraType::new_brown_conrady(params, image_size)) } /// Create a unified camera instance - pub fn new_unified(params: &S::Vector<6>, image_size: ImageSize) -> Self { + pub fn new_unified(params: impl Borrow>, image_size: ImageSize) -> Self { Self::from_model(CameraType::new_unified(params, image_size)) } /// Projects a 3D point in the camera frame to a pixel in the image - pub fn cam_proj(&self, point_in_camera: &S::Vector<3>) -> S::Vector<2> { - self.camera_type.cam_proj(point_in_camera) + pub fn cam_proj(&self, point_in_camera: impl Borrow>) -> S::Vector<2> { + self.camera_type.cam_proj(point_in_camera.borrow()) } /// Unprojects a pixel in the image to a 3D point in the camera frame - assuming z=1 - pub fn cam_unproj(&self, pixel: &S::Vector<2>) -> S::Vector<3> { - self.cam_unproj_with_z(pixel, S::ones()) + pub fn cam_unproj(&self, pixel: impl Borrow>) -> S::Vector<3> { + self.cam_unproj_with_z(pixel.borrow(), S::ones()) } /// Unprojects a pixel in the image to a 3D point in the camera frame - pub fn cam_unproj_with_z(&self, pixel: &S::Vector<2>, z: S) -> S::Vector<3> { - self.camera_type.cam_unproj_with_z(pixel, z) + pub fn cam_unproj_with_z(&self, pixel: impl Borrow>, z: S) -> S::Vector<3> { + self.camera_type.cam_unproj_with_z(pixel.borrow(), z) } /// Distortion - maps a point in the camera z=1 plane to a distorted point - pub fn distort(&self, proj_point_in_camera_z1_plane: &S::Vector<2>) -> S::Vector<2> { - self.camera_type.distort(proj_point_in_camera_z1_plane) + pub fn distort( + &self, + proj_point_in_camera_z1_plane: impl Borrow>, + ) -> S::Vector<2> { + self.camera_type + .distort(proj_point_in_camera_z1_plane.borrow()) } /// Undistortion - maps a distorted pixel to a point in the camera z=1 plane - pub fn undistort(&self, pixel: &S::Vector<2>) -> S::Vector<2> { - self.camera_type.undistort(pixel) + pub fn undistort(&self, pixel: impl Borrow>) -> S::Vector<2> { + self.camera_type.undistort(pixel.borrow()) } /// Derivative of the distortion w.r.t. the point in the camera z=1 plane - pub fn dx_distort_x(&self, point_in_camera: &S::Vector<2>) -> S::Matrix<2, 2> { - self.camera_type.dx_distort_x(point_in_camera) + pub fn dx_distort_x(&self, point_in_camera: impl Borrow>) -> S::Matrix<2, 2> { + self.camera_type.dx_distort_x(point_in_camera.borrow()) } /// Returns the image size @@ -191,7 +200,7 @@ fn dyn_camera_tests() { { let mut cameras: alloc::vec::Vec = alloc::vec![]; cameras.push(DynCameraF64::new_pinhole( - &VecF64::<4>::new(600.0, 599.0, 1.0, 0.5), + VecF64::<4>::new(600.0, 599.0, 1.0, 0.5), ImageSize { width: 3, height: 2, @@ -199,7 +208,7 @@ fn dyn_camera_tests() { )); cameras.push(DynCamera::new_unified( - &VecF64::<6>::from_vec(alloc::vec![998.0, 1000.0, 320.0, 280.0, 0.5, 1.2]), + VecF64::<6>::from_vec(alloc::vec![998.0, 1000.0, 320.0, 280.0, 0.5, 1.2]), ImageSize { width: 640, height: 480, @@ -207,7 +216,7 @@ fn dyn_camera_tests() { )); cameras.push(DynCamera::new_kannala_brandt( - &VecF64::<8>::from_vec(alloc::vec![ + VecF64::<8>::from_vec(alloc::vec![ 999.0, 1000.0, 320.0, 280.0, 0.1, 0.01, 0.001, 0.0001 ]), ImageSize { @@ -217,7 +226,7 @@ fn dyn_camera_tests() { )); cameras.push(DynCamera::new_brown_conrady( - &VecF64::<12>::from_vec(alloc::vec![ + VecF64::<12>::from_vec(alloc::vec![ 288.0, 284.0, 0.5 * (424.0 - 1.0), @@ -262,20 +271,20 @@ fn dyn_camera_tests() { } for d in [1.0, 0.1, 0.5, 1.1, 3.0, 15.0] { - let point_in_camera = camera.cam_unproj_with_z(&pixel, d); + let point_in_camera = camera.cam_unproj_with_z(pixel, d); assert_relative_eq!(point_in_camera[2], d, epsilon = EPS_F64); - let pixel_in_image2 = camera.cam_proj(&point_in_camera); + let pixel_in_image2 = camera.cam_proj(point_in_camera); assert_relative_eq!(pixel_in_image2, pixel, epsilon = 1e-4); } - let ab_in_z1plane = camera.undistort(&pixel); + let ab_in_z1plane = camera.undistort(pixel); - let pixel_in_image3 = camera.distort(&ab_in_z1plane); + let pixel_in_image3 = camera.distort(ab_in_z1plane); assert_relative_eq!(pixel_in_image3, pixel, epsilon = 1e-4); - let dx = camera.dx_distort_x(&pixel); + let dx = camera.dx_distort_x(pixel); let numeric_dx = VectorValuedMapFromVector::static_sym_diff_quotient( - |x: VecF64<2>| camera.distort(&x), + |x: VecF64<2>| camera.distort(x), pixel, EPS_F64, ); @@ -284,24 +293,22 @@ fn dyn_camera_tests() { match camera.camera_type { PerspectiveCameraEnum::Pinhole(camera) => { - let dx_params = camera.dx_distort_params(&pixel); + let dx_params = camera.dx_distort_params(pixel); let params = *camera.params(); let numeric_dx_params = VectorValuedMapFromVector::static_sym_diff_quotient( - |p: VecF64<4>| AffineDistortionImpl::::distort(&p, &pixel), + |p: VecF64<4>| AffineDistortionImpl::::distort(p, pixel), params, EPS_F64, ); assert_relative_eq!(dx_params, numeric_dx_params, epsilon = 1e-4); } PerspectiveCameraEnum::KannalaBrandt(camera) => { - let dx_params = camera.dx_distort_params(&pixel); + let dx_params = camera.dx_distort_params(pixel); let params = *camera.params(); let numeric_dx_params = VectorValuedMapFromVector::static_sym_diff_quotient( - |p: VecF64<8>| { - KannalaBrandtDistortionImpl::::distort(&p, &pixel) - }, + |p: VecF64<8>| KannalaBrandtDistortionImpl::::distort(p, pixel), params, EPS_F64, ); @@ -321,11 +328,11 @@ fn dyn_camera_tests() { // assert_relative_eq!(dx_params, numeric_dx_params, epsilon = 1e-4); } PerspectiveCameraEnum::UnifiedExtended(camera) => { - let dx_params = camera.dx_distort_params(&pixel); + let dx_params = camera.dx_distort_params(pixel); let params = camera.params(); let numeric_dx_params = VectorValuedMapFromVector::static_sym_diff_quotient( - |p: VecF64<6>| UnifiedDistortionImpl::::distort(&p, &pixel), + |p: VecF64<6>| UnifiedDistortionImpl::::distort(p, pixel), *params, EPS_F64, ); diff --git a/crates/sophus_sensor/src/lib.rs b/crates/sophus_sensor/src/lib.rs index 2b3d8893..c0b1ccbd 100644 --- a/crates/sophus_sensor/src/lib.rs +++ b/crates/sophus_sensor/src/lib.rs @@ -32,8 +32,8 @@ pub mod traits; /// sophus sensor prelude pub mod prelude { - pub use crate::traits::IsCameraEnum; - pub use crate::traits::IsPerspectiveCameraEnum; + pub use crate::traits::IsCamera; + pub use crate::traits::IsPerspectiveCamera; pub use crate::traits::IsProjection; pub use sophus_core::prelude::*; } diff --git a/crates/sophus_sensor/src/projections/orthographic.rs b/crates/sophus_sensor/src/projections/orthographic.rs index 9667fd5c..09d52f0c 100644 --- a/crates/sophus_sensor/src/projections/orthographic.rs +++ b/crates/sophus_sensor/src/projections/orthographic.rs @@ -1,6 +1,7 @@ use crate::camera::Camera; 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; @@ -14,11 +15,18 @@ pub struct OrthographisProjectionImpl, const BATCH: usize> { impl, const BATCH: usize> IsProjection for OrthographisProjectionImpl { - fn proj(point_in_camera: &S::Vector<3>) -> S::Vector<2> { - point_in_camera.get_fixed_subvec::<2>(0) + fn proj

(point_in_camera: P) -> S::Vector<2> + where + P: Borrow>, + { + point_in_camera.borrow().get_fixed_subvec::<2>(0) } - fn unproj(point_in_camera: &S::Vector<2>, extension: S) -> S::Vector<3> { + fn unproj

(point_in_camera: P, extension: S) -> S::Vector<3> + where + P: Borrow>, + { + let point_in_camera = point_in_camera.borrow(); S::Vector::<3>::from_array([ point_in_camera.get_elem(0), point_in_camera.get_elem(1), @@ -26,7 +34,10 @@ impl, const BATCH: usize> IsProjection ]) } - fn dx_proj_x(_point_in_camera: &S::Vector<3>) -> S::Matrix<2, 3> { + fn dx_proj_x

(_point_in_camera: P) -> S::Matrix<2, 3> + where + P: Borrow>, + { unimplemented!("dx_proj_x not implemented for ProjectionOrtho") } } diff --git a/crates/sophus_sensor/src/projections/perspective.rs b/crates/sophus_sensor/src/projections/perspective.rs index 97df136d..9cb5298a 100644 --- a/crates/sophus_sensor/src/projections/perspective.rs +++ b/crates/sophus_sensor/src/projections/perspective.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use crate::traits::IsProjection; use sophus_core::linalg::matrix::IsMatrix; use sophus_core::linalg::scalar::IsScalar; @@ -15,14 +17,23 @@ pub struct PerspectiveProjectionImpl, const BATCH: usize> { impl, const BATCH: usize> IsProjection for PerspectiveProjectionImpl { - fn proj(point_in_camera: &S::Vector<3>) -> S::Vector<2> { + fn proj

(point_in_camera: P) -> S::Vector<2> + where + P: Borrow>, + { + let point_in_camera = point_in_camera.borrow(); S::Vector::<2>::from_array([ point_in_camera.get_elem(0) / point_in_camera.get_elem(2), point_in_camera.get_elem(1) / point_in_camera.get_elem(2), ]) } - fn unproj(point_in_camera: &S::Vector<2>, extension: S) -> S::Vector<3> { + fn unproj

(point_in_camera: P, extension: S) -> S::Vector<3> + where + P: Borrow>, + { + let point_in_camera = point_in_camera.borrow(); + S::Vector::<3>::from_array([ point_in_camera.get_elem(0) * extension.clone(), point_in_camera.get_elem(1) * extension.clone(), @@ -30,7 +41,12 @@ impl, const BATCH: usize> IsProjection ]) } - fn dx_proj_x(point_in_camera: &S::Vector<3>) -> S::Matrix<2, 3> { + fn dx_proj_x

(point_in_camera: P) -> S::Matrix<2, 3> + where + P: Borrow>, + { + let point_in_camera = point_in_camera.borrow(); + S::Matrix::<2, 3>::from_array2([ [ S::ones() / point_in_camera.get_elem(2), diff --git a/crates/sophus_sensor/src/traits.rs b/crates/sophus_sensor/src/traits.rs index 23a9c305..64e64a7a 100644 --- a/crates/sophus_sensor/src/traits.rs +++ b/crates/sophus_sensor/src/traits.rs @@ -1,3 +1,5 @@ +use core::borrow::Borrow; + use crate::camera_enum::perspective_camera::UnifiedCamera; use crate::prelude::*; use crate::BrownConradyCamera; @@ -23,63 +25,93 @@ pub trait IsCameraDistortionImpl< } /// Distortion - maps a point in the camera z=1 plane to a distorted point - fn distort( - params: &S::Vector, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Vector<2>; + fn distort(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>; /// Undistortion - maps a distorted pixel to a point in the camera z=1 plane - fn undistort(params: &S::Vector, distorted_point: &S::Vector<2>) -> S::Vector<2>; + fn undistort(params: PA, distorted_point: PO) -> S::Vector<2> + where + PA: Borrow>, + PO: Borrow>; /// Derivative of the distortion w.r.t. the point in the camera z=1 plane - fn dx_distort_x( - params: &S::Vector, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, 2>; + fn dx_distort_x(params: PA, proj_point_in_camera_z1_plane: PO) -> S::Matrix<2, 2> + where + PA: Borrow>, + PO: Borrow>; /// Derivative of the distortion w.r.t. the parameters - fn dx_distort_params( - params: &S::Vector, - proj_point_in_camera_z1_plane: &S::Vector<2>, - ) -> S::Matrix<2, PARAMS>; + fn dx_distort_params( + params: PA, + proj_point_in_camera_z1_plane: PO, + ) -> S::Matrix<2, PARAMS> + where + PA: Borrow>, + PO: Borrow>; } /// Camera projection implementation trait pub trait IsProjection, const BATCH: usize> { /// Projects a 3D point in the camera frame to a 2D point in the z=1 plane - fn proj(point_in_camera: &S::Vector<3>) -> S::Vector<2>; + fn proj

(point_in_camera: P) -> S::Vector<2> + where + P: Borrow>; /// Unprojects a 2D point in the z=1 plane to a 3D point in the camera frame - fn unproj(point_in_camera: &S::Vector<2>, extension: S) -> S::Vector<3>; + fn unproj

(point_in_camera: P, extension: S) -> S::Vector<3> + where + P: Borrow>; /// Derivative of the projection w.r.t. the point in the camera frame - fn dx_proj_x(point_in_camera: &S::Vector<3>) -> S::Matrix<2, 3>; + fn dx_proj_x

(point_in_camera: P) -> S::Matrix<2, 3> + where + P: Borrow>; } /// Camera trait -pub trait IsCameraEnum, const BATCH: usize> { +pub trait IsCamera, const BATCH: usize> { /// Creates a new pinhole camera - fn new_pinhole(params: &S::Vector<4>, image_size: ImageSize) -> Self; + fn new_pinhole

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>; /// Creates a new Kannala-Brandt camera - fn new_kannala_brandt(params: &S::Vector<8>, image_size: ImageSize) -> Self; + fn new_kannala_brandt

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>; /// Creates a new Brown-Conrady camera - fn new_brown_conrady(params: &S::Vector<12>, image_size: ImageSize) -> Self; + fn new_brown_conrady

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>; /// Creates a new Unified Extended camera - fn new_unified(params: &S::Vector<6>, image_size: ImageSize) -> Self; + fn new_unified

(params: P, image_size: ImageSize) -> Self + where + P: Borrow>; /// Returns the image size fn image_size(&self) -> ImageSize; /// Projects a 3D point in the camera frame to a pixel in the image - fn cam_proj(&self, point_in_camera: &S::Vector<3>) -> S::Vector<2>; + fn cam_proj

(&self, point_in_camera: P) -> S::Vector<2> + where + P: Borrow>; /// Unprojects a pixel in the image to a 3D point in the camera frame - fn cam_unproj_with_z(&self, pixel: &S::Vector<2>, z: S) -> S::Vector<3>; + fn cam_unproj_with_z

(&self, pixel: P, z: S) -> S::Vector<3> + where + P: Borrow>; /// Distortion - maps a point in the camera z=1 plane to a distorted point - fn distort(&self, proj_point_in_camera_z1_plane: &S::Vector<2>) -> S::Vector<2>; + fn distort

(&self, proj_point_in_camera_z1_plane: P) -> S::Vector<2> + where + P: Borrow>; /// Undistortion - maps a distorted pixel to a point in the camera z=1 plane - fn undistort(&self, pixel: &S::Vector<2>) -> S::Vector<2>; + fn undistort

(&self, pixel: P) -> S::Vector<2> + where + P: Borrow>; /// Derivative of the distortion w.r.t. the point in the camera z=1 plane - fn dx_distort_x(&self, proj_point_in_camera_z1_plane: &S::Vector<2>) -> S::Matrix<2, 2>; + fn dx_distort_x

(&self, proj_point_in_camera_z1_plane: P) -> S::Matrix<2, 2> + where + P: Borrow>; /// Returns the brown-conrady camera fn try_get_brown_conrady(self) -> Option>; @@ -95,7 +127,7 @@ pub trait IsCameraEnum, const BATCH: usize> { } /// Dynamic camera trait -pub trait IsPerspectiveCameraEnum, const BATCH: usize> { +pub trait IsPerspectiveCamera, const BATCH: usize> { /// Return the first four parameters: fx, fy, cx, cy fn pinhole_params(&self) -> S::Vector<4>; } diff --git a/crates/sophus_viewer/src/interactions/inplane_interaction.rs b/crates/sophus_viewer/src/interactions/inplane_interaction.rs index b58d3928..26e00b7a 100644 --- a/crates/sophus_viewer/src/interactions/inplane_interaction.rs +++ b/crates/sophus_viewer/src/interactions/inplane_interaction.rs @@ -102,7 +102,7 @@ impl InplaneInteraction { /// scene_from_camera - always the identity pub fn scene_from_camera(&self) -> Isometry3F64 { - Isometry3::from_translation(&VecF64::<3>::new(0.0, 0.0, 0.0)) + Isometry3::from_translation(VecF64::<3>::new(0.0, 0.0, 0.0)) } /// zoom diff --git a/crates/sophus_viewer/src/interactions/orbit_interaction.rs b/crates/sophus_viewer/src/interactions/orbit_interaction.rs index 645c7645..2826cd48 100644 --- a/crates/sophus_viewer/src/interactions/orbit_interaction.rs +++ b/crates/sophus_viewer/src/interactions/orbit_interaction.rs @@ -137,16 +137,16 @@ impl OrbitalInteraction { let scene_from_camera = self.scene_from_camera; let camera_in_scene = scene_from_camera.translation(); let zoom: f64 = (0.002 * smooth_scroll_delta.y) as f64; - let focus_point_in_scene = scene_from_camera.transform(&focus_point_in_camera); + let focus_point_in_scene = scene_from_camera.transform(focus_point_in_camera); let camera_to_focus_point_vec_in_scene = focus_point_in_scene - camera_in_scene; let new_camera_in_scene = camera_in_scene + camera_to_focus_point_vec_in_scene * zoom; let mut new_scene_from_camera = self.scene_from_camera; - new_scene_from_camera.set_translation(&new_camera_in_scene); + new_scene_from_camera.set_translation(new_camera_in_scene); let focus_point_in_camera = new_scene_from_camera .inverse() - .transform(&focus_point_in_scene); + .transform(focus_point_in_scene); let z_in_camera = focus_point_in_camera[2]; let ndc_z = self.clipping_planes.ndc_z_from_metric_z(z_in_camera); @@ -164,13 +164,12 @@ impl OrbitalInteraction { if smooth_scroll_delta.x != 0.0 { let delta_z: f64 = (smooth_scroll_delta.x) as f64; let delta = 0.002 * VecF64::<6>::new(0.0, 0.0, 0.0, 0.0, 0.0, delta_z); - let camera_from_scene_point = Isometry3::from_translation(&focus_point_in_camera); + let camera_from_scene_point = Isometry3::from_translation(focus_point_in_camera); - self.scene_from_camera = - self.scene_from_camera - .group_mul(&camera_from_scene_point.group_mul( - &Isometry3::exp(&delta).group_mul(&camera_from_scene_point.inverse()), - )); + self.scene_from_camera = self.scene_from_camera + * camera_from_scene_point + * Isometry3::exp(delta) + * camera_from_scene_point.inverse(); } } @@ -244,12 +243,11 @@ impl OrbitalInteraction { let delta = 0.01 * VecF64::<6>::new(0.0, 0.0, 0.0, -delta_y as f64, delta_x as f64, 0.0); let camera_from_scene_point = - Isometry3::from_translation(&cam.cam_unproj_with_z(&pixel, depth)); - self.scene_from_camera = - self.scene_from_camera - .group_mul(&camera_from_scene_point.group_mul( - &Isometry3::exp(&delta).group_mul(&camera_from_scene_point.inverse()), - )); + Isometry3::from_translation(cam.cam_unproj_with_z(&pixel, depth)); + self.scene_from_camera = self.scene_from_camera + * camera_from_scene_point + * Isometry3::exp(delta) + * camera_from_scene_point.inverse(); } else if response.dragged_by(egui::PointerButton::Primary) { // translate scene @@ -268,9 +266,8 @@ impl OrbitalInteraction { ); let mut scene_from_camera = self.scene_from_camera; let delta = p0 - p1; - let translation_update = scene_from_camera.factor().transform(&delta); - scene_from_camera - .set_translation(&(scene_from_camera.translation() + translation_update)); + let translation_update = scene_from_camera.factor().transform(delta); + scene_from_camera.set_translation(scene_from_camera.translation() + translation_update); self.scene_from_camera = scene_from_camera; if let Some(focus) = &mut self.maybe_scene_focus { diff --git a/justfile b/justfile index ab4feee7..08c4b787 100644 --- a/justfile +++ b/justfile @@ -13,8 +13,11 @@ build-std: build-simd: cargo +nightly build --release --all-targets --features simd +test-simd: + cargo +nightly test --release --features simd + test: - cargo +nightly test --release + cargo test --release format: pre-commit run -a