From 3f037925a6c4796d1333a5c00284e9ba60a1c064 Mon Sep 17 00:00:00 2001 From: Hauke Strasdat Date: Sun, 22 Dec 2024 12:44:41 -0800 Subject: [PATCH] plane df --- crates/sophus_geo/src/hyperplane.rs | 61 +++++++++++- crates/sophus_lie/src/groups/rotation2.rs | 8 ++ crates/sophus_lie/src/groups/rotation3.rs | 93 +++++++++++++++++++ .../src/groups/translation_product_product.rs | 13 +++ .../src/lie_group/real_lie_group.rs | 69 ++++++++++++++ crates/sophus_lie/src/traits.rs | 5 + 6 files changed, 248 insertions(+), 1 deletion(-) diff --git a/crates/sophus_geo/src/hyperplane.rs b/crates/sophus_geo/src/hyperplane.rs index 94a333a..3959733 100644 --- a/crates/sophus_geo/src/hyperplane.rs +++ b/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, const DOF: usize, const DIM: usize, const BATCH: usize> { @@ -87,6 +90,34 @@ impl, const BATCH: usize> Plane { } } +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, + > = VecF64::<3>::new(1.0, 2.0, 3.0); let finite_diff = VectorValuedMapFromVector::::static_sym_diff_quotient( proj_x_onto::, 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, const BATCH: usize>(v: S::Vector<6>) -> S::Vector<3> { + let plane = Plane::::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::::static_fw_autodiff( + proj_x_onto::, + 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 + ); + } } diff --git a/crates/sophus_lie/src/groups/rotation2.rs b/crates/sophus_lie/src/groups/rotation2.rs index 0f25d9a..597ff34 100644 --- a/crates/sophus_lie/src/groups/rotation2.rs +++ b/crates/sophus_lie/src/groups/rotation2.rs @@ -197,6 +197,14 @@ impl, const BATCH_SIZE: usize> .abs() .less_equal(&S::from_f64(EPS_F64)) } + + fn dparams_matrix(_params: &::Vector<2>, col_idx: u8) -> ::Matrix<2, 2> { + match col_idx { + 0 => S::Matrix::identity(), + 1 => S::Matrix::from_f64_array2([[0.0, -1.0], [1.0, 0.0]]), + _ => panic!("Invalid column index: {}", col_idx), + } + } } /// 2d rotation group - SO(2) diff --git a/crates/sophus_lie/src/groups/rotation3.rs b/crates/sophus_lie/src/groups/rotation3.rs index 1169a2c..66d7276 100644 --- a/crates/sophus_lie/src/groups/rotation3.rs +++ b/crates/sophus_lie/src/groups/rotation3.rs @@ -519,6 +519,99 @@ impl, const BATCH: usize> IsRealLieGroupImpl::Vector<4>, col_idx: u8) -> ::Matrix<3, 4> { + let re = params.get_elem(0); + let i = params.get_elem(1); + let j = params.get_elem(2); + let k = params.get_elem(3); + + // helper lambda: + let scaled = |val: S, factor: f64| -> S { val * S::from_f64(factor) }; + + match col_idx { + // -------------------------------------------------- + // col_x + // + // partial wrt re => (0, 2k, -2j) + // partial wrt i => (0, 2j, 2k) + // partial wrt j => (-4j, 2i, -2re) + // partial wrt k => (-4k, 2re, 2i) + 0 => S::Matrix::from_array2([ + [S::zero(), S::zero(), scaled(j, -4.0), scaled(k, -4.0)], + [ + scaled(k, 2.0), + scaled(j, 2.0), + scaled(i, 2.0), + scaled(re, 2.0), + ], + [ + scaled(j, -2.0), + scaled(k, 2.0), + scaled(re, -2.0), + scaled(i, 2.0), + ], + ]), + + // -------------------------------------------------- + // col_y + // + // partial wrt re => (-2k, 0, 2i) + // partial wrt i => (2j, -4i, 2re) + // partial wrt j => (2i, 0, 2k) + // partial wrt k => (-2re, -4k, 2j) + 1 => S::Matrix::from_array2([ + [ + scaled(k, -2.0), // row0, partial wrt re + scaled(j, 2.0), // row0, partial wrt i + scaled(i, 2.0), // row0, partial wrt j + scaled(re, -2.0), // row0, partial wrt k + ], + [ + S::zero(), // row1, partial wrt re + scaled(i, -4.0), // row1, partial wrt i + S::zero(), // row1, partial wrt j + scaled(k, -4.0), // row1, partial wrt k + ], + [ + scaled(i, 2.0), // row2, partial wrt re + scaled(re, 2.0), // row2, partial wrt i + scaled(k, 2.0), // row2, partial wrt j + scaled(j, 2.0), // row2, partial wrt k + ], + ]), + + // -------------------------------------------------- + // col_z + // + // partial wrt re => ( 2j, -2i, 0 ) + // partial wrt i => ( 2k, -2re, -4i ) + // partial wrt j => ( 2re, 2k, -4j ) + // partial wrt k => ( 2i, 2j, 0 ) + 2 => S::Matrix::from_array2([ + [ + scaled(j, 2.0), // row0 wrt re + scaled(k, 2.0), // row0 wrt i + scaled(re, 2.0), // row0 wrt j + scaled(i, 2.0), // row0 wrt k + ], + [ + scaled(i, -2.0), // row1 wrt re + scaled(re, -2.0), // row1 wrt i + scaled(k, 2.0), // row1 wrt j + scaled(j, 2.0), // row1 wrt k + ], + [ + S::zero(), // row2 wrt re + scaled(i, -4.0), // row2 wrt i + scaled(j, -4.0), // row2 wrt j + S::zero(), // row2 wrt k + ], + ]), + + _ => panic!("Invalid column index: {}", col_idx), + } + } } impl, const BATCH: usize> crate::traits::IsLieFactorGroupImpl diff --git a/crates/sophus_lie/src/groups/translation_product_product.rs b/crates/sophus_lie/src/groups/translation_product_product.rs index 48ec7a4..d6f64e3 100644 --- a/crates/sophus_lie/src/groups/translation_product_product.rs +++ b/crates/sophus_lie/src/groups/translation_product_product.rs @@ -472,6 +472,19 @@ impl< fn has_shortest_path_ambiguity(params: &::Vector) -> ::Mask { Factor::has_shortest_path_ambiguity(&Self::factor_params(params)) } + + fn dparams_matrix(params: &::Vector, col_idx: u8) -> ::Matrix { + let factor_params = &Self::factor_params(params); + + if col_idx < POINT as u8 { + S::Matrix::block_mat1x2::( + S::Matrix::zeros(), + Factor::dparams_matrix(factor_params, col_idx), + ) + } else { + S::Matrix::block_mat1x2::(S::Matrix::identity(), S::Matrix::zeros()) + } + } } impl< diff --git a/crates/sophus_lie/src/lie_group/real_lie_group.rs b/crates/sophus_lie/src/lie_group/real_lie_group.rs index fbb0dba..60ce290 100644 --- a/crates/sophus_lie/src/lie_group/real_lie_group.rs +++ b/crates/sophus_lie/src/lie_group/real_lie_group.rs @@ -115,6 +115,22 @@ where let b = b.borrow(); G::db_a_mul_b(a.params(), b.params()) } + + /// 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 { + 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 { + self.dparams_matrix(col_idx) + .mat_mul(Self::da_a_mul_b(Self::identity(), self)) + .mat_mul(&Self::dx_exp_x_at_0()) + } } impl< @@ -140,6 +156,7 @@ pub trait RealLieGroupTest { Self::exp_log_jacobians_tests(); Self::hat_jacobians_tests(); Self::mul_jacobians_tests(); + Self::matrix_jacobians_tests(); Self::interpolation_test(); } @@ -149,6 +166,9 @@ pub trait RealLieGroupTest { /// Test group multiplication jacobians. fn mul_jacobians_tests(); + /// Test matrix jacobians. + fn matrix_jacobians_tests(); + /// Test adjoint jacobian. fn adjoint_jacobian_tests(); @@ -544,6 +564,55 @@ macro_rules! def_real_group_test_template { } } + fn matrix_jacobians_tests() { + use crate::traits::IsLieGroup; + 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, + | -> <$dual_scalar as IsScalar<$batch>>::Vector { + 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), + epsilon = 0.0001, + ); + + { + let dual_fn = | + v: <$dual_scalar as IsScalar<$batch>>::Vector, + | -> <$dual_scalar as IsScalar<$batch>>::Vector { + 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, + ); + } + } + } + + } + fn interpolation_test() { let group_examples: alloc::vec::Vec<_> = Self::element_examples(); diff --git a/crates/sophus_lie/src/traits.rs b/crates/sophus_lie/src/traits.rs index 41c4ece..08c45f7 100644 --- a/crates/sophus_lie/src/traits.rs +++ b/crates/sophus_lie/src/traits.rs @@ -144,6 +144,11 @@ pub trait IsRealLieGroupImpl< /// derivative of exponential map times a point at the identity fn dx_exp_x_times_point_at_0(point: &S::Vector) -> S::Matrix; + /// derivative of matrix representation with respect to the internal parameters + /// + /// precondition: column index in [0, AMBIENT-1] + fn dparams_matrix(params: &S::Vector, col_idx: u8) -> S::Matrix; + /// are there multiple shortest paths to the identity? fn has_shortest_path_ambiguity(params: &S::Vector) -> S::Mask; }