Skip to content

Commit

Permalink
plane df
Browse files Browse the repository at this point in the history
  • Loading branch information
@strasdat
strasdat committed Dec 22, 2024
1 parent 1b97095 commit 3f03792
Show file tree
Hide file tree
Showing 6 changed files with 248 additions and 1 deletion.
61 changes: 60 additions & 1 deletion crates/sophus_geo/src/hyperplane.rs
View file
Original file line number Diff line number Diff line change
@@ -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> {
Expand Down Expand Up @@ -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;
Expand All @@ -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,
Expand All @@ -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
);
}
}
8 changes: 8 additions & 0 deletions crates/sophus_lie/src/groups/rotation2.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -197,6 +197,14 @@ impl<S: IsRealScalar<BATCH_SIZE>, const BATCH_SIZE: usize>
.abs()
.less_equal(&S::from_f64(EPS_F64))
}

fn dparams_matrix(_params: &<S>::Vector<2>, col_idx: u8) -> <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]]),
_ => panic!("Invalid column index: {}", col_idx),
}
}
}

/// 2d rotation group - SO(2)
Expand Down
93 changes: 93 additions & 0 deletions crates/sophus_lie/src/groups/rotation3.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -519,6 +519,99 @@ impl<S: IsRealScalar<BATCH>, const BATCH: usize> IsRealLieGroupImpl<S, 3, 4, 3,
.abs()
.less_equal(&S::from_f64(EPS_F64.sqrt()))
}

fn dparams_matrix(params: &<S>::Vector<4>, col_idx: u8) -> <S>::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<S: IsScalar<BATCH>, const BATCH: usize> crate::traits::IsLieFactorGroupImpl<S, 3, 4, 3, BATCH>
Expand Down
13 changes: 13 additions & 0 deletions crates/sophus_lie/src/groups/translation_product_product.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -472,6 +472,19 @@ impl<
fn has_shortest_path_ambiguity(params: &<S>::Vector<PARAMS>) -> <S>::Mask {
Factor::has_shortest_path_ambiguity(&Self::factor_params(params))
}

fn dparams_matrix(params: &<S>::Vector<PARAMS>, col_idx: u8) -> <S>::Matrix<POINT, PARAMS> {
let factor_params = &Self::factor_params(params);

if col_idx < POINT as u8 {
S::Matrix::block_mat1x2::<POINT, SPARAMS>(
S::Matrix::zeros(),
Factor::dparams_matrix(factor_params, col_idx),
)
} else {
S::Matrix::block_mat1x2::<POINT, SPARAMS>(S::Matrix::identity(), S::Matrix::zeros())
}
}
}

impl<
Expand Down
69 changes: 69 additions & 0 deletions crates/sophus_lie/src/lie_group/real_lie_group.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<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<
Expand All @@ -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();
}

Expand All @@ -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();

Expand Down Expand Up @@ -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<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),
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,
);
}
}
}

}


fn interpolation_test() {
let group_examples: alloc::vec::Vec<_> = Self::element_examples();
Expand Down
5 changes: 5 additions & 0 deletions crates/sophus_lie/src/traits.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<POINT>) -> S::Matrix<POINT, DOF>;

/// 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>;

/// are there multiple shortest paths to the identity?
fn has_shortest_path_ambiguity(params: &S::Vector<PARAMS>) -> S::Mask;
}
Expand Down

0 comments on commit 3f03792

Please sign in to comment.