diff --git a/NEWS.md b/NEWS.md index d5e53e5ae..4517fcd05 100644 --- a/NEWS.md +++ b/NEWS.md @@ -5,6 +5,12 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). +## [0.10.7] – 2024-11-16 + +### Added + +* `adjoint_matrix` for Lie groups, with optimized implementations for SO(2), SO(3), SE(2) and SE(3). + ## [0.10.6] – 2024-11-06 ### Added diff --git a/Project.toml b/Project.toml index 42c6a802a..033aa9bfa 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "Manifolds" uuid = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" authors = ["Seth Axen ", "Mateusz Baran ", "Ronny Bergmann ", "Antoine Levitt "] -version = "0.10.6" +version = "0.10.7" [deps] Einsum = "b7d42ee7-0b51-5a75-98ca-779d3107e4c0" @@ -54,7 +54,7 @@ Graphs = "1.4" HybridArrays = "0.4" Kronecker = "0.4, 0.5" LinearAlgebra = "1.6" -ManifoldDiff = "0.3.7" +ManifoldDiff = "0.3.13" ManifoldsBase = "0.15.18" Markdown = "1.6" MatrixEquations = "2.2" diff --git a/docs/src/misc/notation.md b/docs/src/misc/notation.md index 483a4e738..58852aea2 100644 --- a/docs/src/misc/notation.md +++ b/docs/src/misc/notation.md @@ -13,7 +13,7 @@ Within the documented functions, the utf8 symbols are used whenever possible, as | ``\operatorname{Ad}_p(X)`` | adjoint action of element ``p`` of a Lie group on the element ``X`` of the corresponding Lie algebra | | | | ``×`` | Cartesian product of two manifolds | | see [`ProductManifold`](@extref `ManifoldsBase.ProductManifold`) | | ``^{\wedge}`` | (n-ary) Cartesian power of a manifold | | see [`PowerManifold`](@extref `ManifoldsBase.PowerManifold`) | -| ``⋅^\mathrm{H}`` | conjugate/Hermitian transpose | | +| ``⋅^\mathrm{H}`` | conjugate/Hermitian transpose | | | | ``a`` | coordinates of a point in a chart | | see [`get_parameters`](@ref) | | ``\frac{\mathrm{D}}{\mathrm{d}t}`` | covariant derivative of a vector field ``X(t)`` | | | | ``T^*_p \mathcal M`` | the cotangent space at ``p`` | | | @@ -27,40 +27,78 @@ Within the documented functions, the utf8 symbols are used whenever possible, as | ``\gamma`` | a geodesic | ``\gamma_{p;q}``, ``\gamma_{p,X}`` | connecting two points ``p,q`` or starting in ``p`` with velocity ``X``. | | ``\operatorname{grad} f(p)`` | (Riemannian) gradient of function ``f \colon \mathcal{M} → ℝ`` at ``p \in \mathcal{M}`` | | | | ``\nabla f(p)`` | (Euclidean) gradient of function ``f \colon \mathcal{M} → ℝ`` at ``p \in \mathcal{M}`` but thought of as evaluated in the embedding | `G` | | -| ``\circ`` | a group operation | | -| ``⋅^\mathrm{H}`` | Hermitian or conjugate transposed for both complex or quaternion matrices| | +| ``\circ`` | a group operation | | | +| ``⋅^\mathrm{H}`` | Hermitian or conjugate transposed for both complex or quaternion matrices| | | | ``\operatorname{Hess} f(p)`` | (Riemannian) Hessian of function ``f \colon T_p\mathcal{M} → T_p\mathcal M`` (i.e. the 1-1-tensor form) at ``p \in \mathcal{M}`` | | | | ``\nabla^2 f(p)`` | (Euclidean) Hessian of function ``f`` in the embedding | `H` | | -| ``e`` | identity element of a group | | -| ``I_k`` | identity matrix of size ``k×k`` | | +| ``e`` | identity element of a group | | | +| ``I_k`` | identity matrix of size ``k×k`` | | | | ``k`` | indices | ``i,j`` | | -| ``\langle⋅,⋅\rangle`` | inner product (in ``T_p \mathcal M``) | ``\langle⋅,⋅\rangle_p, g_p(⋅,⋅)`` | -| ``\operatorname{retr}^{-1}_pq``| an inverse retraction | | -| ``\mathfrak g`` | a Lie algebra | | -| ``\mathcal{G}`` | a (Lie) group | | +| ``\langle⋅,⋅\rangle`` | inner product (in ``T_p \mathcal M``) | ``\langle⋅,⋅\rangle_p, g_p(⋅,⋅)`` | | +| ``\operatorname{retr}^{-1}_pq``| an inverse retraction | | | +| ``\mathfrak g`` | a Lie algebra | | | +| ``\mathcal{G}`` | a (Lie) group | | | | ``\log_p q`` | logarithmic map at ``p \in \mathcal M`` of a point ``q \in \mathcal M`` | ``\log_p(q)`` | | | ``\mathcal M`` | a manifold | ``\mathcal M_1, \mathcal M_2,\ldots,\mathcal N`` | | | ``N_p \mathcal M`` | the normal space of the tangent space ``T_p \mathcal M`` in some embedding ``\mathcal E`` that should be clear from context | | | | ``V`` | a normal vector from ``N_p \mathcal M`` | ``W`` | | -| ``\operatorname{Exp}`` | the matrix exponential | | -| ``\operatorname{Log}`` | the matrix logarithm | | -| ``\mathcal P_{q\gets p}X`` | parallel transport | | of the vector ``X`` from ``T_p\mathcal M`` to ``T_q\mathcal M`` -| ``\mathcal P_{p,Y}X`` | parallel transport in direction ``Y`` | | of the vector ``X`` from ``T_p\mathcal M`` to ``T_q\mathcal M``, ``q = \exp_pY`` -| ``\mathcal P_{t_1\gets t_0}^cX`` | parallel transport along the curve ``c``| ``\mathcal P^cX=\mathcal P_{1\gets 0}^cX`` | of the vector ``X`` from ``p=c(0)`` to ``c(1)`` +| ``\operatorname{Exp}`` | the matrix exponential | | | +| ``\operatorname{Log}`` | the matrix logarithm | | | +| ``\mathcal P_{q\gets p}X`` | parallel transport | | of the vector ``X`` from ``T_p\mathcal M`` to ``T_q\mathcal M`` | +| ``\mathcal P_{p,Y}X`` | parallel transport in direction ``Y`` | | of the vector ``X`` from ``T_p\mathcal M`` to ``T_q\mathcal M``, ``q = \exp_pY`` | +| ``\mathcal P_{t_1\gets t_0}^cX`` | parallel transport along the curve ``c``| ``\mathcal P^cX=\mathcal P_{1\gets 0}^cX`` | of the vector ``X`` from ``p=c(0)`` to ``c(1)`` | | ``p`` | a point on ``\mathcal M`` | ``p_1, p_2, \ldots,q`` | for 3 points one might use ``x,y,z`` | -| ``\operatorname{retr}_pX``| a retraction | | -| ``\kappa_p(X, Y)`` | sectional curvature | | +| ``\operatorname{retr}_pX``| a retraction | | | +| ``\kappa_p(X, Y)`` | sectional curvature | | | | ``ξ`` | a set of tangent vectors | ``\{X_1,\ldots,X_n\}`` | | | ``J_{2n} \in ℝ^{2n×2n}`` | the [`SymplecticElement`](@ref) | | | | ``T_p \mathcal M`` | the tangent space at ``p`` | | | | ``X`` | a tangent vector from ``T_p \mathcal M`` | ``X_1,X_2,\ldots,Y,Z`` | sometimes written with base point ``X_p`` | -| ``\operatorname{tr}`` | trace (of a matrix) | | -| ``⋅^\mathrm{T}`` | transposed | | -| ``e_i \in \mathbb R^n`` | the ``i``th unit vector | ``e_i^n`` | the space dimension (``n``) is omitted, when clear from context -| ``B`` | a vector bundle | | -| ``\mathcal T_{q\gets p}X`` | vector transport | | of the vector ``X`` from ``T_p\mathcal M`` to ``T_q\mathcal M`` +| ``\operatorname{tr}`` | trace (of a matrix) | | | +| ``⋅^\mathrm{T}`` | transposed | | | +| ``e_i \in \mathbb R^n`` | the ``i``th unit vector | ``e_i^n`` | the space dimension (``n``) is omitted, when clear from context | +| ``B`` | a vector bundle | | | +| ``\mathcal T_{q\gets p}X`` | vector transport | | of the vector ``X`` from ``T_p\mathcal M`` to ``T_q\mathcal M`` | | ``\mathcal T_{p,Y}X`` | vector transport in direction ``Y`` | | of the vector ``X`` from ``T_p\mathcal M`` to ``T_q\mathcal M``, where ``q`` is determined by ``Y``, for example using the exponential map or some retraction. | -| ``\operatorname{Vol}(\mathcal M)`` | volume of manifold ``\mathcal M`` | | -| ``\theta_p(X)`` | volume density for vector ``X`` tangent at point ``p`` | | +| ``\operatorname{Vol}(\mathcal M)`` | volume of manifold ``\mathcal M`` | | | +| ``\theta_p(X)`` | volume density for vector ``X`` tangent at point ``p`` | | | | ``\mathcal W`` | the Weingarten map ``\mathcal W: T_p\mathcal M × N_p\mathcal M → T_p\mathcal M`` | ``\mathcal W_p`` | the second notation to emphasize the dependency of the point ``p\in\mathcal M`` | -| ``0_k`` | the ``k×k`` zero matrix. | | +| ``0_k`` | the ``k×k`` zero matrix. | | | + +## Comparison with notation commonly used in robotics + +In robotics, a different notation is commonly used. +The table below shows a quick guide how to translate between them for people coming from robotics background. +We use [SolaDerayAtchuthan:2021](@cite) as the primary robotics source. + +| Robotics concept | Manifolds.jl notation | +|:--|:--------------- | +| ``p \circ q`` | `compose(G, p, q)` | +| ``p^{-1}``| `inv(G, p)` | +| ``\mathcal{E}`` | `Identity(G)` or `identity_element(G)` | +| group action ``p\cdot p_m`` | `apply(A, p, p_m)` | +| Lie group exponential ``\exp\colon \mathfrak{g} \to \mathcal{G}``, ``\exp(X)=p`` | `exp_lie(G, p)` | +| Lie group logarithm ``\log\colon \mathcal{G} \to \mathfrak{g}``, ``\log(p)=X`` | `log_lie(G, X)` | +| ``n``-D vector | `TranslationGroup(n)`; its action is `TranslationAction(Euclidean(n), TranslationGroup(n))` | +| circle ``S^1`` | `CircleGroup()`; its action is `ComplexPlanarRotation` | +| rotation ``\mathrm{SO}(n)`` | `SpecialOrthogonal(n)`; its action is `RotationAction(Euclidean(n), SpecialOrthogonal(n))` | +| rigid motion ``\mathrm{SE}(n)`` | `SpecialEuclidean(n)`; its action is `RotationTranslationAction(Euclidean(n), SpecialEuclidean(n))` | +| unit quaternions ``S^3`` | `UnitaryMatrices(1, H)`; note that 3-sphere and the group of rotations (with its bi-invariant metric) are homeomorphic but not isomorphic | +| size (as in Table I) | related to `representation_size(G)` | +| dim (as in Table I) | `manifold_dimension(G)` | +| Lie algebra element with coordinates ``\tau^{\wedge}`` | `hat(G, Identity(G), tau)` | +| coordinates of an element of Lie algebra ``X^{\vee}`` | `vee(G, Identity(G), X)` | +| capital exponential map ``\operatorname{Exp}`` | `exp_lie(G, hat(G, Identity(G), tau))` | +| capital logarithmic map ``\operatorname{Log}`` | `vee(G, Identity(G), log_lie(G, p))` | +| right-``\oplus``, ``p \oplus \tau`` | `compose(G, exp_lie(G, hat(G, Identity(G), tau)))` | +| right-``\ominus``, ``p \ominus q`` | `vee(G, Identity(G), log_lie(G, compose(G, inv(G, q), p)))`| +| left-``\oplus``, ``\tau \oplus p`` | `compose(G, exp_lie(G, hat(G, Identity(G), tau)), p)` | +| left-``\ominus``, ``p \ominus q`` | `vee(G, Identity(G), log_lie(G, compose(G, p, inv(G, q))))` | +| adjoint ``\mathrm{Ad}_{p}(\tau^{\wedge})`` | `adjoint_action(G, p, hat(G, Identity(G), tau))` | +| adjoint matrix ``\mathrm{Ad}_{p}`` | `adjoint_matrix(G, p)` | +| Jacobian of group inversion and composition | these can be easily constructed from the adjoint matrix | +| left and right Jacobians of a function | In JuliaManifolds there is always one preferred way to store tangent vectors specified by each manifold, and so we follow the standard mathematical convention of having one Jacobian which follows the selected tangent vector storage convention. See for example `jacobian_exp_argument`, `jacobian_exp_basepoint`, `jacobian_log_argument`, `jacobian_log_basepoint` from `ManifoldDiff.jl`. | +| left and right Jacobians (of a group) ``\mathbf{J}_l, \mathbf{J}_r`` | `jacobian_exp_argument` for exponential coordinates. For other coordinate systems no replacement is available yet. | +| Jacobians of group actions | not available yet | + +Be also careful that the meaning of ``\mathbf{x}`` is inconsistent in Table I from [SolaDerayAtchuthan:2021](@cite). It's a complex number for circle, quaternion for quaternion rotation and column vectors for other rows. diff --git a/docs/src/references.bib b/docs/src/references.bib index 9683061c5..d01b1c751 100644 --- a/docs/src/references.bib +++ b/docs/src/references.bib @@ -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}, diff --git a/src/Manifolds.jl b/src/Manifolds.jl index 2b36221c2..cf1aea2d0 100644 --- a/src/Manifolds.jl +++ b/src/Manifolds.jl @@ -182,6 +182,8 @@ import ManifoldDiff: diagonalizing_projectors, jacobi_field, jacobi_field!, + jacobian_exp_argument, + jacobian_exp_argument!, riemannian_gradient, riemannian_gradient!, riemannian_Hessian, @@ -326,6 +328,7 @@ using ManifoldsBase: ziptuples using ManifoldDiff: ManifoldDiff using ManifoldDiff: + allocate_jacobian, default_differential_backend, _derivative, _derivative!, @@ -1029,6 +1032,8 @@ export adjoint_action, adjoint_apply_diff_group!, adjoint_inv_diff, adjoint_inv_diff!, + adjoint_matrix, + adjoint_matrix!, affine_matrix, apply, apply!, diff --git a/src/groups/group.jl b/src/groups/group.jl index 5ec6888e9..26faddc67 100644 --- a/src/groups/group.jl +++ b/src/groups/group.jl @@ -538,6 +538,33 @@ end @trait_function adjoint_inv_diff!(G::AbstractDecoratorManifold, Y, p, X) +""" + adjoint_matrix(G::AbstractManifold, p, B::AbstractBasis=DefaultOrthonormalBasis()) + +Compute the adjoint matrix related to conjugation of vectors by element `p` of Lie group `G` +for basis `B`. It is the matrix ``A`` such that for each element `X` of the Lie algebra +with coefficients ``c`` in basis `B`, ``Ac`` is the vector of coefficients of `X` conjugated +by `p` in basis `B`. +""" +function adjoint_matrix(G::AbstractManifold, p, B::AbstractBasis=DefaultOrthonormalBasis()) + J = allocate_jacobian(G, G, adjoint_matrix, p) + return adjoint_matrix!(G, J, p, B) +end + +function adjoint_matrix!( + G::AbstractManifold, + J, + p, + B::AbstractBasis=DefaultOrthonormalBasis(), +) + Bb = get_basis(G, p, B) + Vs = get_vectors(G, p, Bb) + for i in eachindex(Vs) + get_coordinates!(G, view(J, :, i), p, adjoint_action(G, p, Vs[i]), B) + end + return J +end + function ManifoldDiff.differential_exp_argument_lie_approx!( M::AbstractManifold, Z, diff --git a/src/groups/special_euclidean.jl b/src/groups/special_euclidean.jl index d84559948..fe1484cd3 100644 --- a/src/groups/special_euclidean.jl +++ b/src/groups/special_euclidean.jl @@ -190,6 +190,61 @@ function adjoint_action( return TFVector([cross(t, Rω) + R * r; Rω], fX.basis) end +@doc raw""" + adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{2}}}, p) + +Compute the adjoint matrix for the group [`SpecialEuclidean`](@ref)`(2)` at point `p` +in default coordinates. The formula follows Section 10.6.2 in [Chirikjian:2012](@cite) +but with additional scaling by ``\sqrt{2}`` due to a different choice of inner product. +The formula reads +````math +\begin{pmatrix} +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) + return @SMatrix [ + R[1, 1] R[1, 2] t[2]/sqrt(2) + R[2, 1] R[2, 2] -t[1]/sqrt(2) + 0 0 1 + ] +end +@doc raw""" + adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{3}}}, p) + +Compute the adjoint matrix for the group [`SpecialEuclidean`](@ref)`(3)` at point `p` +in default coordinates. The formula follows Section 10.6.9 in [Chirikjian:2012](@cite) with +changes due to different conventions. The formula reads +````math +\begin{pmatrix} +R & UR/\sqrt{2} \\ +0_{3×3} & R +\end{pmatrix}. +```` +where ``R`` is the rotation matrix of `p` and ``U`` is the matrix +````math +\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 vector of `p`. +""" +function adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{3}}}, p) + t, R = submanifold_components(p) + Z = @SMatrix zeros(3, 3) + c = sqrt(2) \ @SMatrix [0 -t[3] t[2]; t[3] 0 -t[1]; -t[2] t[1] 0] + U = c * R + return vcat(hcat(R, U), hcat(Z, R)) +end + @doc raw""" affine_matrix(G::SpecialEuclidean, p) -> AbstractMatrix @@ -494,6 +549,133 @@ 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{1}{θ}\sin(θ) & \frac{1}{θ} (1-\cos(θ)) & \frac{1}{\sqrt{2} θ^2}(t_1(\sin(θ) - θ) + t_2(\cos(θ) - 1)) \\ +\frac{1}{θ}(-1+\cos(θ)) & \frac{1}{θ}\sin(θ) & \frac{1}{\sqrt{2} θ^2}(t_2(\sin(θ) - θ) + t_1(-\cos(θ) + 1)) \\ +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 +\begin{align*} +Q = &\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) +\end{align*} +```` +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) + 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) + 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) diff --git a/src/groups/special_orthogonal.jl b/src/groups/special_orthogonal.jl index e5d30cae1..b93eb1998 100644 --- a/src/groups/special_orthogonal.jl +++ b/src/groups/special_orthogonal.jl @@ -1,17 +1,35 @@ @doc raw""" - SpecialOrthogonal{n} = GeneralUnitaryMultiplicationGroup{n,ℝ,DeterminantOneMatrices} + SpecialOrthogonal{T} = GeneralUnitaryMultiplicationGroup{T,ℝ,DeterminantOneMatrices} Special orthogonal group ``\mathrm{SO}(n)`` represented by rotation matrices, see [`Rotations`](@ref). # Constructor SpecialOrthogonal(n) """ -const SpecialOrthogonal{n} = GeneralUnitaryMultiplicationGroup{n,ℝ,DeterminantOneMatrices} +const SpecialOrthogonal{T} = GeneralUnitaryMultiplicationGroup{T,ℝ,DeterminantOneMatrices} function SpecialOrthogonal(n; parameter::Symbol=:type) return GeneralUnitaryMultiplicationGroup(Rotations(n; parameter=parameter)) end +""" + adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{2}}}, p) + +Compte the adjoint matrix for [`SpecialOrthogonal`](@ref)`(2)` at point `p`, which is equal +to `1`. See [SolaDerayAtchuthan:2021](@cite), Appendix A. +""" +adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{2}}}, p) = @SMatrix [1] +""" + adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{3}}}, p) + +Compte the adjoint matrix for [`SpecialOrthogonal`](@ref)`(3)` at point `p`, which is equal +to `p`. See [Chirikjian:2012](@cite), Section 10.6.6. +""" +adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{3}}}, p) = p + +adjoint_matrix!(::SpecialOrthogonal{TypeParameter{Tuple{2}}}, J, p) = fill!(J, 1) +adjoint_matrix!(::SpecialOrthogonal{TypeParameter{Tuple{3}}}, J, p) = copyto!(J, p) + Base.inv(::SpecialOrthogonal, p) = transpose(p) Base.inv(::SpecialOrthogonal, e::Identity{MultiplicationOperation}) = e diff --git a/src/manifolds/Rotations.jl b/src/manifolds/Rotations.jl index 204a23faf..c20751478 100644 --- a/src/manifolds/Rotations.jl +++ b/src/manifolds/Rotations.jl @@ -205,6 +205,57 @@ function inverse_retract_qr!(M::Rotations, X, p, q) return project!(SkewSymmetricMatrices(n), X, p, X) end +@doc raw""" + jacobian_exp_argument(M::Rotations{TypeParameter{Tuple{2}}}, p, X) + +Compute Jacobian of the exponential map with respect to the argument `X` in orthonormal +coordinates on the [`Rotations`](@ref)`(2)` manifold. It is equal to matrix ``[1]``, see +[SolaDerayAtchuthan:2021](@cite), Appendix A. +""" +function jacobian_exp_argument(::Rotations{TypeParameter{Tuple{2}}}, p, X) + return @SMatrix ones(eltype(X), 1, 1) +end +@doc raw""" + jacobian_exp_argument(M::Rotations{TypeParameter{Tuple{3}}}, p, X) + +Compute Jacobian of the exponential map with respect to the argument `X` in orthonormal +coordinates on the [`Rotations`](@ref)`(3)` manifold. The formula reads +````math +𝕀 + \frac{\cos(θ) - 1}{θ^2} X + \frac{θ - \sin(θ)}{θ^3} X^2, +```` +where ``θ`` is the norm of `X`. +It is adapted from [Chirikjian:2012](@cite), Eq. (10.86), to `Manifolds.jl` conventions. +""" +function jacobian_exp_argument(M::Rotations{TypeParameter{Tuple{3}}}, p, X) + J = allocate_jacobian(M, M, jacobian_exp_argument, p) + return jacobian_exp_argument!(M, J, p, X) +end + +function jacobian_exp_argument!( + ::Rotations{TypeParameter{Tuple{2}}}, + J::AbstractMatrix, + p, + X, +) + J .= 1 + return J +end +function jacobian_exp_argument!( + M::Rotations{TypeParameter{Tuple{3}}}, + J::AbstractMatrix, + p, + X, +) + θ = norm(M, p, X) / sqrt(2) + copyto!(J, I) + if θ ≉ 0 + a = (cos(θ) - 1) / θ^2 + b = (θ - sin(θ)) / θ^3 + J .+= a .* X .+ b .* (X^2) + end + return J +end + @doc raw""" normal_rotation_distribution(M::Rotations, p, σ::Real) diff --git a/test/groups/groups_general.jl b/test/groups/groups_general.jl index a48f8fc23..40c34f811 100644 --- a/test/groups/groups_general.jl +++ b/test/groups/groups_general.jl @@ -322,6 +322,17 @@ using Manifolds: end end end + + @testset "Jacobians" begin + M = SpecialOrthogonal(3) + p = [ + -0.333167290022488 -0.7611396995437196 -0.5564763378954822 + 0.8255218425902797 0.049666662385985494 -0.5621804959741897 + 0.4555362161951786 -0.646683524164589 0.6117901399243428 + ] + # testing the fallback definition just in case + @test invoke(adjoint_matrix, Tuple{AbstractManifold,Any}, M, p) ≈ p + end end struct NotImplementedAction <: AbstractGroupAction{LeftAction} end diff --git a/test/groups/special_euclidean.jl b/test/groups/special_euclidean.jl index d063fdc98..a910655c8 100644 --- a/test/groups/special_euclidean.jl +++ b/test/groups/special_euclidean.jl @@ -492,4 +492,107 @@ using Manifolds: end end end + + @testset "Jacobians" begin + M = SpecialEuclidean(2) + p = ArrayPartition( + [-0.6205177383168391, 0.9437210292185024], + [ + -0.5506838288169109 0.834713915470173 + -0.834713915470173 -0.5506838288169109 + ], + ) + X = ArrayPartition( + [-0.12879180916758373, 1.0474807811628344], + [ + 0.0 1.4618350647546596 + -1.4618350647546596 0.0 + ], + ) + X2 = ArrayPartition( + [-0.12879180916758373, 1.0474807811628344], + [ + 0.0 0.0 + 0.0 0.0 + ], + ) + + 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_adj + + Jref_exp_inv_arg = [ + 0.680014877576939 -0.6096817894295923 -0.2889783387063578 + 0.6096817894295923 0.680014877576939 -0.20011168505842836 + 0.0 0.0 1.0 + ] + Jref_exp_inv_arg2 = [ + 1.0 0.0 -0.3703403817614111 + 0.0 1.0 -0.045534780811841105 + 0.0 0.0 1.0 + ] + + @test Manifolds.jacobian_exp_inv_argument(M, p, X) ≈ Jref_exp_inv_arg + @test Manifolds.jacobian_exp_inv_argument(M, p, X2) ≈ Jref_exp_inv_arg2 + + M = SpecialEuclidean(3) + p = ArrayPartition( + [0.1124045202347309, -0.4336604812325255, 0.8520978475672548], + [ + 0.590536813431926 0.6014916127888292 -0.538027984148 + -0.7691833864513029 0.21779306754302752 -0.6007687556269085 + -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 + ], + ) + X2 = ArrayPartition( + [-1.2718227195512866, 0.3557308974320734, 0.5635823415430814], + [ + 0.0 0.0 0.0 + 0.0 0.0 0.0 + 0.0 0.0 0.0 + ], + ) + + 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 + 0.0 0.0 0.0 0.5905368134319258 0.6014916127888291 -0.5380279841480001 + 0.0 0.0 0.0 -0.7691833864513032 0.21779306754302766 -0.6007687556269088 + 0.0 0.0 0.0 -0.24417860264358288 0.7686182534099045 0.5912721797413911 + ] + + @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 + ] + Jref_exp_inv_arg2 = [ + 1.0 0.0 0.0 0.0 0.19925644773105286 -0.12576986492589765 + 0.0 1.0 0.0 -0.19925644773105286 0.0 -0.4496572347309157 + 0.0 0.0 1.0 0.12576986492589765 0.4496572347309157 0.0 + 0.0 0.0 0.0 1.0 0.0 0.0 + 0.0 0.0 0.0 0.0 1.0 0.0 + 0.0 0.0 0.0 0.0 0.0 1.0 + ] + + @test Manifolds.jacobian_exp_inv_argument(M, p, X) ≈ Jref_exp_inv_arg + @test Manifolds.jacobian_exp_inv_argument(M, p, X2) ≈ Jref_exp_inv_arg2 + end end diff --git a/test/groups/special_orthogonal.jl b/test/groups/special_orthogonal.jl index 32753dd0b..4d1d9239a 100644 --- a/test/groups/special_orthogonal.jl +++ b/test/groups/special_orthogonal.jl @@ -224,4 +224,27 @@ using Manifolds: LeftForwardAction, RightBackwardAction atol=1e-12, ) end + + @testset "Jacobians" begin + M = SpecialOrthogonal(2) + p = [ + -0.7689185267244146 -0.6393467754356441 + 0.6393467754356439 -0.7689185267244146 + ] + @test adjoint_matrix(M, p) == @SMatrix [1] + J = @MMatrix [0.0] + adjoint_matrix!(M, J, p) + @test J == @SMatrix [1] + + M = SpecialOrthogonal(3) + p = [ + -0.333167290022488 -0.7611396995437196 -0.5564763378954822 + 0.8255218425902797 0.049666662385985494 -0.5621804959741897 + 0.4555362161951786 -0.646683524164589 0.6117901399243428 + ] + @test adjoint_matrix(M, p) == p + J = similar(p) + adjoint_matrix!(M, J, p) + @test J == p + end end diff --git a/test/manifolds/rotations.jl b/test/manifolds/rotations.jl index a19ac5e95..55ed383a2 100644 --- a/test/manifolds/rotations.jl +++ b/test/manifolds/rotations.jl @@ -326,4 +326,40 @@ include("../header.jl") -0.3903114578816011 0.4913864108919558 0.0 ] end + @testset "Jacobians" begin + M = Rotations(2) + p = [ + 0.38024046142595025 0.9248876642568981 + -0.9248876642568981 0.38024046142595014 + ] + X = [ + 0.0 0.40294834454872025 + -0.40294834454872025 0.0 + ] + @test ManifoldDiff.jacobian_exp_argument(M, p, X) == @SMatrix [1] + J = fill(0.0, 1, 1) + ManifoldDiff.jacobian_exp_argument!(M, J, p, X) + @test J == @SMatrix [1] + + M = Rotations(3) + p = [ + 0.8795914880107569 -0.39921238866388364 -0.2587436626398777 + 0.4643792859455722 0.6024065774550045 0.6491981163124461 + -0.10329904648012433 -0.6911843344406933 0.7152576618394751 + ] + X = [ + 0.0 -0.5656000980668913 0.7650118907017176 + 0.5656000980668913 0.0 0.9710556005789448 + -0.7650118907017176 -0.9710556005789448 0.0 + ] + Jref = [ + 0.8624842949736633 0.12898141742603422 -0.41055104894342076 + -0.3547043184601566 0.8081393303537744 -0.3494729262397322 + 0.24366619850830287 0.4809472670341492 0.7678272125532423 + ] + @test ManifoldDiff.jacobian_exp_argument(M, p, X) ≈ Jref + J = fill(0.0, 3, 3) + ManifoldDiff.jacobian_exp_argument!(M, J, p, X) + @test J ≈ Jref + end end