mul operator for group_mul

sophus-vision · Dec 14, 2024 · 56f5269 · 56f5269
1 parent 11b2883
commit 56f5269
Show file tree

Hide file tree

Showing 23 changed files with 149 additions and 100 deletions.
diff --git a/crates/sophus_lie/src/groups/isometry2.rs b/crates/sophus_lie/src/groups/isometry2.rs
@@ -1,7 +1,7 @@
-use crate::average::iterative_average;
-use crate::average::IterativeAverageError;
 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;
@@ -98,7 +98,7 @@ impl<S: IsSingleScalar + PartialOrd> HasAverage<S, 3, 4, 2, 3> for Isometry2<S,
 
 #[test]
 fn isometry2_prop_tests() {
-    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")]

diff --git a/crates/sophus_lie/src/groups/isometry3.rs b/crates/sophus_lie/src/groups/isometry3.rs
@@ -2,8 +2,8 @@ use log::warn;
 
 use super::rotation3::Rotation3Impl;
 use super::translation_product_product::TranslationProductGroupImpl;
-use crate::average::iterative_average;
-use crate::average::IterativeAverageError;
+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;
@@ -110,7 +110,7 @@ impl<S: IsSingleScalar + PartialOrd> HasAverage<S, 6, 7, 3, 4> for Isometry3<S,
 
 #[test]
 fn isometry3_prop_tests() {
-    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")]

diff --git a/crates/sophus_lie/src/groups/rotation2.rs b/crates/sophus_lie/src/groups/rotation2.rs
@@ -328,21 +328,19 @@ impl<S: IsSingleScalar + PartialOrd> HasAverage<S, 1, 2, 2, 2> for Rotation2<S,
 
         for parent_from_body in parent_from_body_transforms {
             average_tangent = average_tangent
-                + parent_from_body0
-                    .inverse()
-                    .group_mul(parent_from_body)
+                + (parent_from_body0.inverse() * parent_from_body)
                     .log()
                     .scaled(w.clone());
         }
 
-        Ok(parent_from_body0.group_mul(&LieGroup::exp(&average_tangent)))
+        Ok(parent_from_body0 * LieGroup::exp(&average_tangent))
     }
 }
 
 #[test]
 fn rotation2_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")]

diff --git a/crates/sophus_lie/src/groups/rotation3.rs b/crates/sophus_lie/src/groups/rotation3.rs
@@ -1,5 +1,5 @@
-use crate::average::iterative_average;
-use crate::average::IterativeAverageError;
+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;
@@ -924,7 +924,7 @@ impl<S: IsSingleScalar + PartialOrd> Rotation3<S, 1> {
 #[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;

diff --git 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
@@ -7,6 +7,15 @@ 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<
@@ -149,7 +158,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
@@ -327,8 +336,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 +348,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 → crates/sophus_lie/src/lie_group/average.rs b/crates/sophus_lie/src/average.rs → 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
@@ -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<BATCH_SIZE>,
+        const DOF: usize,
+        const PARAMS: usize,
+        const POINT: usize,
+        const AMBIENT: usize,
+        const BATCH_SIZE: usize,
+        G: IsLieGroupImpl<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE>,
+    > Mul<LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>>
+    for LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>
+{
+    type Output = LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>;
+
+    fn mul(self, rhs: Self) -> Self::Output {
+        self.group_mul(&rhs)
+    }
+}
+
+// a * &b
+impl<
+        S: IsScalar<BATCH_SIZE>,
+        const DOF: usize,
+        const PARAMS: usize,
+        const POINT: usize,
+        const AMBIENT: usize,
+        const BATCH_SIZE: usize,
+        G: IsLieGroupImpl<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE>,
+    > Mul<&LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>>
+    for LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>
+{
+    type Output = LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>;
+
+    fn mul(self, rhs: &Self) -> Self::Output {
+        self.group_mul(rhs)
+    }
+}
+
+// &a * &b
+impl<
+        S: IsScalar<BATCH_SIZE>,
+        const DOF: usize,
+        const PARAMS: usize,
+        const POINT: usize,
+        const AMBIENT: usize,
+        const BATCH_SIZE: usize,
+        G: IsLieGroupImpl<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE>,
+    > Mul<LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>>
+    for &LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>
+{
+    type Output = LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>;
+
+    fn mul(self, rhs: LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>) -> Self::Output {
+        self.group_mul(&rhs)
+    }
+}
+
+// a * &b
+impl<
+        S: IsScalar<BATCH_SIZE>,
+        const DOF: usize,
+        const PARAMS: usize,
+        const POINT: usize,
+        const AMBIENT: usize,
+        const BATCH_SIZE: usize,
+        G: IsLieGroupImpl<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE>,
+    > Mul<&LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>>
+    for &LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>
+{
+    type Output = LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>;
+
+    fn mul(self, rhs: &LieGroup<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>) -> Self::Output {
+        self.group_mul(rhs)
+    }
+}
diff --git a/crates/sophus_lie/src/lie_group_manifold.rs → ...s_lie/src/lie_group/lie_group_manifold.rs b/crates/sophus_lie/src/lie_group_manifold.rs → ...s_lie/src/lie_group/lie_group_manifold.rs
@@ -86,12 +86,12 @@ impl<
 {
     fn oplus(&self, tangent: &<S as IsScalar<BATCH_SIZE>>::Vector<DOF>) -> Self {
         Self {
-            group: LieGroup::<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>::exp(tangent)
-                .group_mul(&self.group),
+            group: &LieGroup::<S, DOF, PARAMS, POINT, AMBIENT, BATCH_SIZE, G>::exp(tangent)
+                * &self.group,
         }
     }
 
     fn ominus(&self, rhs: &Self) -> <S as IsScalar<BATCH_SIZE>>::Vector<DOF> {
-        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 → ...ophus_lie/src/lie_group/real_lie_group.rs b/crates/sophus_lie/src/real_lie_group.rs → ...ophus_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;
@@ -71,7 +71,7 @@ where
 
     /// derivative of log(exp(x)) at the identity
     pub fn dx_log_a_exp_x_b_at_0(a: &Self, b: &Self) -> S::Matrix<DOF, DOF> {
-        let ab = a.group_mul(b);
+        let ab = a * b;
         Self::dx_log_x(ab.params())
             .mat_mul(Self::da_a_mul_b(&Self::identity(), &ab))
             .mat_mul(Self::dx_exp_x_at_0())
@@ -322,7 +322,7 @@ macro_rules! def_real_group_test_template {
                         let log_x = |t: <$scalar as IsScalar<$batch>>::Vector<DOF>|
                             -> <$scalar as IsScalar<$batch>>::Vector<DOF>
                             {
-                                Self::exp(&t).group_mul(&g).log()
+                                (Self::exp(&t) * g).log()
                             };
                         let o = <$scalar as IsScalar<$batch>>::Vector::zeros();
 
@@ -381,7 +381,7 @@ macro_rules! def_real_group_test_template {
                         let dual_log_x = |t: <$dual_scalar as IsScalar<$batch>>::Vector<DOF>|
                             -> <$dual_scalar as IsScalar<$batch>>::Vector<DOF>
                             {
-                                dual_a.group_mul(
+                                (&dual_a *
                                         &<$dual_group>::exp(&t)
                                         .group_mul(&dual_b)
                                     ).log()

diff --git a/crates/sophus_opt/src/example_problems/cam_calib.rs b/crates/sophus_opt/src/example_problems/cam_calib.rs
@@ -86,9 +86,8 @@ impl CamCalibProblem {
             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_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::<f64>() - 0.5, rng.gen::<f64>() - 0.5);
@@ -107,9 +106,8 @@ impl CamCalibProblem {
                 });
             }
 
-            let true_cam2_from_cam0 = true_world_from_cameras[2]
-                .inverse()
-                .group_mul(&true_world_from_cameras[0]);
+            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::<f64>() - 0.5, rng.gen::<f64>() - 0.5);

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
@@ -43,7 +43,7 @@ fn res_fn<Scalar: IsSingleScalar + IsScalar<1>>(
     isometry: Isometry2<Scalar, 1>,
     isometry_prior_mean: Isometry2<Scalar, 1>,
 ) -> Scalar::Vector<3> {
-    Isometry2::<Scalar, 1>::group_mul(&isometry, &isometry_prior_mean.inverse()).log()
+    (isometry * isometry_prior_mean.inverse()).log()
 }
 
 impl IsResidualFn<3, 1, (), Isometry2F64, Isometry2F64> for Isometry2PriorCostFn {
@@ -60,7 +60,7 @@ 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::<DualScalar, 1>::exp(&x).group_mul(&isometry.to_dual_c());
+            let pp = Isometry2::<DualScalar, 1>::exp(&x) * isometry.to_dual_c();
             res_fn(
                 pp,
                 Isometry2::from_params(&DualVector::from_real_vector(

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
@@ -44,7 +44,7 @@ fn res_fn<Scalar: IsScalar<1> + IsSingleScalar>(
     isometry: Isometry3<Scalar, 1>,
     isometry_prior_mean: Isometry3<Scalar, 1>,
 ) -> Scalar::Vector<6> {
-    Isometry3::<Scalar, 1>::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,7 +61,7 @@ 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::<DualScalar, 1>::exp(&x).group_mul(&isometry.to_dual_c());
+            let pp = Isometry3::<DualScalar, 1>::exp(&x) * isometry.to_dual_c();
             res_fn(
                 pp,
                 Isometry3::from_params(&DualVector::from_real_vector(*isometry_prior.0.params())),