From 7dc93ce78d60243babd186068bb8d2d529bf1edd Mon Sep 17 00:00:00 2001 From: strasdat Date: Sun, 22 Dec 2024 14:32:38 -0800 Subject: [PATCH] dx_proj_onto_plane_at_0 --- crates/sophus_geo/src/hyperplane.rs | 160 ++++++++++++++++-- crates/sophus_lie/src/groups/rotation2.rs | 2 +- crates/sophus_lie/src/groups/rotation3.rs | 2 +- .../src/groups/translation_product_product.rs | 4 +- .../src/lie_group/real_lie_group.rs | 36 +++- crates/sophus_lie/src/traits.rs | 2 +- 6 files changed, 177 insertions(+), 29 deletions(-) diff --git a/crates/sophus_geo/src/hyperplane.rs b/crates/sophus_geo/src/hyperplane.rs index 94a333a..e93ad42 100644 --- a/crates/sophus_geo/src/hyperplane.rs +++ b/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, const DOF: usize, const DIM: usize, const BATCH: usize> { @@ -64,7 +68,7 @@ impl, const BATCH: usize> Line { 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, const BATCH: usize> Plane { } } +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::::from_isometry3(Isometry3::rot_y(0.2)); + + fn proj_x_onto, const BATCH: usize>(v: S::Vector<3>) -> S::Vector<3> { + let plane = Plane::::from_isometry3(Isometry3::rot_y(S::from_f64(0.2))); + plane.proj_onto(v) + } - let plane = Plane::::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, + > = VecF64::<3>::new(1.0, 2.0, 3.0); + let finite_diff = VectorValuedMapFromVector::::static_sym_diff_quotient( + proj_x_onto::, + a, + EPS_F64, + ); + let auto_grad = VectorValuedMapFromVector::::static_fw_autodiff( + proj_x_onto::, + a, + ); - fn proj_x_onto, const BATCH: usize>(v: S::Vector<3>) -> S::Vector<3> { - let plane = Plane::::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, 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 + ); + } + + 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<3>) -> S::Vector<2> { + let line = Line::::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::::static_fw_autodiff( + proj_x_onto::, + VecF64::zeros(), + ); - let a = VecF64::<3>::new(1.0, 2.0, 3.0); - let finite_diff = VectorValuedMapFromVector::::static_sym_diff_quotient( - proj_x_onto::, - a, - EPS_F64, - ); - let auto_grad = VectorValuedMapFromVector::::static_fw_autodiff( - proj_x_onto::, - 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 + ); + } } diff --git a/crates/sophus_lie/src/groups/rotation2.rs b/crates/sophus_lie/src/groups/rotation2.rs index 597ff34..d17d5c0 100644 --- a/crates/sophus_lie/src/groups/rotation2.rs +++ b/crates/sophus_lie/src/groups/rotation2.rs @@ -198,7 +198,7 @@ impl, const BATCH_SIZE: usize> .less_equal(&S::from_f64(EPS_F64)) } - fn dparams_matrix(_params: &::Vector<2>, col_idx: u8) -> ::Matrix<2, 2> { + fn dparams_matrix(_params: &::Vector<2>, col_idx: usize) -> ::Matrix<2, 2> { match col_idx { 0 => S::Matrix::identity(), 1 => S::Matrix::from_f64_array2([[0.0, -1.0], [1.0, 0.0]]), diff --git a/crates/sophus_lie/src/groups/rotation3.rs b/crates/sophus_lie/src/groups/rotation3.rs index 66d7276..779a4ab 100644 --- a/crates/sophus_lie/src/groups/rotation3.rs +++ b/crates/sophus_lie/src/groups/rotation3.rs @@ -520,7 +520,7 @@ impl, const BATCH: usize> IsRealLieGroupImpl::Vector<4>, col_idx: u8) -> ::Matrix<3, 4> { + fn dparams_matrix(params: &::Vector<4>, col_idx: usize) -> ::Matrix<3, 4> { let re = params.get_elem(0); let i = params.get_elem(1); let j = params.get_elem(2); diff --git a/crates/sophus_lie/src/groups/translation_product_product.rs b/crates/sophus_lie/src/groups/translation_product_product.rs index d6f64e3..263024e 100644 --- a/crates/sophus_lie/src/groups/translation_product_product.rs +++ b/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: &::Vector, col_idx: u8) -> ::Matrix { + fn dparams_matrix(params: &::Vector, col_idx: usize) -> ::Matrix { let factor_params = &Self::factor_params(params); - if col_idx < POINT as u8 { + if col_idx < POINT { S::Matrix::block_mat1x2::( S::Matrix::zeros(), Factor::dparams_matrix(factor_params, col_idx), 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 0b92643..36ac3d1 100644 --- a/crates/sophus_lie/src/lie_group/real_lie_group.rs +++ b/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 { + pub fn dparams_matrix(&self, col_idx: usize) -> S::Matrix { 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 { + 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, | -> <$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), + foo_from_bar.dparams_matrix(i), 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), + epsilon = 0.0001, + ); + } } } diff --git a/crates/sophus_lie/src/traits.rs b/crates/sophus_lie/src/traits.rs index 08c45f7..7c845ae 100644 --- a/crates/sophus_lie/src/traits.rs +++ b/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, col_idx: u8) -> S::Matrix; + fn dparams_matrix(params: &S::Vector, col_idx: usize) -> S::Matrix; /// are there multiple shortest paths to the identity? fn has_shortest_path_ambiguity(params: &S::Vector) -> S::Mask;