Skip to content

Commit

Permalink
jacobians of exp_inv wrt argument on SE(2) and SE(3)
Browse files Browse the repository at this point in the history
  • Loading branch information
@mateuszbaran
mateuszbaran committed Nov 15, 2024
1 parent 7460f20 commit adaae1b
Show file tree
Hide file tree
Showing 3 changed files with 173 additions and 5 deletions.
12 changes: 12 additions & 0 deletions docs/src/references.bib
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,18 @@ @article{Bacak:2014
VOLUME = {24},
YEAR = {2014}
}
@article{BarfootFurgale:2014,
title = {Associating {Uncertainty} {With} {Three}-{Dimensional} {Poses} for {Use} in {Estimation} {Problems}},
volume = {30},
issn = {1941-0468},
doi = {10.1109/TRO.2014.2298059},
number = {3},
journal = {IEEE Transactions on Robotics},
author = {Barfoot, Timothy D. and Furgale, Paul T.},
month = jun,
year = {2014},
pages = {679--693},
}
@article{BendokatZimmermann:2021,
AUTHOR = {Bendokat, Thomas and Zimmermann, Ralf},
EPRINT = {2108.12447},
Expand Down
124 changes: 123 additions & 1 deletion src/groups/special_euclidean.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,9 +203,9 @@ R_{1,1} & R_{1,2} & t_2 \\
R_{2,1} & R_{2,2} & -t_1 \\
0 & 0 & 1
\end{pmatrix},
````
where ``R`` is the rotation matrix part of `p` and ``[t_1, t_2]`` is the translation part
of `p`.
````
"""
function adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{2}}}, p)
t, R = submanifold_components(p)
Expand Down Expand Up @@ -549,6 +549,128 @@ function exp_lie!(G::SpecialEuclidean{TypeParameter{Tuple{3}}}, q, X)
return q
end

@doc raw"""
jacobian_exp_inv_argument(
M::SpecialEuclidean{TypeParameter{Tuple{2}}},
p,
X,
)
Compute Jacobian matrix of the invariant exponential map on [`SpecialEuclidean`](@ref)`(2)`.
The formula reads
````math
\begin{pmatrix}
\frac{\sin(θ)}{\theta} & \frac{1-\cos(\theta)}{\theta} & \frac{t_1(\sin(θ) - θ) + t_2(\cos(θ) - 1)}{\sqrt{2} θ^2} \\
\frac{-1+\cos(\theta)}{\theta} & \frac{\sin(θ)}{\theta} & \frac{t_2(\sin(θ) - θ) + t_1(-\cos(θ) + 1)}{\sqrt{2} θ^2} \\
0 & 0 & 1
\end{pmatrix}.
````
where ``θ`` is the norm of `X` and ``[t_1, t_2]`` is the translation part
of `X`.
It is adapted from [Chirikjian:2012](@cite), Section 10.6.2, to `Manifolds.jl` conventions.
"""
jacobian_exp_inv_argument(M::SpecialEuclidean{TypeParameter{Tuple{2}}}, p, X)
@doc raw"""
jacobian_exp_inv_argument(
M::SpecialEuclidean{TypeParameter{Tuple{3}}},
p,
X,
)
Compute Jacobian matrix of the invariant exponential map on [`SpecialEuclidean`](@ref)`(3)`.
The formula reads
````math
\begin{pmatrix}
R & Q \\
0_{3×3} & R
\end{pmatrix},
````
where ``R`` is the Jacobian of exponential map on [`Rotations`](@ref)`(3)` with respect to
the argument, and ``Q`` is
````math
\frac{1}{2} T - \frac{θ - \sin(θ)}{θ^3} (X_r T + T X_r + X_r T X_r) + \frac{1 - \frac{θ^2}{2} - \cos(θ)}{θ^4} (X_r^2 T + T X_r^2 - 3 X_r T X_r) + \frac{1}{2}\left(\frac{1 - \frac{θ^2}{2} - \cos(θ)}{θ^4} - 3 \frac{θ - \sin(θ) - \frac{θ^3}{6}}{θ^5}\right) (X_r T X_r^2 + X_r^2 T X_r)
````
where ``X_r`` is the rotation part of ``X`` and ``T`` is
````math
\frac{1}{\sqrt{2}}\begin{pmatrix}
0 & -t_3 & t_2 \\
t_3 & 0 & -t_1 \\
-t_2 & t_1 & 0
\end{pmatrix},
````
where ``[t_1, t_2, t_3]`` is the translation part of `X`.
It is adapted from [BarfootFurgale:2014](@cite), Eq. (102), to `Manifolds.jl` conventions.
"""
jacobian_exp_inv_argument(M::SpecialEuclidean{TypeParameter{Tuple{3}}}, p, X)
function jacobian_exp_inv_argument(M::SpecialEuclidean, p, X)
J = allocate_jacobian(M, M, jacobian_exp_inv_argument, p)
return jacobian_exp_inv_argument!(M, J, p, X)
end
function jacobian_exp_inv_argument!(
M::SpecialEuclidean{TypeParameter{Tuple{2}}},
J::AbstractMatrix,
p,
X,
)
θ = norm(X.x[2]) / sqrt(2)
t1, t2 = X.x[1]
copyto!(J, I)
if θ ≈ 0
J[1, 3] = -t2 / (sqrt(2) * 2)
J[2, 3] = t1 / (sqrt(2) * 2)

Check warning on line 620 in src/groups/special_euclidean.jl

View check run for this annotation

Codecov / codecov/patch

src/groups/special_euclidean.jl#L619-L620 

Added lines #L619 - L620 were not covered by tests
else
J[1, 1] = J[2, 2] = sin(θ) / θ
J[1, 2] = (cos(θ) - 1) / θ
J[2, 1] = -J[1, 2]
J[1, 3] = (t1 * (sin(θ) - θ) + t2 * (cos(θ) - 1)) / (sqrt(2) * θ^2)
J[2, 3] = (t2 * (sin(θ) - θ) + t1 * (1 - cos(θ))) / (sqrt(2) * θ^2)
end
return J
end

function jacobian_exp_inv_argument!(
M::SpecialEuclidean{TypeParameter{Tuple{3}}},
J::AbstractMatrix,
p,
X,
)
θ = norm(X.x[2]) / sqrt(2)
t1, t2, t3 = X.x[1]
Xr = X.x[2]
copyto!(J, I)
if θ ≈ 0
J[1, 5] = t3 / (sqrt(2) * 2)
J[1, 6] = -t2 / (sqrt(2) * 2)
J[2, 6] = t1 / (sqrt(2) * 2)
J[2, 4] = -t3 / (sqrt(2) * 2)
J[3, 4] = t2 / (sqrt(2) * 2)
J[3, 5] = -t1 / (sqrt(2) * 2)

Check warning on line 647 in src/groups/special_euclidean.jl

View check run for this annotation

Codecov / codecov/patch

src/groups/special_euclidean.jl#L642-L647 

Added lines #L642 - L647 were not covered by tests
else
a = (cos(θ) - 1) / θ^2
b = (θ - sin(θ)) / θ^3
# top left block
view(J, SOneTo(3), SOneTo(3)) .+= a .* Xr .+ b .* (Xr^2)
# bottom right block
view(J, 4:6, 4:6) .= view(J, SOneTo(3), SOneTo(3))
# top right block
Xr = -Xr
tx = @SMatrix [
0 -t3/sqrt(2) t2/sqrt(2)
t3/sqrt(2) 0 -t1/sqrt(2)
-t2/sqrt(2) t1/sqrt(2) 0
]
J[1:3, 4:6] .= -tx ./ 2
J[1:3, 4:6] .-= (θ - sin(θ)) / (θ^3) * (Xr * tx + tx * Xr + Xr * tx * Xr)
J[1:3, 4:6] .+=
((1 - θ^2 / 2 - cos(θ)) / θ^4) * (Xr^2 * tx + tx * Xr^2 - 3 * Xr * tx * Xr)
J[1:3, 4:6] .+=
0.5 *
((1 - (θ^2) / 2 - cos(θ)) / (θ^4) - 3 * (θ - sin(θ) - (θ^3) / 6) / (θ^5)) *
(Xr * tx * Xr^2 + Xr^2 * tx * Xr)
end
return J
end

@doc raw"""
log_lie(G::SpecialEuclidean, p)
Expand Down
42 changes: 38 additions & 4 deletions test/groups/special_euclidean.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -502,13 +502,28 @@ using Manifolds:
-0.834713915470173 -0.5506838288169109
],
)
X = ArrayPartition(
[-0.12879180916758373, 1.0474807811628344],
[
0.0 1.4618350647546596
-1.4618350647546596 0.0
],
)

Jref = [
Jref_adj = [
-0.5506838288169109 0.834713915470173 0.667311539308751
-0.834713915470173 -0.5506838288169109 0.4387723006103765
0.0 0.0 1.0
]
@test adjoint_matrix(M, p) ≈ Jref
@test adjoint_matrix(M, p) ≈ Jref_adj

Jref_exp_inv_arg = [
0.680014877576939 -0.6096817894295923 -0.2889783387063578
0.6096817894295923 0.680014877576939 -0.20011168505842836
0.0 0.0 1.0
]

@test Manifolds.jacobian_exp_inv_argument(M, p, X) ≈ Jref_exp_inv_arg

M = SpecialEuclidean(3)
p = ArrayPartition(
Expand All @@ -519,8 +534,16 @@ using Manifolds:
-0.24417860264358274 0.7686182534099043 0.5912721797413909
],
)
X = ArrayPartition(
[-1.2718227195512866, 0.3557308974320734, 0.5635823415430814],
[
0.0 0.6404338627384397 -0.3396314473021008
-0.6404338627384397 0.0 0.014392664447157878
0.3396314473021008 -0.014392664447157878 0.0
],
)

Jref = [
Jref_adj = [
0.590536813431926 0.6014916127888292 -0.538027984148 0.5383275472420492 -0.3669179673652647 0.18066746943124343
-0.7691833864513029 0.21779306754302752 -0.6007687556269085 0.375220504480154 0.3013219176415302 -0.37117035706729606
-0.24417860264358274 0.7686182534099043 0.5912721797413909 0.11994849553498667 0.20175458298403887 -0.21273349816106235
Expand All @@ -529,6 +552,17 @@ using Manifolds:
0.0 0.0 0.0 -0.24417860264358288 0.7686182534099045 0.5912721797413911
]

@test adjoint_matrix(M, p) ≈ Jref
@test adjoint_matrix(M, p) ≈ Jref_adj

Jref_exp_inv_arg = [
0.9146894208814211 -0.3056384215994201 0.1640017371487483 0.10780385550476036 0.2228162258032636 -0.01878311460434911
0.3072255245020723 0.9333816528994998 0.02842431173301474 -0.12477070683106668 0.07647695678244679 -0.47764535923870577
-0.16100897999806887 0.04219738908136589 0.9812405108427417 0.2040191617706787 0.383713624853204 0.02291967163749559
0.0 0.0 0.0 0.9146894208814211 -0.3056384215994201 0.1640017371487483
0.0 0.0 0.0 0.3072255245020723 0.9333816528994998 0.02842431173301474
0.0 0.0 0.0 -0.16100897999806887 0.04219738908136589 0.9812405108427417
]

@test Manifolds.jacobian_exp_inv_argument(M, p, X) ≈ Jref_exp_inv_arg
end
end

0 comments on commit adaae1b

Please sign in to comment.