jacobian_exp_argument for rotations

docs/src/misc/notation.md

@@ -98,7 +98,7 @@ We use [SolaDerayAtchuthan:2021](@cite) as the primary robotics source.
 | 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`` | not available yet, though in some cases `jacobian_exp_argument` might be used |
+| 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.

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!,

src/groups/group.jl

@@ -547,7 +547,7 @@ with coefficients ``c`` in basis `B`, ``Ac`` is the vector of coefficients of `X
 by `p` in basis `B`.
 """
 function adjoint_matrix(G::AbstractManifold, p, B::AbstractBasis=DefaultOrthonormalBasis())
-    J = ManifoldDiff.allocate_jacobian(G, G, adjoint_matrix, p)
+    J = allocate_jacobian(G, G, adjoint_matrix, p)
     return adjoint_matrix!(G, J, p, B)
 end
 

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 ManifoldDiff.jacobian_exp_argument!(
+    ::Rotations{TypeParameter{Tuple{2}}},
+    J::AbstractMatrix,
+    p,
+    X,
+)
+    J .= 1
+    return J
+end
+function ManifoldDiff.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)
 

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