dx_proj_onto_plane_at_0

crates/sophus_geo/src/hyperplane.rs

@@ -1,9 +1,13 @@
 use std::borrow::Borrow;
 
+use sophus_core::linalg::MatF64;
+use sophus_core::linalg::VecF64;
 use sophus_core::unit_vector::UnitVector;
 use sophus_lie::prelude::*;
 use sophus_lie::Isometry2;
+use sophus_lie::Isometry2F64;
 use sophus_lie::Isometry3;
+use sophus_lie::Isometry3F64;
 
 /// N-dimensional Hyperplane.
 pub struct HyperPlane<S: IsScalar<BATCH>, const DOF: usize, const DIM: usize, const BATCH: usize> {
@@ -64,7 +68,7 @@ impl<S: IsScalar<BATCH>, const BATCH: usize> Line<S, BATCH> {
         Line {
             origin: parent_from_line_origin.translation(),
             normal: UnitVector::from_vector_and_normalize(
-                &parent_from_line_origin.rotation().matrix().get_col_vec(2),
+                &parent_from_line_origin.rotation().matrix().get_col_vec(1),
             ),
         }
     }
@@ -87,31 +91,151 @@ impl<S: IsScalar<BATCH>, const BATCH: usize> Plane<S, BATCH> {
     }
 }
 
+impl LineF64 {
+    /// Returns the Jacobian of `line.proj_onto(point)` w.r.t. the "line pose" at the identity.
+    ///
+    /// In more details, this is the Jacobian w.r.t. x, with x=0, T' = exp(x) * T,
+    /// plane normal n = T.col(1), plane origin o = T.col(2).
+    pub fn dx_proj_onto_line_at_0(
+        parent_from_plane_origin: &Isometry2F64,
+        point: &VecF64<2>,
+    ) -> MatF64<2, 3> {
+        const NORMAL_IDX: usize = 1;
+        const TRANSLATION_IDX: usize = 2;
+
+        // dx (p - n * dot(p-o, n)) |x=0, T' = exp(x) * T, n = T.col(1), o = T.col(2)
+        //  = dx [-n * dot(p-o, n)]
+
+        let o0 = parent_from_plane_origin
+            .compact()
+            .get_col_vec(TRANSLATION_IDX);
+        let n0 = parent_from_plane_origin.compact().get_col_vec(NORMAL_IDX);
+
+        let p_minus_o0 = point.clone() - o0.clone();
+        let alpha = p_minus_o0.dot(&n0);
+
+        let df_dplane_o = n0.outer(n0);
+        let df_dplane_n = -(MatF64::<2, 2>::identity().scaled(alpha) + n0.outer(p_minus_o0));
+
+        let do_dx: MatF64<2, 3> =
+            parent_from_plane_origin.dx_exp_x_time_matrix_at_0(TRANSLATION_IDX);
+        let dn_dx: MatF64<2, 3> = parent_from_plane_origin.dx_exp_x_time_matrix_at_0(NORMAL_IDX);
+
+        df_dplane_o * do_dx + df_dplane_n * dn_dx
+    }
+}
+
+impl PlaneF64 {
+    /// Returns the Jacobian of `plane.proj_onto(point)` w.r.t. the "plane pose" at the identity.
+    ///
+    /// In more details, this is the Jacobian w.r.t. x, with x=0, T' = exp(x) * T,
+    /// plane normal n = T.col(2), plane origin o = T.col(3).
+    pub fn dx_proj_onto_plane_at_0(
+        parent_from_plane_origin: &Isometry3F64,
+        point: &VecF64<3>,
+    ) -> MatF64<3, 6> {
+        const NORMAL_IDX: usize = 2;
+        const TRANSLATION_IDX: usize = 3;
+
+        // dx (p - n * dot(p-o, n)) |x=0, T' = exp(x) * T, n = T.col(2), o = T.col(3)
+        //  = dx [-n * dot(p-o, n)]
+
+        let o0 = parent_from_plane_origin.compact().get_col_vec(3);
+        let n0 = parent_from_plane_origin.compact().get_col_vec(NORMAL_IDX);
+
+        let p_minus_o0 = point.clone() - o0.clone();
+        let alpha = p_minus_o0.dot(&n0);
+
+        let df_dplane_o = n0.outer(n0);
+        let df_dplane_n = -(MatF64::<3, 3>::identity().scaled(alpha) + n0.outer(p_minus_o0));
+
+        let do_dx: MatF64<3, 6> =
+            parent_from_plane_origin.dx_exp_x_time_matrix_at_0(TRANSLATION_IDX);
+        let dn_dx: MatF64<3, 6> = parent_from_plane_origin.dx_exp_x_time_matrix_at_0(NORMAL_IDX);
+
+        df_dplane_o * do_dx + df_dplane_n * dn_dx
+    }
+}
+
 #[test]
 fn plane_test() {
     use sophus_core::calculus::dual::DualScalar;
     use sophus_core::calculus::maps::VectorValuedMapFromVector;
     use sophus_core::linalg::VecF64;
     use sophus_core::linalg::EPS_F64;
+    {
+        let plane = Plane::<f64, 1>::from_isometry3(Isometry3::rot_y(0.2));
+
+        fn proj_x_onto<S: IsScalar<BATCH>, const BATCH: usize>(v: S::Vector<3>) -> S::Vector<3> {
+            let plane = Plane::<S, BATCH>::from_isometry3(Isometry3::rot_y(S::from_f64(0.2)));
+            plane.proj_onto(v)
+        }
 
-    let plane = Plane::<f64, 1>::from_isometry3(Isometry3::rot_y(0.2));
+        let a: sophus_core::nalgebra::Matrix<
+            f64,
+            sophus_core::nalgebra::Const<3>,
+            sophus_core::nalgebra::Const<1>,
+            sophus_core::nalgebra::ArrayStorage<f64, 3, 1>,
+        > = VecF64::<3>::new(1.0, 2.0, 3.0);
+        let finite_diff = VectorValuedMapFromVector::<f64, 1>::static_sym_diff_quotient(
+            proj_x_onto::<f64, 1>,
+            a,
+            EPS_F64,
+        );
+        let auto_grad = VectorValuedMapFromVector::<DualScalar, 1>::static_fw_autodiff(
+            proj_x_onto::<DualScalar, 1>,
+            a,
+        );
 
-    fn proj_x_onto<S: IsScalar<BATCH>, const BATCH: usize>(v: S::Vector<3>) -> S::Vector<3> {
-        let plane = Plane::<S, BATCH>::from_isometry3(Isometry3::rot_y(S::from_f64(0.2)));
-        plane.proj_onto(v)
+        approx::assert_abs_diff_eq!(finite_diff, auto_grad, epsilon = 0.00001);
+        approx::assert_abs_diff_eq!(plane.dx_proj_x_onto(), auto_grad, epsilon = 0.00001);
+        {
+            fn proj_x_onto<S: IsScalar<BATCH>, const BATCH: usize>(
+                v: S::Vector<6>,
+            ) -> S::Vector<3> {
+                let plane = Plane::<S, BATCH>::from_isometry3(
+                    Isometry3::exp(v) * Isometry3::rot_y(S::from_f64(0.2)),
+                );
+                let a = S::Vector::<3>::from_f64_array([1.0, 2.0, 3.0]);
+                plane.proj_onto(a)
+            }
+
+            let auto_grad = VectorValuedMapFromVector::<DualScalar, 1>::static_fw_autodiff(
+                proj_x_onto::<DualScalar, 1>,
+                VecF64::zeros(),
+            );
+
+            approx::assert_abs_diff_eq!(
+                Plane::dx_proj_onto_plane_at_0(
+                    &Isometry3::rot_y(0.2),
+                    &VecF64::<3>::new(1.0, 2.0, 3.0)
+                ),
+                auto_grad,
+                epsilon = 0.00001
+            );
+        }
+
+        approx::assert_abs_diff_eq!(finite_diff, auto_grad, epsilon = 0.00001);
+        approx::assert_abs_diff_eq!(plane.dx_proj_x_onto(), auto_grad, epsilon = 0.00001);
     }
+    {
+        fn proj_x_onto<S: IsScalar<BATCH>, const BATCH: usize>(v: S::Vector<3>) -> S::Vector<2> {
+            let line = Line::<S, BATCH>::from_isometry2(
+                Isometry2::exp(v) * Isometry2::rot(S::from_f64(0.2)),
+            );
+            let a = S::Vector::<2>::from_f64_array([1.0, 2.0]);
+            line.proj_onto(a)
+        }
+
+        let auto_grad = VectorValuedMapFromVector::<DualScalar, 1>::static_fw_autodiff(
+            proj_x_onto::<DualScalar, 1>,
+            VecF64::zeros(),
+        );
 
-    let a = VecF64::<3>::new(1.0, 2.0, 3.0);
-    let finite_diff = VectorValuedMapFromVector::<f64, 1>::static_sym_diff_quotient(
-        proj_x_onto::<f64, 1>,
-        a,
-        EPS_F64,
-    );
-    let auto_grad = VectorValuedMapFromVector::<DualScalar, 1>::static_fw_autodiff(
-        proj_x_onto::<DualScalar, 1>,
-        a,
-    );
-
-    approx::assert_abs_diff_eq!(finite_diff, auto_grad, epsilon = 0.00001);
-    approx::assert_abs_diff_eq!(plane.dx_proj_x_onto(), auto_grad, epsilon = 0.00001);
+        approx::assert_abs_diff_eq!(
+            Line::dx_proj_onto_line_at_0(&Isometry2::rot(0.2), &VecF64::<2>::new(1.0, 2.0)),
+            auto_grad,
+            epsilon = 0.00001
+        );
+    }
 }

crates/sophus_lie/src/groups/rotation2.rs

@@ -198,7 +198,7 @@ impl<S: IsRealScalar<BATCH_SIZE>, const BATCH_SIZE: usize>
             .less_equal(&S::from_f64(EPS_F64))
     }
 
-    fn dparams_matrix(_params: &<S>::Vector<2>, col_idx: u8) -> <S>::Matrix<2, 2> {
+    fn dparams_matrix(_params: &<S>::Vector<2>, col_idx: usize) -> <S>::Matrix<2, 2> {
         match col_idx {
             0 => S::Matrix::identity(),
             1 => S::Matrix::from_f64_array2([[0.0, -1.0], [1.0, 0.0]]),

crates/sophus_lie/src/groups/rotation3.rs

@@ -520,7 +520,7 @@ impl<S: IsRealScalar<BATCH>, const BATCH: usize> IsRealLieGroupImpl<S, 3, 4, 3,
             .less_equal(&S::from_f64(EPS_F64.sqrt()))
     }
 
-    fn dparams_matrix(params: &<S>::Vector<4>, col_idx: u8) -> <S>::Matrix<3, 4> {
+    fn dparams_matrix(params: &<S>::Vector<4>, col_idx: usize) -> <S>::Matrix<3, 4> {
         let re = params.get_elem(0);
         let i = params.get_elem(1);
         let j = params.get_elem(2);

crates/sophus_lie/src/groups/translation_product_product.rs

@@ -473,10 +473,10 @@ impl<
         Factor::has_shortest_path_ambiguity(&Self::factor_params(params))
     }
 
-    fn dparams_matrix(params: &<S>::Vector<PARAMS>, col_idx: u8) -> <S>::Matrix<POINT, PARAMS> {
+    fn dparams_matrix(params: &<S>::Vector<PARAMS>, col_idx: usize) -> <S>::Matrix<POINT, PARAMS> {
         let factor_params = &Self::factor_params(params);
 
-        if col_idx < POINT as u8 {
+        if col_idx < POINT {
             S::Matrix::block_mat1x2::<POINT, SPARAMS>(
                 S::Matrix::zeros(),
                 Factor::dparams_matrix(factor_params, col_idx),

crates/sophus_lie/src/lie_group/real_lie_group.rs

@@ -119,9 +119,18 @@ where
     /// derivative of matrix representation with respect to the internal parameters
     ///
     /// precondition: column index in [0, AMBIENT-1]
-    pub fn dparams_matrix(&self, col_idx: u8) -> S::Matrix<POINT, PARAMS> {
+    pub fn dparams_matrix(&self, col_idx: usize) -> S::Matrix<POINT, PARAMS> {
         G::dparams_matrix(&self.params, col_idx)
     }
+
+    /// derivative of matrix representation: (exp(x) * T).col(i) at x=0
+    ///
+    /// precondition: column index in [0, AMBIENT-1]
+    pub fn dx_exp_x_time_matrix_at_0(&self, col_idx: usize) -> S::Matrix<POINT, DOF> {
+        self.dparams_matrix(col_idx)
+            .mat_mul(Self::da_a_mul_b(Self::identity(), self))
+            .mat_mul(&Self::dx_exp_x_at_0())
+    }
 }
 
 impl<
@@ -560,30 +569,45 @@ macro_rules! def_real_group_test_template {
                 let group_examples: alloc::vec::Vec<_> = Self::element_examples();
                 const PARAMS: usize = <$group>::PARAMS;
                 const POINT: usize = <$group>::POINT;
+                const DOF: usize = <$group>::DOF;
                 const AMBIENT: usize = <$group>::AMBIENT;
 
                 for foo_from_bar in &group_examples {
-
                     for i in 0..AMBIENT{
 
-
                         let params =  foo_from_bar.params();
-
                         let  dual_fn = |
                             v: <$dual_scalar as IsScalar<$batch>>::Vector<PARAMS>,
                         | -> <$dual_scalar as IsScalar<$batch>>::Vector<POINT> {
                             let m =  <$dual_group>::from_params(&v).compact();
                             m.get_col_vec(i)
                         };
-
                         let auto_d = VectorValuedMapFromVector::<$dual_scalar, $batch>
                         ::static_fw_autodiff(dual_fn, params.clone());
 
                         approx::assert_relative_eq!(
                             auto_d,
-                            foo_from_bar.dparams_matrix(i as u8),
+                            foo_from_bar.dparams_matrix(i),
                             epsilon = 0.0001,
                         );
+
+                        {
+                            let  dual_fn = |
+                                v: <$dual_scalar as IsScalar<$batch>>::Vector<DOF>,
+                            | -> <$dual_scalar as IsScalar<$batch>>::Vector<POINT> {
+                                let m =   ( <$dual_group>::exp(v)* foo_from_bar.to_dual_c()).compact();
+                                m.get_col_vec(i)
+                            };
+                            let o = <$scalar as IsScalar<$batch>>::Vector::zeros();
+                            let auto_d = VectorValuedMapFromVector::<$dual_scalar, $batch>
+                            ::static_fw_autodiff(dual_fn,o);
+
+                            approx::assert_relative_eq!(
+                                auto_d,
+                                foo_from_bar.dx_exp_x_time_matrix_at_0(i),
+                                epsilon = 0.0001,
+                            );
+                        }
                     }
                 }
 

crates/sophus_lie/src/traits.rs

@@ -147,7 +147,7 @@ pub trait IsRealLieGroupImpl<
     /// derivative of matrix representation with respect to the internal parameters
     ///
     /// precondition: column index in [0, AMBIENT-1]
-    fn dparams_matrix(params: &S::Vector<PARAMS>, col_idx: u8) -> S::Matrix<POINT, PARAMS>;
+    fn dparams_matrix(params: &S::Vector<PARAMS>, col_idx: usize) -> S::Matrix<POINT, PARAMS>;
 
     /// are there multiple shortest paths to the identity?
     fn has_shortest_path_ambiguity(params: &S::Vector<PARAMS>) -> S::Mask;