dx_proj_onto_plane_at_0

crates/sophus_geo/src/hyperplane.rs

@@ -1,9 +1,12 @@
 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::Isometry3;
+use sophus_lie::Isometry3F64;
 
 /// N-dimensional Hyperplane.
 pub struct HyperPlane<S: IsScalar<BATCH>, const DOF: usize, const DIM: usize, const BATCH: usize> {
@@ -87,6 +90,34 @@ impl<S: IsScalar<BATCH>, const BATCH: usize> Plane<S, BATCH> {
     }
 }
 
+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[:,2], plane origin o = T[:,3].
+    pub fn dx_proj_onto_plane_at_0(
+        parent_from_plane_origin: &Isometry3F64,
+        point: &VecF64<3>,
+    ) -> MatF64<3, 6> {
+        // dx (p - n * dot(p-o, n)) |x=0, T' = exp(x) * T, n = T[:,2], o = T[:,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(2);
+
+        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(3); // partial origin col wrt x => (3x6)
+        let dn_dx: MatF64<3, 6> = parent_from_plane_origin.dx_exp_x_time_matrix_at_0(2); // partial normal col wrt x => (3x6)
+
+        df_dplane_o * do_dx + df_dplane_n * dn_dx
+    }
+}
+
 #[test]
 fn plane_test() {
     use sophus_core::calculus::dual::DualScalar;
@@ -101,7 +132,12 @@ fn plane_test() {
         plane.proj_onto(v)
     }
 
-    let a = VecF64::<3>::new(1.0, 2.0, 3.0);
+    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,
@@ -114,4 +150,27 @@ fn plane_test() {
 
     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
+        );
+    }
 }

crates/sophus_lie/src/lie_group/real_lie_group.rs

@@ -122,6 +122,15 @@ where
     pub fn dparams_matrix(&self, col_idx: u8) -> S::Matrix<POINT, PARAMS> {
         G::dparams_matrix(&self.params, col_idx)
     }
+
+    /// derivative of matrix representation: (exp(x) * T)[:,i] at x=0
+    ///
+    /// precondition: column index in [0, AMBIENT-1]
+    pub fn dx_exp_x_time_matrix_at_0(&self, col_idx: u8) -> 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,22 +569,19 @@ 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());
 
@@ -584,6 +590,24 @@ macro_rules! def_real_group_test_template {
                             foo_from_bar.dparams_matrix(i as u8),
                             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 as u8),
+                                epsilon = 0.0001,
+                            );
+                        }
                     }
                 }