Skip to content

Commit

Permalink
dx_proj_onto_plane_at_0
Browse files Browse the repository at this point in the history
  • Loading branch information
strasdat committed Dec 22, 2024
1 parent 260c48c commit 2b23a53
Show file tree
Hide file tree
Showing 6 changed files with 177 additions and 29 deletions.
160 changes: 142 additions & 18 deletions crates/sophus_geo/src/hyperplane.rs
Original file line number Diff line number Diff line change
@@ -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> {
Expand Down Expand Up @@ -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),
),
}
}
Expand All @@ -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[:,1], plane origin o = T[:,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[:,1], o = T[:,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[:,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> {
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[:,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(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
);
}
}
2 changes: 1 addition & 1 deletion crates/sophus_lie/src/groups/rotation2.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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]]),
Expand Down
2 changes: 1 addition & 1 deletion crates/sophus_lie/src/groups/rotation3.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions crates/sophus_lie/src/groups/translation_product_product.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down
36 changes: 30 additions & 6 deletions crates/sophus_lie/src/lie_group/real_lie_group.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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)[:,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<
Expand Down Expand Up @@ -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,
);
}
}
}

Expand Down
2 changes: 1 addition & 1 deletion crates/sophus_lie/src/traits.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 2b23a53

Please sign in to comment.